ObjectRecognitionWrapper.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    MemoryX::Helpers
* @author     Alexey Kozlov ( kozlov at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "ObjectRecognitionWrapper.h"
 
// MemoryX
#include <MemoryX/core/entity/Entity.h>
 
// ArmarXCore
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
 
namespace memoryx::EntityWrappers
{
    // ObjectRecognitionWrapper attributes
    const std::string ATTR_RECOGNITION_METHOD = "recognitionMethod";
    const std::string ATTR_MOTION_MODEL = "defaultMotionModel";
    const std::string ATTR_RECOGNITION_QUERIES = "recognitionQueries";
 
    // TexturedRecognitionWrapper attributes
    const std::string ATTR_FEATURE_FILE = "featureFile";
 
    // SegmentableRecognitionWrapper attributes
    const std::string ATTR_OBJECT_COLOR = "objectColor";
    const std::string ATTR_SEGMENTABLE_DATA = "segmentableObjectData";
 
    // ArMarkerWrapper attributes
    const std::string ATTR_AR_MARKER_ID = "arMarkerID";
    const std::string ATTR_AR_MARKER_SIZE = "arMarkerSize";
    const std::string ATTR_AR_MARKER_TRANSLATION = "arMarkerTranslation";
    const std::string ATTR_AR_MARKER_ROTATION = "arMarkerRotation";
 
    // PointCloudLocalizerWrapper attributes
    const std::string POINTCLOUD_FILE = "pointcloudFile";
    const std::string POINTCLOUD_EXP_MATCH_DIST = "expectedMatchDistance";
    const std::string POINTCLOUD_MISMATCH_DIST = "mismatchDistance";
 
 
    std::string ObjectRecognitionWrapper::getRecognitionMethod() const
    {
        if (getEntity()-> hasAttribute(ATTR_RECOGNITION_METHOD))
        {
            return EntityPtr::dynamicCast(getEntity())->getAttributeValue(ATTR_RECOGNITION_METHOD)->getString();
        }
 
        return "";
    }
 
    std::string ObjectRecognitionWrapper::getDefaultMotionModel() const
    {
        if (getEntity()-> hasAttribute(ATTR_MOTION_MODEL))
        {
            return EntityPtr::dynamicCast(getEntity())->getAttributeValue(ATTR_MOTION_MODEL)->getString();
        }
 
        return "";
    }
 
    void ObjectRecognitionWrapper::setRecognitionMethod(const std::string& recognitionMethod)
    {
        if (recognitionMethod != "")
        {
            EntityAttributeBasePtr recognitionMethodAttr = new EntityAttribute(ATTR_RECOGNITION_METHOD);
            recognitionMethodAttr->setValue(new armarx::Variant(recognitionMethod));
            getEntity()-> putAttribute(recognitionMethodAttr);
        }
    }
 
    void ObjectRecognitionWrapper::setDefaultMotionModel(const std::string& defaultMotionModel)
    {
        if (defaultMotionModel != "")
        {
            EntityAttributeBasePtr motionModelAttr = new EntityAttribute(ATTR_MOTION_MODEL);
            motionModelAttr->setValue(new armarx::Variant(defaultMotionModel));
            getEntity()-> putAttribute(motionModelAttr);
        }
    }
 
    void ObjectRecognitionWrapper::addLocalizationQuery(const LocalizationQueryPtr& query)
    {
        if (getEntity()-> hasAttribute(ATTR_RECOGNITION_QUERIES))
        {
            getEntity()-> getAttribute(ATTR_RECOGNITION_QUERIES)->addValue(new armarx::Variant(query));
        }
        else
        {
            EntityAttributeBasePtr recognitionQueryAttr = new EntityAttribute(ATTR_RECOGNITION_QUERIES);
            recognitionQueryAttr->addValue(new armarx::Variant(query));
            getEntity()-> putAttribute(recognitionQueryAttr);
        }
    }
 
    LocalizationQueryList ObjectRecognitionWrapper::getLocalizationQueries()
    {
        LocalizationQueryList resultList;
 
        if (!getEntity()-> hasAttribute(ATTR_RECOGNITION_QUERIES))
        {
            return resultList;
        }
 
        int numberElements = getEntity()-> getAttribute(ATTR_RECOGNITION_QUERIES)->size();
 
        for (int e = 0 ; e < numberElements ; e++)
        {
            armarx::VariantPtr var = armarx::VariantPtr::dynamicCast(getEntity()-> getAttribute(ATTR_RECOGNITION_QUERIES)->getValueAt(e));
            resultList.push_back(var->getClass<LocalizationQuery>());
        }
 
        return resultList;
    }
 
    void ObjectRecognitionWrapper::removeLocalizationQuery(const std::string& queryName)
    {
        if (!getEntity()-> hasAttribute(ATTR_RECOGNITION_QUERIES))
        {
            return;
        }
 
        int numberElements = getEntity()-> getAttribute(ATTR_RECOGNITION_QUERIES)->size();
 
        for (int e = 0 ; e < numberElements ; e++)
        {
            armarx::VariantPtr var = armarx::VariantPtr::dynamicCast(getEntity()-> getAttribute(ATTR_RECOGNITION_QUERIES)->getValueAt(e));
            std::string currentQueryName = var->getClass<LocalizationQuery>()->queryName;
 
            if (currentQueryName == queryName)
            {
                getEntity()-> getAttribute(ATTR_RECOGNITION_QUERIES)->removeValueAt(e);
                e--;
                numberElements--;
            }
        }
    }
 
    void ObjectRecognitionWrapper::updateLocalizationQuery(const std::string& queryName, const LocalizationQueryPtr& query)
    {
        if (!getEntity()-> hasAttribute(ATTR_RECOGNITION_QUERIES))
        {
            ARMARX_ERROR_S << "Entity " << getEntity()->getName() << " does not have Attribute " << ATTR_RECOGNITION_QUERIES;
            return;
        }
 
        int numberElements = getEntity()-> getAttribute(ATTR_RECOGNITION_QUERIES)->size();
        EntityAttributePtr updatedQueries = new EntityAttribute(ATTR_RECOGNITION_QUERIES);
 
        for (int e = 0 ; e < numberElements ; e++)
        {
            armarx::VariantPtr var = armarx::VariantPtr::dynamicCast(getEntity()-> getAttribute(ATTR_RECOGNITION_QUERIES)->getValueAt(e));
            LocalizationQueryPtr currentQuery = var->getClass<LocalizationQuery>();
 
            armarx::VariantPtr newVar = new armarx::Variant();
            newVar->setType(armarx::VariantType::LocalizationQuery);
 
            if (currentQuery->queryName != queryName)   // copy
            {
                newVar->setClass(LocalizationQueryPtr::dynamicCast(currentQuery->ice_clone()));
            }
            else                                        // insert new query
            {
                newVar->setClass(LocalizationQueryPtr::dynamicCast(query->ice_clone()));
            }
 
            updatedQueries->addValue(newVar);
        }
 
        getEntity()-> removeAttribute(ATTR_RECOGNITION_QUERIES);
        getEntity()-> putAttribute(updatedQueries);
    }
 
 
    Ice::ObjectPtr ObjectRecognitionWrapper::ice_clone() const
    {
        return new ObjectRecognitionWrapper(*this);
    }
 
 
    TexturedRecognitionWrapper::TexturedRecognitionWrapper(const GridFileManagerPtr& gfm):
        AbstractFileEntityWrapper(gfm)
    {
    }
 
    std::string TexturedRecognitionWrapper::getFeatureFile() const
    {
        if (getEntity()-> hasAttribute(ATTR_FEATURE_FILE))
        {
            const std::string featureFileName = cacheAttributeFile(ATTR_FEATURE_FILE, true);
            return featureFileName;
        }
 
        return "";
    }
 
    void TexturedRecognitionWrapper::setFeatureFile(const std::string& featureFileName, const std::string& filesDBName)
    {
        if (featureFileName != "")
        {
            EntityAttributeBasePtr featureFileAttr = new EntityAttribute(ATTR_FEATURE_FILE);
            fileManager->storeFileToAttr(filesDBName, featureFileName, featureFileAttr);
            cleanUpAttributeFiles(getEntity()-> getAttribute(ATTR_FEATURE_FILE), featureFileAttr);
            getEntity()-> putAttribute(featureFileAttr);
        }
    }
 
    Ice::ObjectPtr TexturedRecognitionWrapper::ice_clone() const
    {
        return new TexturedRecognitionWrapper(*this);
    }
 
 
 
    SegmentableRecognitionWrapper::SegmentableRecognitionWrapper(const GridFileManagerPtr& gfm):
        AbstractFileEntityWrapper(gfm)
    {
    }
 
    std::string SegmentableRecognitionWrapper::getDataFiles() const
    {
        std::vector<std::string> fileNames;
 
        if (getEntity()-> hasAttribute(ATTR_SEGMENTABLE_DATA))
        {
            fileManager->ensureFilesInCache(getEntity()-> getAttribute(ATTR_SEGMENTABLE_DATA), fileNames, true);
        }
 
        if (fileNames.size() == 0)
        {
            return "";
        }
 
        // retrieve path
        std::filesystem::path path;
        path = fileNames.at(0);
 
        return path.parent_path().c_str();
    }
 
    void SegmentableRecognitionWrapper::setDataFiles(const std::string& dataPath, const std::string& filesDBName)
    {
        if (dataPath != "")
        {
            EntityAttributeBasePtr featureFileAttr = new EntityAttribute(ATTR_SEGMENTABLE_DATA);
            //        if(getEntity()–>getAttribute(ATTR_SEGMENTABLE_DATA))
            //            fileManager->removeAttrFiles(getEntity()–>getAttribute(ATTR_SEGMENTABLE_DATA));
            ARMARX_VERBOSE_S << "STORING datapath " << dataPath;
            fileManager->storeDirectoryToAttr(filesDBName, dataPath, featureFileAttr);
            cleanUpAttributeFiles(getEntity()-> getAttribute(ATTR_SEGMENTABLE_DATA), featureFileAttr);
 
            getEntity()-> putAttribute(featureFileAttr);
        }
    }
 
    ObjectColor SegmentableRecognitionWrapper::getObjectColor() const
    {
        if (getEntity()-> hasAttribute(ATTR_OBJECT_COLOR))
        {
            return (ObjectColor) EntityPtr::dynamicCast(getEntity())->getAttributeValue(ATTR_OBJECT_COLOR)->getInt();
        }
 
        return eNone;
    }
 
    void SegmentableRecognitionWrapper::setObjectColor(const ObjectColor& color)
    {
        EntityAttributeBasePtr objectColorAttr = new EntityAttribute(ATTR_OBJECT_COLOR);
        objectColorAttr->setValue(new armarx::Variant(int(color)));
        getEntity()-> putAttribute(objectColorAttr);
    }
 
    Ice::ObjectPtr SegmentableRecognitionWrapper::ice_clone() const
    {
        return new SegmentableRecognitionWrapper(*this);
    }
 
 
    HandMarkerBallWrapper::HandMarkerBallWrapper(const GridFileManagerPtr& gfm):
        AbstractFileEntityWrapper(gfm)
    {
    }
 
    ObjectColor HandMarkerBallWrapper::getObjectColor() const
    {
        if (getEntity()-> hasAttribute(ATTR_OBJECT_COLOR))
        {
            return (ObjectColor) EntityPtr::dynamicCast(getEntity())->getAttributeValue(ATTR_OBJECT_COLOR)->getInt();
        }
        else
        {
            return eNone;
        }
    }
 
    void HandMarkerBallWrapper::setObjectColor(const ObjectColor& color)
    {
        EntityAttributeBasePtr objectColorAttr = new EntityAttribute(ATTR_OBJECT_COLOR);
        objectColorAttr->setValue(new armarx::Variant(int(color)));
        getEntity()-> putAttribute(objectColorAttr);
    }
 
    Ice::ObjectPtr HandMarkerBallWrapper::ice_clone() const
    {
        return new HandMarkerBallWrapper(*this);
    }
 
 
    ArMarkerWrapper::ArMarkerWrapper(const GridFileManagerPtr& gfm):
        AbstractFileEntityWrapper(gfm)
    {
    }
 
    std::vector<int> ArMarkerWrapper::getArMarkerIDs() const
    {
        std::vector<int> result;
 
        if (getEntity()-> hasAttribute(ATTR_AR_MARKER_ID))
        {
            EntityPtr p = EntityPtr::dynamicCast(getEntity());
            EntityAttributeBasePtr attr = p->getAttribute(ATTR_AR_MARKER_ID);
 
            for (int i = 0; i < attr->size(); i++)
            {
                result.push_back(attr->getValueAt(i)->getInt());
            }
        }
        else
        {
            ARMARX_WARNING_S << "Entity doesn't have the requested attribute " << ATTR_AR_MARKER_ID;
        }
 
        return result;
    }
 
    void ArMarkerWrapper::setArMarkerIDs(const std::vector<int>& newMarkerIDs)
    {
        EntityAttributeBasePtr attr = new EntityAttribute(ATTR_AR_MARKER_ID);
 
        for (size_t i = 0; i < newMarkerIDs.size(); i++)
        {
            attr->addValue(new armarx::Variant(newMarkerIDs.at(i)));
        }
 
        getEntity()-> putAttribute(attr);
    }
 
    std::vector<float> ArMarkerWrapper::getArMarkerSizes() const
    {
        std::vector<float> result;
 
        if (getEntity()-> hasAttribute(ATTR_AR_MARKER_SIZE))
        {
            EntityPtr p = EntityPtr::dynamicCast(getEntity());
            EntityAttributeBasePtr attr = p->getAttribute(ATTR_AR_MARKER_SIZE);
 
            for (int i = 0; i < attr->size(); i++)
            {
                result.push_back(attr->getValueAt(i)->getFloat());
            }
        }
        else
        {
            ARMARX_WARNING_S << "Entity doesn't have the requested attribute " << ATTR_AR_MARKER_SIZE;
        }
 
        return result;
    }
 
    void ArMarkerWrapper::setArMarkerSizes(const std::vector<float>& newMarkerSizes)
    {
        EntityAttributeBasePtr attr = new EntityAttribute(ATTR_AR_MARKER_SIZE);
 
        for (size_t i = 0; i < newMarkerSizes.size(); i++)
        {
            attr->addValue(new armarx::Variant(newMarkerSizes.at(i)));
        }
 
        getEntity()-> putAttribute(attr);
    }
 
    std::vector<Eigen::Vector3f> memoryx::EntityWrappers::ArMarkerWrapper::getTransformationToCenterTranslations() const
    {
        std::vector<Eigen::Vector3f> result;
 
        if (getEntity()-> hasAttribute(ATTR_AR_MARKER_TRANSLATION))
        {
            EntityPtr p = EntityPtr::dynamicCast(getEntity());
            EntityAttributeBasePtr attr = p->getAttribute(ATTR_AR_MARKER_TRANSLATION);
 
            for (int i = 0; i < attr->size(); i++)
            {
                armarx::Vector3BasePtr vecBase = armarx::VariantPtr::dynamicCast(attr->getValueAt(i))->getClass<armarx::Vector3Base>();
                Eigen::Vector3f vec;
                vec(0) = vecBase->x;
                vec(1) = vecBase->y;
                vec(2) = vecBase->z;
                result.push_back(vec);
            }
        }
        else
        {
            ARMARX_WARNING_S << "Entity doesn't have the requested attribute " << ATTR_AR_MARKER_TRANSLATION;
        }
 
        return result;
    }
 
    void ArMarkerWrapper::setTransformationToCenterTranslations(const std::vector<Eigen::Vector3f>& newTranslations)
    {
        EntityAttributeBasePtr attr = new EntityAttribute(ATTR_AR_MARKER_TRANSLATION);
 
        for (size_t i = 0; i < newTranslations.size(); i++)
        {
            armarx::Vector3 vec(newTranslations.at(i));
            attr->addValue(new armarx::Variant(vec));
        }
 
        getEntity()-> putAttribute(attr);
    }
 
    std::vector<Eigen::Vector3f> ArMarkerWrapper::getTransformationToCenterRotationsRPY() const
    {
        std::vector<Eigen::Vector3f> result;
 
        if (getEntity()-> hasAttribute(ATTR_AR_MARKER_ROTATION))
        {
            EntityPtr p = EntityPtr::dynamicCast(getEntity());
            EntityAttributeBasePtr attr = p->getAttribute(ATTR_AR_MARKER_ROTATION);
 
            for (int i = 0; i < attr->size(); i++)
            {
                armarx::Vector3BasePtr vecBase = armarx::VariantPtr::dynamicCast(attr->getValueAt(i))->getClass<armarx::Vector3Base>();
                Eigen::Vector3f vec;
                vec(0) = vecBase->x;
                vec(1) = vecBase->y;
                vec(2) = vecBase->z;
                result.push_back(vec);
            }
        }
        else
        {
            ARMARX_WARNING_S << "Entity doesn't have the requested attribute " << ATTR_AR_MARKER_ROTATION;
        }
 
        return result;
    }
 
    void ArMarkerWrapper::setTransformationToCenterRotationsRPY(const std::vector<Eigen::Vector3f>& newRotations)
    {
        EntityAttributeBasePtr attr = new EntityAttribute(ATTR_AR_MARKER_ROTATION);
 
        for (size_t i = 0; i < newRotations.size(); i++)
        {
            armarx::Vector3 vec(newRotations.at(i));
            attr->addValue(new armarx::Variant(vec));
        }
 
        getEntity()-> putAttribute(attr);
    }
 
    std::vector<Eigen::Matrix4f> ArMarkerWrapper::getTransformationsToCenter() const
    {
        std::vector<Eigen::Matrix4f> result;
 
        if (getEntity()-> hasAttribute(ATTR_AR_MARKER_TRANSLATION) && getEntity()-> hasAttribute(ATTR_AR_MARKER_ROTATION))
        {
            EntityPtr p = EntityPtr::dynamicCast(getEntity());
            EntityAttributeBasePtr attrTransl = p->getAttribute(ATTR_AR_MARKER_TRANSLATION);
            EntityAttributeBasePtr attrRot = p->getAttribute(ATTR_AR_MARKER_ROTATION);
 
            for (int i = 0; i < attrTransl->size() && i < attrRot->size(); i++)
            {
                Eigen::Matrix4f newTrafo = Eigen::Matrix4f::Identity();
                armarx::Vector3BasePtr vecBase = armarx::VariantPtr::dynamicCast(attrTransl->getValueAt(i))->getClass<armarx::Vector3Base>();
                newTrafo(0, 3) = vecBase->x;
                newTrafo(1, 3) = vecBase->y;
                newTrafo(2, 3) = vecBase->z;
                vecBase = armarx::VariantPtr::dynamicCast(attrRot->getValueAt(i))->getClass<armarx::Vector3Base>();
                newTrafo.block<3, 3>(0, 0) = (Eigen::AngleAxisf(vecBase->x, Eigen::Vector3f::UnitX())
                                              * Eigen::AngleAxisf(vecBase->y, Eigen::Vector3f::UnitY())
                                              * Eigen::AngleAxisf(vecBase->z, Eigen::Vector3f::UnitZ())).matrix();
                result.push_back(newTrafo);
            }
        }
        else
        {
            ARMARX_WARNING_S << "Entity doesn't have both requested attributes " << ATTR_AR_MARKER_TRANSLATION << " and " << ATTR_AR_MARKER_ROTATION;
        }
 
        return result;
    }
 
    Ice::ObjectPtr ArMarkerWrapper::ice_clone() const
    {
        return new ArMarkerWrapper(*this);
    }
 
 
 
    PointCloudLocalizerWrapper::PointCloudLocalizerWrapper(const GridFileManagerPtr& gfm):
        AbstractFileEntityWrapper(gfm)
    {
    }
 
 
    std::string PointCloudLocalizerWrapper::getPointCloudFileName() const
    {
        if (getEntity()-> hasAttribute(POINTCLOUD_FILE))
        {
            const std::string fileName = cacheAttributeFile(POINTCLOUD_FILE, true);
            return fileName;
        }
 
        return "";
    }
 
 
    void PointCloudLocalizerWrapper::setPointCloudFileName(const std::string& fileName, const std::string& filesDBName)
    {
        if (fileName != "")
        {
            EntityAttributeBasePtr fileAttr = new EntityAttribute(POINTCLOUD_FILE);
            fileManager->storeFileToAttr(filesDBName, fileName, fileAttr);
            cleanUpAttributeFiles(getEntity()-> getAttribute(POINTCLOUD_FILE), fileAttr);
            getEntity()-> putAttribute(fileAttr);
        }
    }
 
 
    void PointCloudLocalizerWrapper::getExpectedMatchingDistance(float& expectedMatchDistance, float& mismatchDistance)
    {
        if (getEntity()-> hasAttribute(POINTCLOUD_EXP_MATCH_DIST) && getEntity()-> hasAttribute(POINTCLOUD_MISMATCH_DIST))
        {
            EntityPtr p = EntityPtr::dynamicCast(getEntity());
            expectedMatchDistance = p->getAttributeValue(POINTCLOUD_EXP_MATCH_DIST)->getFloat();
            mismatchDistance = p->getAttributeValue(POINTCLOUD_MISMATCH_DIST)->getFloat();
        }
        else
        {
            ARMARX_IMPORTANT_S << "Attribute " << POINTCLOUD_EXP_MATCH_DIST << " or " << POINTCLOUD_MISMATCH_DIST << " not set for object " << getEntity()-> getName();
            expectedMatchDistance = 0;
            mismatchDistance = 0;
        }
    }
 
 
    void PointCloudLocalizerWrapper::setExpectedMatchingDistance(const float expectedMatchDistance, const float mismatchDistance)
    {
        EntityAttributePtr expectedMatchDistanceAttr = new EntityAttribute(POINTCLOUD_EXP_MATCH_DIST);
        expectedMatchDistanceAttr->setValue(new armarx::Variant(expectedMatchDistance));
        getEntity()-> putAttribute(expectedMatchDistanceAttr);
        EntityAttributePtr mismatchDistanceAttr = new EntityAttribute(POINTCLOUD_MISMATCH_DIST);
        mismatchDistanceAttr->setValue(new armarx::Variant(mismatchDistance));
        getEntity()-> putAttribute(mismatchDistanceAttr);
    }
 
 
    Ice::ObjectPtr PointCloudLocalizerWrapper::ice_clone() const
    {
        return new PointCloudLocalizerWrapper(*this);
    }
}