SimoxObjectWrapper.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 "SimoxObjectWrapper.h"
 
// ArmarXCore
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
// MemoryX
#include <Inventor/nodes/SoMaterial.h>
#include <MemoryX/libraries/helpers/GaussianMixtureHelpers/WestGMMReducer.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
 
// Simox-VirtualRobot
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
#include <VirtualRobot/XML/ObjectIO.h>
 
// Coin3D headers
#include <Inventor/VRMLnodes/SoVRMLImageTexture.h>
#include <Inventor/actions/SoSearchAction.h>
#include <Inventor/nodes/SoFile.h>
#include <Inventor/nodes/SoGroup.h>
#include <Inventor/nodes/SoImage.h>
#include <Inventor/nodes/SoNode.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoTexture2.h>
#include <Inventor/nodes/SoTexture3.h>
 
// Eigen
#include <cfloat>
#include <filesystem>
 
#include <Eigen/Eigenvalues>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
 
namespace memoryx::EntityWrappers
{
    namespace fs = std::filesystem;
 
    const std::string ATTR_TEXTURE_FILES = "texFiles";
    const std::string ATTR_IV_FILE = "IvFile";
    const std::string ATTR_IV_COLLISION_FILE = "IvFileCollision";
    const std::string ATTR_MANIPULATION_FILE = "ManipulationFile";
    const std::string ATTR_UNCERTAINTY_COLOR_MAP = "UncertaintyColorMap";
    const std::string ATTR_UNCERTAINTY_TRANSPARENCY = "UncertaintyTransparency";
    const std::string ATTR_PUTDOWN_ORIENTATION_RPY = "PutdownOrientationRPY";
 
    const std::string MO_TEMP_FILE_NAME = "___tmpManipulationObject___.xml";
 
    // default uncertainty visu params
    const float ELLIPSE_RADIUS_IN_SIGMAS = 3.;
    const float HEATMAP_MIN_VARIANCE = 1.; // mm
    const float HEATMAP_DENSITY_THRESHOLD = expf(-15.);
    const int HEATMAP_GRID_SIZE = 100;
 
    SimoxObjectWrapper::SimoxObjectWrapper(const GridFileManagerPtr& gfm,
                                           VirtualRobot::RobotIO::RobotDescription loadMode) :
        AbstractFileEntityWrapper(gfm), uncertaintyVisuType(eEllipse)
    {
        coinVisFactory.reset(new VirtualRobot::CoinVisualizationFactory());
 
        cachePath = fileManager->getFileCachePath();
        moTempFile = (cachePath / MO_TEMP_FILE_NAME).string();
 
        ellipseRadiusInSigmas = ELLIPSE_RADIUS_IN_SIGMAS;
        heatmapMinVariance = HEATMAP_MIN_VARIANCE; // mm
        heatmapDensityThreshold = HEATMAP_DENSITY_THRESHOLD;
        heatmapGridSize = HEATMAP_GRID_SIZE;
        if (loadMode != VirtualRobot::RobotIO::eFull &&
            loadMode != VirtualRobot::RobotIO::eCollisionModel)
        {
            throw armarx::LocalException("Loadmode must either be eFull oder eCollisionModel");
        }
        this->loadMode = loadMode;
    }
 
    SimoxObjectWrapper::~SimoxObjectWrapper()
    {
    }
 
    Ice::ObjectPtr
    SimoxObjectWrapper::ice_clone() const
    {
        return new SimoxObjectWrapper(*this);
    }
 
    VirtualRobot::VisualizationNodePtr
    SimoxObjectWrapper::getVisualization() const
    {
        if (simoxObject)
        {
            return simoxObject->getVisualization();
        }
        else
        {
            cacheAttributeFiles(ATTR_TEXTURE_FILES, true);
            const std::string ivFName = cacheAttributeFile(ATTR_IV_FILE, true);
            return coinVisFactory->getVisualizationFromFile(ivFName);
        }
    }
 
    VirtualRobot::CollisionModelPtr
    SimoxObjectWrapper::getCollisionModel() const
    {
        if (simoxObject)
        {
            return simoxObject->getCollisionModel();
        }
        else
        {
            cacheAttributeFiles(ATTR_TEXTURE_FILES, true);
            const std::string ivFName = cacheAttributeFile(ATTR_IV_COLLISION_FILE, true);
            return VirtualRobot::CollisionModelPtr(new VirtualRobot::CollisionModel(
                coinVisFactory->getVisualizationFromFile(ivFName)));
        }
    }
 
    VirtualRobot::ManipulationObjectPtr
    SimoxObjectWrapper::getManipulationObject() const
    {
        ARMARX_TRACE;
        if (!simoxObject)
        {
            ARMARX_TRACE;
            loadSimoxObject();
        }
 
        return simoxObject;
    }
 
    std::string
    SimoxObjectWrapper::getManipulationObjectFileName() const
    {
        cacheAttributeFiles(ATTR_TEXTURE_FILES, true);
        const std::string ivFName = cacheAttributeFile(ATTR_IV_FILE, true);
        const std::string ivFNameCollision = cacheAttributeFile(ATTR_IV_COLLISION_FILE, true);
 
        const std::string moFName = cacheAttributeFile(ATTR_MANIPULATION_FILE, true);
 
        return moFName;
    }
 
