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);
    }
} // namespace memoryx::EntityWrappers