    std::string
    SimoxObjectWrapper::setAndStoreManipulationObject(const VirtualRobot::ManipulationObjectPtr mo,
                                                      const std::string& filesDBName)
    {
        setManipulationObject(mo);
        return storeManipulationObject(mo, filesDBName);
    }
 
    void
    SimoxObjectWrapper::setManipulationObject(const VirtualRobot::ManipulationObjectPtr mo)
    {
        //    clear(false);
        simoxObject = mo;
    }
 
    void
    SimoxObjectWrapper::setAndStoreManipulationFile(const std::string& xmlFName,
                                                    const std::string& filesDBName)
    {
        if (!xmlFName.empty())
        {
            VirtualRobot::ManipulationObjectPtr mo = loadManipulationObjectFile(xmlFName);
            const std::string fileId = setAndStoreManipulationObject(mo, filesDBName);
            ARMARX_INFO_S << " Saved manipulation object file: " << xmlFName << " (Id: " << fileId
                          << ")";
        }
        else
        {
            ARMARX_WARNING_S << "xmlFName empty";
        }
    }
 
    void
    SimoxObjectWrapper::setManipulationFile(const std::string& xmlFName)
    {
        if (!xmlFName.empty())
        {
            ARMARX_INFO_S << " Setting manipulation object file: " << xmlFName << armarx::flush;
            VirtualRobot::ManipulationObjectPtr mo = loadManipulationObjectFile(xmlFName);
            setManipulationObject(mo);
        }
    }
 
    void
    SimoxObjectWrapper::setAndStoreModelIVFiles(const std::string& ivFNameVis,
                                                const std::string& ivFNameCollision,
                                                const std::string& filesDBName)
    {
        VirtualRobot::ManipulationObjectPtr mo = createManipulationObjectFromIvFiles(
            getEntity()->getName(), ivFNameVis, ivFNameCollision);
        const std::string ivFileId = setAndStoreManipulationObject(mo, filesDBName);
        ARMARX_INFO_S << " Saved IV model file: " << ivFNameVis << " (Id: " << ivFileId << ")";
    }
 
    void
    SimoxObjectWrapper::setModelIVFiles(const std::string& ivFNameVis,
                                        const std::string& ivFNameCollision)
    {
        VirtualRobot::ManipulationObjectPtr mo = createManipulationObjectFromIvFiles(
            getEntity()->getName(), ivFNameVis, ivFNameCollision);
        setManipulationObject(mo);
    }
 
    void
    SimoxObjectWrapper::setUncertaintyVisuType(UncertaintyVisuType visuType)
    {
        this->uncertaintyVisuType = visuType;
    }
 
    void
    SimoxObjectWrapper::setUncertaintyVisuParams(float ellipseRadiusInSigmas,
                                                 float heatmapMinVariance,
                                                 float heatmapDensityThreshold,
                                                 int heatmapGridSize)
    {
        this->ellipseRadiusInSigmas = ellipseRadiusInSigmas;
        this->heatmapMinVariance = heatmapMinVariance;
        this->heatmapDensityThreshold = heatmapDensityThreshold;
        this->heatmapGridSize = heatmapGridSize;
    }
 
    VirtualRobot::ColorMap
    SimoxObjectWrapper::getUncertaintyColorMap()
    {
        if (getEntity()->hasAttribute(ATTR_UNCERTAINTY_COLOR_MAP))
        {
            const std::string mapName =
                getEntity()->getAttribute(ATTR_UNCERTAINTY_COLOR_MAP)->getValue()->getString();
 
            if (mapName == "eHot")
            {
                return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHot);
            }
            else if (mapName == "eIntensity")
            {
                return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eIntensity);
            }
            else if (mapName == "eRed")
            {
                return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eRed);
            }
            else if (mapName == "eGreen")
            {
                return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eGreen);
            }
            else if (mapName == "eBlue")
            {
                return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eBlue);
            }
            else if (mapName == "eHotAlpha")
            {
                return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHotAlpha);
            }
            else if (mapName == "eRedAlpha")
            {
                return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eRedAlpha);
            }
            else if (mapName == "eGreenAlpha")
            {
                return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eGreenAlpha);
            }
            else if (mapName == "eBlueAlpha")
            {
                return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eBlueAlpha);
            }
            else
            {
                return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHot);
            }
        }
        else
        {
            return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHot);
        }
    }
 
    float
    SimoxObjectWrapper::getUncertaintyEllipseTransparency()
    {
        if (getEntity()->hasAttribute(ATTR_UNCERTAINTY_TRANSPARENCY))
        {
            return getEntity()->getAttribute(ATTR_UNCERTAINTY_TRANSPARENCY)->getValue()->getFloat();
        }
        else
        {
            return 0.3f;
        }
    }
 
    void
    SimoxObjectWrapper::updateFromEntity(const EntityBasePtr& entity)
    {
        setEntity(entity);
 
        refreshVisuPose();
    }
 
    void
    SimoxObjectWrapper::refreshVisuPose()
    {
        //    std::cout << "Refreshing visu pose... " << std::endl;
 
        // only object instances are supported so far
        ObjectInstancePtr obj = ObjectInstancePtr::dynamicCast(getEntity());
 
        if (!obj)
        {
            return;
        }
 
        Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
 
        // Position
        armarx::FramedPositionPtr objPos = obj->getPosition();
        if (objPos)
        {
            m.block(0, 3, 3, 1) = objPos->toEigen();
        }
 
        // Orientation
        armarx::FramedOrientationPtr objOrient = obj->getOrientation();
 
        if (objOrient)
        {
            m.block(0, 0, 3, 3) = objOrient->toEigen();
        }
 
        VirtualRobot::ManipulationObjectPtr mo = getManipulationObject();
 
        if (!mo)
        {
            //ARMARX_WARNING_S << "Manipulation object is null";
        }
        else
        {
            mo->setGlobalPose(m);
        }
 
 
        // Position uncertainty
        EntityAttributeBasePtr posAttr = obj->getPositionAttribute();
        ProbabilityMeasureBasePtr posDist = posAttr->getUncertainty();
 
        if (posDist)
        {
            static const std::string UNCERTAINTY_VISU_NAME = "positionUncertainty";
            VirtualRobot::VisualizationNodePtr moVis;
 
            if (mo)
            {
                moVis = mo->getVisualization();
 
                if (moVis)
                {
                    moVis->detachVisualization(UNCERTAINTY_VISU_NAME);
                }
                else
                {
                    //ARMARX_WARNING_S << "Manipulation object's visu is null";
                }
            }
 
 
            GaussianMixtureDistributionBasePtr posGM =
                GaussianMixtureDistribution::FromProbabilityMeasure(posDist);
 
            if (posGM)
            {
 
                VirtualRobot::VisualizationNodePtr posDistVisu;
 
                posGM->normalize();
 
                const VirtualRobot::ColorMap cmap = getUncertaintyColorMap();
 
                switch (uncertaintyVisuType)
                {
                    case eEllipse:
                        posDistVisu = getUncertaintyEllipsesVisu(
                            posGM, objPos->toEigen(), cmap, getUncertaintyEllipseTransparency());
                        break;
 
                    case eHeatMap:
                        posDistVisu = getUncertaintyHeatVisu(posGM, objPos->toEigen(), false, cmap);
                        break;
 
                    case eHeatSurface:
                        posDistVisu = getUncertaintyHeatVisu(posGM, objPos->toEigen(), true, cmap);
                        break;
 
                    default:
                        ARMARX_ERROR_S << "Uncertainty visualization type not supported!"
                                       << armarx::flush;
                }
 
                if (posDistVisu)
                {
                    // Object orientation shouldn't affect uncertainty, but it does,
                    // since technically attached visu's are child nodes of main object.
                    // Thus apply inverse transformation to compensate for this
                    Eigen::Matrix3f objOrientInv = objOrient->toEigen().inverse();
                    Eigen::Matrix4f dm = Eigen::Matrix4f::Identity();
                    dm.block(0, 0, 3, 3) = objOrientInv;
                    posDistVisu->setGlobalPose(dm);
 
                    if (moVis)
                    {
                        moVis->attachVisualization(UNCERTAINTY_VISU_NAME, posDistVisu);
                    }
                    else
                    {
                        //ARMARX_WARNING_S << "Manipulation object's visu is null";
                    }
                }
            }
        }
    }
 
    VirtualRobot::VisualizationNodePtr
    SimoxObjectWrapper::getUncertaintyEllipsesVisu(GaussianMixtureDistributionBasePtr posGM,
                                                   const Eigen::Vector3f& objPos,
                                                   const VirtualRobot::ColorMap& cmap,
                                                   float transparency)
    {
        static float COV_SCALE_FACTOR = 1.f; // make cov more realistic
 
        std::vector<VirtualRobot::VisualizationNodePtr> compVisuList;
 
        for (int i = 0; i < posGM->size(); ++i)
        {
            GaussianMixtureComponent comp = posGM->getComponent(i);
            NormalDistributionPtr gaussian = NormalDistributionPtr::dynamicCast(comp.gaussian);
 
            Eigen::Matrix3f cov = gaussian->toEigenCovariance() * COV_SCALE_FACTOR;
 
            Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> es(cov);
            Eigen::Vector3f eval = es.eigenvalues();
            Eigen::Matrix3f evec = es.eigenvectors();
            //        std::cout << "evalues: " << eval << ", evectors: " << evec << std::endl << std::endl;
 
            VirtualRobot::VisualizationFactory::Color color = cmap.getColor(comp.weight);
 
            SoMaterial* matBody = new SoMaterial;
            matBody->diffuseColor.setValue(color.r, color.b, color.g);
            matBody->ambientColor.setValue(color.r, color.b, color.g);
            matBody->transparency.setValue(transparency);
 
            VirtualRobot::VisualizationNodePtr compVisu(
                new VirtualRobot::CoinVisualizationNode(coinVisFactory->CreateEllipse(
                    ellipseRadiusInSigmas * sqrt(eval(0)) / 1000., // mm -> m
                    ellipseRadiusInSigmas * sqrt(eval(1)) / 1000.,
                    ellipseRadiusInSigmas * sqrt(eval(2)) / 1000.,
                    matBody, // default material
                    false // hide axis
                    )));
 
            Eigen::Matrix4f dm = Eigen::Matrix4f::Identity();
            dm.block(0, 0, 3, 3) = evec;
            dm.block(0, 3, 3, 1) = gaussian->toEigenMean() - objPos;
            compVisu->setGlobalPose(dm);
 
            compVisuList.push_back(compVisu);
        }
 
        return coinVisFactory->createUnitedVisualization(compVisuList);
    }
 
    VirtualRobot::VisualizationNodePtr
    SimoxObjectWrapper::getUncertaintyHeatVisu(GaussianMixtureDistributionBasePtr posGM,
                                               const Eigen::Vector3f& objPos,
                                               bool heatSurface,
                                               const VirtualRobot::ColorMap& cmap)
    {
        const float SCALE_FACTOR = sqrt(
            2 * M_PI *
            heatmapMinVariance); // scale axes in a way that the height of distribution with sigmaX=sigmaY=1mm is 1.0
        const float COV_SCALE_FACTOR = 1.f; // make cov more realistic
        const float LOG_PROB_CUTOFF = -logf(heatmapDensityThreshold);
        const float MAX_CLUSTER_VARIANCE = 100; // AABB side size in mm
 
        std::vector<VirtualRobot::VisualizationNodePtr> clusterVisuList;
 
        WestGMMReducer reducer;
        GaussianMixtureDistributionBasePtr reducedGMM =
            reducer.reduceByMaxAABB(posGM, MAX_CLUSTER_VARIANCE);
 
        // iterate over clusters
        for (int k = 0; k < reducedGMM->size(); ++k)
        {
 
            // calculate common mean
            /*        float minX = FLT_MAX, minY = FLT_MAX, maxX = FLT_MIN, maxY = FLT_MIN;
                    for (int i = 0; i < posGM->size(); ++i)
                    {
                        GaussianMixtureComponent comp = posGM->getComponent(i);
                        NormalDistributionPtr gaussian = NormalDistributionPtr::dynamicCast(comp.gaussian);
 
                        Eigen::Vector3f mean = gaussian->toEigenMean();
                        float varX = 6. * sqrt(gaussian->getCovariance(0, 0) * COV_SCALE_FACTOR);
                        float varY = 6. * sqrt(gaussian->getCovariance(1, 1) * COV_SCALE_FACTOR);
                        minX = std::min(minX, mean(0) - varX);
                        minY = std::min(minY, mean(1) - varY);
                        maxX = std::max(maxX, mean(0) + varX);
                        maxY = std::max(maxY, mean(1) + varY);
                    }
                    float sx = maxX - minX;
                    float sy = maxY - minY;*/
 
            NormalDistributionPtr compGaussian =
                NormalDistributionPtr::dynamicCast(reducedGMM->getComponent(k).gaussian);
            Eigen::Vector3f clusterCenter = compGaussian->toEigenMean();
            //        clusterCenter << minX + sx / 2., minY + sy / 2., objPos(2);
            std::cout << "Cluster mean: " << clusterCenter << std::endl;
            //        std::cout << "MinX: " << minX << ", MaxX: " << maxX << ", minY: " << minY << ", MaxY: " << maxY << std::endl;
 
            clusterCenter /= SCALE_FACTOR; // scale mean
 
            float sx = 6. * sqrtf(compGaussian->getCovariance(0, 0));
            float sy = 6. * sqrtf(compGaussian->getCovariance(1, 1));
            int GRID_SIZE_X = (int)round(sqrt(sx / sy) * (float)heatmapGridSize);
            int GRID_SIZE_Y = (int)round(sqrt(sy / sx) * (float)heatmapGridSize);
            float CELL_SIZE_X = sx / (float)GRID_SIZE_X / SCALE_FACTOR; //
            float CELL_SIZE_Y = sy / (float)GRID_SIZE_Y / SCALE_FACTOR; //
 
            Eigen::MatrixXf grid = Eigen::MatrixXf::Zero(GRID_SIZE_X, GRID_SIZE_Y);
 
            for (int i = 0; i < posGM->size(); ++i)
            {
                GaussianMixtureComponent comp = posGM->getComponent(i);
                NormalDistributionPtr gaussian = NormalDistributionPtr::dynamicCast(comp.gaussian);
 
                FloatVector meanVec = comp.gaussian->getMean();
                Eigen::Vector2f mean;
                mean << meanVec[0], meanVec[1];
                mean /= SCALE_FACTOR; // scale mean
 
                Eigen::Matrix3f cov3D = gaussian->toEigenCovariance();
                Eigen::Matrix2f cov =
                    cov3D.block(0, 0, 2, 2) * COV_SCALE_FACTOR / (SCALE_FACTOR * SCALE_FACTOR);
                Eigen::Matrix2f covInv = cov.inverse();
                float covDet = cov.determinant();
                float covDetSqrtInv = powf(covDet, -0.5f);
 
                Eigen::Vector2f pos;
                pos(0) = clusterCenter(0) - (float)(GRID_SIZE_X * CELL_SIZE_X) / 2.;
 
                for (int xc = 0; xc < GRID_SIZE_X; ++xc)
                {
                    pos(1) = clusterCenter(1) - (float)(GRID_SIZE_Y * CELL_SIZE_Y) / 2.;
 
                    for (int yc = 0; yc < GRID_SIZE_Y; ++yc)
                    {
                        float pdfValue =
                            (LOG_PROB_CUTOFF +
                             log(evaluate2DGaussian(pos, mean, covInv, covDetSqrtInv))) /
                            LOG_PROB_CUTOFF * comp.weight;
 
                        // normalize value to [0, 1]
                        if (pdfValue < 0.)
                        {
                            // probablity density too low -> set it to 0, since we can ignore such small differences for visu
                            //                    std::cout << "Cutting off value: " << pdfValue << ", new value: 0.0" << std::endl;
                            pdfValue = 0.f;
                        }
                        else if (pdfValue > 1.)
                        {
                            // probablity density too high (i.e. stddev < 1 mm) -> cut off at 1
                            pdfValue = 1.f;
                        }
 
                        grid(xc, yc) += pdfValue;
 
                        //                std::cout << "Pos: " << pos << ", mean: " << mean << ", value: " << grid(xc, yc) << std::endl;
                        pos(1) += CELL_SIZE_Y; // step y
                    }
 
                    pos(0) += CELL_SIZE_X; // step x
                }
            }
 
            VirtualRobot::VisualizationNodePtr clusterVisu;
 
            if (heatSurface)
            {
                clusterVisu.reset(
                    new VirtualRobot::CoinVisualizationNode(coinVisFactory->Create2DHeightMap(
                        grid, // prob map
                        CELL_SIZE_X * SCALE_FACTOR, // x cell size in mm
                        CELL_SIZE_Y * SCALE_FACTOR, // y cell size in mm
                        100., // height in mm
                        cmap, // color map
                        false, // draw zero cells
                        false // draw lines between cells
                        )));
            }
            else
            {
                clusterVisu.reset(new VirtualRobot::CoinVisualizationNode(
                    coinVisFactory->Create2DMap(grid, // prob map
                                                CELL_SIZE_X * SCALE_FACTOR, // x cell size in mm
                                                CELL_SIZE_Y * SCALE_FACTOR, // y cell size in mm
                                                cmap, // color map
                                                false, // draw zero cells
                                                false // draw lines between cells
                                                )));
            }
 
            Eigen::Matrix4f dm = Eigen::Matrix4f::Identity();
            dm.block(0, 3, 3, 1) = clusterCenter * SCALE_FACTOR - objPos;
            clusterVisu->setGlobalPose(dm);
 
            clusterVisuList.push_back(clusterVisu);
        } // end clusters loop
 
        return coinVisFactory->createUnitedVisualization(clusterVisuList);
        ;
    }
 
    float
    SimoxObjectWrapper::evaluate2DGaussian(const Eigen::Vector2f& point,
                                           const Eigen::Vector2f& mean,
                                           const Eigen::Matrix2f& covInv,
                                           float covDetSqrtInv)
    {
        Eigen::Matrix<float, 1, 1> inner =
            -0.5 * (point - mean).transpose() * covInv * (point - mean);
        float e = exp(inner(0, 0));
 
        static const float c = 1. / 2 * M_PI;
 
        return c * covDetSqrtInv * e;
    }
 
    float
    SimoxObjectWrapper::evaluate3DGaussian(const Eigen::Vector3f& point,
                                           const Eigen::Vector3f& mean,
                                           const Eigen::Matrix3f& covInv,
                                           float covDet)
    {
        Eigen::Matrix<float, 1, 1> inner =
            -0.5 * (point - mean).transpose() * covInv * (point - mean);
        float e = exp(inner(0, 0));
 
        return pow(2 * M_PI, -3.0f / 2.0f) * pow(covDet, -0.5) * e;
    }
 
    void
    SimoxObjectWrapper::clear(bool removeFiles /* = true */)
    {
        if (getEntity() && removeFiles)
        {
            fileManager->removeAttrFiles(getEntity()->getAttribute(ATTR_MANIPULATION_FILE));
            fileManager->removeAttrFiles(getEntity()->getAttribute(ATTR_IV_COLLISION_FILE));
            fileManager->removeAttrFiles(getEntity()->getAttribute(ATTR_IV_FILE));
            fileManager->removeAttrFiles(getEntity()->getAttribute(ATTR_TEXTURE_FILES));
        }
 
        simoxObject.reset();
    }
 
    /*
     * private methods
     */
    void
    SimoxObjectWrapper::loadSimoxObject() const
    {
        ARMARX_TRACE;
        if (loadMode != VirtualRobot::RobotIO::eStructure)
        {
            ARMARX_TRACE;
            cacheAttributeFiles(ATTR_TEXTURE_FILES, true);
        }
 
        const std::string ivFName = (loadMode == VirtualRobot::RobotIO::eFull ||
                                     (loadMode == VirtualRobot::RobotIO::eCollisionModel &&
                                      !getEntity()->hasAttribute(ATTR_IV_COLLISION_FILE)))
                                        ? cacheAttributeFile(ATTR_IV_FILE, true)
                                        : "";
        ARMARX_TRACE;
        const std::string ivFNameCollision = (loadMode == VirtualRobot::RobotIO::eFull ||
                                              loadMode == VirtualRobot::RobotIO::eCollisionModel)
                                                 ? cacheAttributeFile(ATTR_IV_COLLISION_FILE, true)
                                                 : "";
        ARMARX_TRACE;
        const std::string moFName = cacheAttributeFile(ATTR_MANIPULATION_FILE, true);
 
        ARMARX_TRACE;
        if (moFName.empty() && ivFName.empty() && ivFNameCollision.empty())
        {
            ARMARX_WARNING_S << "SimoxWrapper was not able to find ManipulationObjectFile or an "
                                "ivFile an ivCollision of ObjectInstance "
                             << getEntity()->getName() << std::endl;
        }
        else if (ivFName.empty() && loadMode == VirtualRobot::RobotIO::eFull)
        {
            ARMARX_WARNING_S
                << "SimoxWrapper was not able to find IvFile Attribute of ObjectInstance "
                << getEntity()->getName() << std::endl;
        }
        else if (ivFNameCollision.empty() && loadMode == VirtualRobot::RobotIO::eCollisionModel)
        {
            ARMARX_WARNING_S
                << "SimoxWrapper was not able to find ivFNameCollision Attribute of ObjectInstance "
                << getEntity()->getName() << std::endl;
        }
 
        if (!moFName.empty())
        {
            ARMARX_TRACE;
            ARMARX_DEBUG_S << "Loading simox object " << moFName;
            simoxObject = loadManipulationObjectFile(moFName);
        }
        else
        {
            ARMARX_TRACE;
            simoxObject = createManipulationObjectFromIvFiles(
                getEntity()->getName(), ivFName, ivFNameCollision);
        }
    }
 
    std::string
    SimoxObjectWrapper::cacheAttributeFile(const std::string& attrName,
                                           bool preserveOriginalFName /* = false */) const
    {
        ARMARX_TRACE;
        std::string result = "";
 
        if (getEntity()->hasAttribute(attrName))
        {
            ARMARX_TRACE;
            ARMARX_DEBUG_S << "Caching file(s) from attribute \"" << attrName << "\"";
 
            fileManager->ensureFileInCache(
                getEntity()->getAttribute(attrName), result, preserveOriginalFName);
        }
        else
        {
            ARMARX_DEBUG_S << "Attribute \"" << attrName << "\" not available";
        }
 
        return result;
    }
 
    bool
    SimoxObjectWrapper::cacheAttributeFiles(const std::string& attrName,
                                            bool preserveOriginalFName /* = false */) const
    {
        if (getEntity()->hasAttribute(attrName))
        {
            //        ARMARX_INFO_S << "Caching file(s) from attribute \"" << attrName << "\"" << armarx::flush;
 
            return fileManager->ensureFilesInCache(getEntity()->getAttribute(attrName),
                                                   preserveOriginalFName);
        }
        else
        {
            return false;
        }
    }
 
    VirtualRobot::ManipulationObjectPtr
    SimoxObjectWrapper::loadManipulationObjectFile(const std::string& xmlFName) const
    {
        return VirtualRobot::ObjectIO::loadManipulationObject(xmlFName);
    }
 
    VirtualRobot::ManipulationObjectPtr
    SimoxObjectWrapper::createManipulationObjectFromIvFiles(
        const std::string& objName,
        const std::string& ivFNameVis,
        const std::string& ivFNameCollision) const
    {
        VirtualRobot::ManipulationObjectPtr mo(new VirtualRobot::ManipulationObject(objName));
 
        mo->setFilename(objName + ".xml");
 
        VirtualRobot::VisualizationNodePtr visFull =
            (!ivFNameVis.empty()) ? coinVisFactory->getVisualizationFromFile(ivFNameVis)
                                  : VirtualRobot::VisualizationNodePtr();
 
        if (visFull)
        {
            mo->setVisualization(visFull);
        }
 
        VirtualRobot::VisualizationNodePtr visCollision =
            ivFNameCollision.empty() ? VirtualRobot::VisualizationNodePtr()
                                     : coinVisFactory->getVisualizationFromFile(ivFNameCollision);
 
        if (visCollision)
        {
            VirtualRobot::CollisionModelPtr collisionModel(
                new VirtualRobot::CollisionModel(visCollision));
            mo->setCollisionModel(collisionModel);
        }
 
        return mo;
    }
 
    std::string
    SimoxObjectWrapper::storeManipulationObject(const VirtualRobot::ManipulationObjectPtr mo,
                                                const std::string& filesDBName)
    {
        if (!mo)
        {
            return "";
        }
 
        EntityAttributeBasePtr oldMOFileAttr = getEntity()->getAttribute(ATTR_MANIPULATION_FILE);
 
 
        // store .iv files and their textures first
        fs::path visuFName = mo->getVisualization()->getFilename();
        fs::path collisionFName = mo->getCollisionModel()->getVisualization()->getFilename();
        storeEntityIVFiles(visuFName.string(), collisionFName.string(), filesDBName);
 
        // we don't want relative filepaths for IVs in MO XML file, since they would become invalid after caching on local system
        // so create a new XML with IVs truncated to filenames only
        const std::string onlyFilenameVisu = visuFName.filename().string();
        mo->getVisualization()->setFilename(onlyFilenameVisu,
                                            mo->getVisualization()->usedBoundingBoxVisu());
 
        const std::string onlyFilenameCol = collisionFName.filename().string();
        mo->getCollisionModel()->getVisualization()->setFilename(
            onlyFilenameCol, mo->getCollisionModel()->getVisualization()->usedBoundingBoxVisu());
 
        VirtualRobot::ObjectIO::saveManipulationObject(mo, moTempFile);
 
        EntityAttributeBasePtr moAttr = new EntityAttribute(ATTR_MANIPULATION_FILE);
 
 
        std::string fileId = fileManager->storeFileToAttr(
            filesDBName, moTempFile, moAttr, fs::path(mo->getFilename()).filename().string());
 
        cleanUpAttributeFiles(oldMOFileAttr, moAttr);
        getEntity()->putAttribute(moAttr);
 
        fs::remove(moTempFile);
        return fileId;
    }
 
    void
    SimoxObjectWrapper::storeEntityIVFiles(const std::string& visuFName,
                                           const std::string& collisionFName,
                                           const std::string& filesDBName,
                                           bool processTextures)
    {
        NameList textures;
        NameList::const_iterator itTex;
 
        EntityAttributeBasePtr oldIVColFileAttr = getEntity()->getAttribute(ATTR_IV_COLLISION_FILE);
        EntityAttributeBasePtr oldIVFileAttr = getEntity()->getAttribute(ATTR_IV_FILE);
        EntityAttributeBasePtr oldTextureFileAttr = getEntity()->getAttribute(ATTR_TEXTURE_FILES);
 
        EntityAttributeBasePtr visuAttr = new EntityAttribute(ATTR_IV_FILE);
 
        std::string visu_filename = visuFName;
        if (!visu_filename.empty())
        {
            // Make filename an absolute path if needed
            if (std::filesystem::path(visu_filename).is_relative())
            {
                visu_filename = (cachePath / visu_filename).string();
            }
 
            ARMARX_INFO << " Saving visualization iv file: " << visu_filename;
            fileManager->storeFileToAttr(filesDBName, visu_filename, visuAttr);
            cleanUpAttributeFiles(oldIVFileAttr, visuAttr);
            getEntity()->putAttribute(visuAttr);
 
            if (processTextures)
            {
                SimoxObjectWrapper::FindIvTextures(visu_filename, textures);
            }
        }
 
        std::string col_filename = collisionFName;
        if (!col_filename.empty())
        {
            // Make filename an absolute path if needed
            if (std::filesystem::path(col_filename).is_relative())
            {
                col_filename = (cachePath / col_filename).string();
            }
 
            ARMARX_INFO_S << " Saving collision iv file: " << col_filename << armarx::flush;
            EntityAttributeBasePtr collisionAttr = new EntityAttribute(ATTR_IV_COLLISION_FILE);
 
            // if the same .iv-file is used as both visu and collision model, we don't need to store it again,
            // just copy a reference instead
            if (col_filename == visu_filename)
            {
                collisionAttr->addValue(visuAttr->getValue());
            }
            else
            {
                fileManager->storeFileToAttr(filesDBName, col_filename, collisionAttr);
 
                if (processTextures)
                {
                    SimoxObjectWrapper::FindIvTextures(col_filename, textures);
                }
            }
 
            cleanUpAttributeFiles(oldIVColFileAttr, collisionAttr);
            getEntity()->putAttribute(collisionAttr);
        }
 
 
        if (!textures.empty())
        {
            EntityAttributeBasePtr texAttr = new EntityAttribute(ATTR_TEXTURE_FILES);
 
            //fileManager->storeFilesToAttr(filesDBName,localBaseDir,textures,texAttr);
            for (itTex = textures.begin(); itTex != textures.end(); ++itTex)
            {
                ARMARX_INFO_S << " Saving texture file: " << *itTex << armarx::flush;
                fileManager->storeFileToAttr(filesDBName, *itTex, texAttr);
            }
 
            cleanUpAttributeFiles(oldTextureFileAttr, texAttr);
            getEntity()->putAttribute(texAttr);
        }
        else if (
            oldTextureFileAttr) // new object has no texture, remove old texture attributes and files
        {
            removeAttributeFiles(oldTextureFileAttr);
            getEntity()->removeAttribute(oldTextureFileAttr->getName());
        }
    }
 
    void
    SimoxObjectWrapper::FindIvTextures(const std::string& ivFName, NameList& textures)
    {
        // TODO replace with CoinVisualizationFactory call ???
        SoInput in;
 
        if (!in.openFile(ivFName.c_str()))
        {
            ARMARX_WARNING_S << "Could not open iv file:" << ivFName << std::endl;
            return;
        }
 
        SoNode* n = SoDB::readAll(&in);
        n->ref();
        SimoxObjectWrapper::GetAllFilenames(n, textures, ivFName);
        n->unref();
    }
 
    Eigen::Vector3f
    SimoxObjectWrapper::getPutdownOrientationRPY()
    {
        Eigen::Vector3f result = {0, 0, 0};
 
        if (getEntity()->hasAttribute(ATTR_PUTDOWN_ORIENTATION_RPY))
        {
            EntityPtr p = EntityPtr::dynamicCast(getEntity());
            EntityAttributeBasePtr attr = p->getAttribute(ATTR_PUTDOWN_ORIENTATION_RPY);
 
            if (attr->size() > 0)
            {
                armarx::Vector3BasePtr vecBase =
                    armarx::VariantPtr::dynamicCast(attr->getValueAt(0))
                        ->getClass<armarx::Vector3Base>();
                result(0) = vecBase->x;
                result(1) = vecBase->y;
                result(2) = vecBase->z;
            }
        }
 
        return result;
    }
 
    void
    SimoxObjectWrapper::setPutdownOrientationRPY(Eigen::Vector3f& rpy)
    {
        EntityAttributeBasePtr attr = new EntityAttribute(ATTR_PUTDOWN_ORIENTATION_RPY);
        armarx::Vector3 vec(rpy);
        attr->addValue(new armarx::Variant(vec));
        getEntity()->putAttribute(attr);
    }
 
    void
    SimoxObjectWrapper::GetAllFilenames(SoNode* node,
                                        std::vector<std::string>& storeFilenames,
                                        const std::string& origFile)
    {
        if (!node)
        {
            return;
        }
 
        if (node->getTypeId() == SoFile::getClassTypeId())
        {
            // get filename
            SoFile* fileNode = (SoFile*)node;
            SbString fileNodeName = fileNode->getFullName();
 
            if (!fileNodeName)
            {
                ARMARX_INFO_S << "Empty file?!";
                SbString s2 = fileNode->name.getValue();
 
                if (!s2)
                {
                    ARMARX_INFO_S << "Empty relative name";
                }
                else
                {
                    storeFilenames.push_back(s2.getString());
                }
            }
            else
            {
                storeFilenames.push_back(fileNodeName.getString());
            }
 
            // process file data
            SoGroup* fileChildren = fileNode->copyChildren();
            SimoxObjectWrapper::GetAllFilenames(
                fileChildren, storeFilenames, fileNodeName.getString());
        }
        else if (node->getTypeId().isDerivedFrom(SoGroup::getClassTypeId()))
        {
            SoGroup* groupNode = (SoGroup*)node;
 
            // process group node
            for (int i = 0; i < groupNode->getNumChildren(); i++)
            {
                SimoxObjectWrapper::GetAllFilenames(
                    groupNode->getChild(i), storeFilenames, origFile);
            }
        }
        else if (node->getTypeId() == SoImage::getClassTypeId())
        {
            // get image filename
            SbString imageFilename = ((SoImage*)node)->filename.getValue();
            storeFilenames.push_back(SimoxObjectWrapper::GetAbsolutePath(imageFilename, origFile));
        }
        else if (node->getTypeId() == SoTexture2::getClassTypeId())
        {
            // get filename
            SbString texture2Filename = ((SoTexture2*)node)->filename.getValue();
            storeFilenames.push_back(
                SimoxObjectWrapper::GetAbsolutePath(texture2Filename, origFile));
        }
        else if (node->getTypeId() == SoTexture3::getClassTypeId())
        {
            ARMARX_WARNING_S << "Texture3 nyi..." << std::endl;
        }
        else //if (node->getTypeId() == SoVRMLImageTexture::getClassTypeId())
        {
            SoSearchAction sa;
            sa.setType(SoVRMLImageTexture::getClassTypeId());
            sa.setInterest(SoSearchAction::ALL);
            sa.setSearchingAll(TRUE);
            sa.apply(node);
 
            SoPathList& pathList = sa.getPaths();
            if (pathList.getLength() <= 0)
            {
                return;
            }
            SoFullPath* p = (SoFullPath*)pathList[0];
            if (!p->getTail()->isOfType(SoVRMLImageTexture::getClassTypeId()))
            {
                return;
            }
            SoVRMLImageTexture* texture = (SoVRMLImageTexture*)p->getTail();
            if (texture->url.getNum() <= 0)
            {
                return;
            }
            for (int i = 0; i < texture->url.getNum(); ++i)
            {
                auto path = SimoxObjectWrapper::GetAbsolutePath(texture->url[i], origFile);
                if (!path.empty() && fs::exists(path))
                {
                    storeFilenames.push_back(path);
                    break;
                }
                if (i == texture->url.getNum() - 1)
                {
                    Ice::StringSeq textures;
                    for (int j = 0; j < texture->url.getNum(); ++j)
                    {
                        textures.push_back(texture->url[j].getString());
                    }
                    ARMARX_ERROR << "Could not make any of the texture paths absolute: "
                                 << simox::alg::join(textures, ", ");
                }
            }
 
            //        ARMARX_IMPORTANT_S << "VRML ImageTexture of node: " << node->getName().getString() << " : " << texture->url[0].getString() << VAROUT(origFile) << VAROUT(storeFilenames);
        }
    }
 
    std::string
    SimoxObjectWrapper::GetAbsolutePath(SbString filename, const std::string& origFile)
    {
        if (!filename)
        {
            //ARMARX_INFO_S << "Empty relative name"; // skip
            return std::string();
        }
        else
        {
            fs::path filepath(armarx::ArmarXDataPath::cleanPath(filename.getString()));
            fs::path absOrigFileDirPath;
            std::string resultPathStr;
            if (!origFile.empty())
            {
                absOrigFileDirPath =
                    fs::path(armarx::ArmarXDataPath::cleanPath(origFile)).parent_path();
            }
            if (filepath.is_absolute())
            {
                try
                {
                    return armarx::ArmarXDataPath::relativeTo(absOrigFileDirPath.string(),
                                                              filepath.string());
                }
                catch (...)
                {
                    return std::string();
                }
            }
            else if (!filepath.has_parent_path())
            {
                return (absOrigFileDirPath / filepath).string();
            }
            else if (armarx::ArmarXDataPath::mergePaths(
                         absOrigFileDirPath.string(), filepath.string(), resultPathStr))
            {
                return resultPathStr;
            }
 
            //        completeFile = relativePath / fs::path(filename.getString());
        }
        return std::string();
    }
} // namespace memoryx::EntityWrappers