CommonPlacesTester.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "CommonPlacesTester.h"
 
#include <RobotAPI/libraries/core/Pose.h>
 
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <MemoryX/libraries/helpers/GaussianMixtureHelpers/ISDDistance.h>
#include <MemoryX/libraries/helpers/GaussianMixtureHelpers/MahalanobisDistance.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
 
// Object Factories
#include <fstream>
#include <iostream>
#include <thread>
 
#include <Eigen/Core>
 
#include <ArmarXCore/observers/ObserverObjectFactories.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
 
namespace memoryx
{
    void
    CommonPlacesTester::onInitComponent()
    {
        usingProxy("WorkingMemory");
        usingProxy("LongtermMemory");
        usingProxy("CommonPlacesLearner");
 
        testToRun = getProperty<std::string>("TestToRun").getValue();
    }
 
    void
    CommonPlacesTester::onConnectComponent()
    {
        std::cout << "Starting CommonPlacesTester" << std::endl;
 
        const Ice::CommunicatorPtr ic = getIceManager()->getCommunicator();
 
 
        memoryPrx = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
        longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>("LongtermMemory");
        cpLearnerPrx = getProxy<CommonPlacesLearnerInterfacePrx>("CommonPlacesLearner");
 
        ltmSegmentName = getProperty<std::string>("LTMSegment").getValue();
        cpLearnerPrx->setLTMSegmentName(ltmSegmentName);
 
        objectToLoad = getProperty<std::string>("ObjectToLoad").getValue();
 
        // load snapshot, if specified
        const std::string snapshotName = getProperty<std::string>("SnapshotToLoad").getValue();
 
        if (!snapshotName.empty())
        {
            longtermMemoryPrx->loadWorkingMemorySnapshot(snapshotName, memoryPrx);
            ARMARX_INFO << "Snapshot succcessfully loaded: " << snapshotName;
        }
 
        if (testToRun == "LearnFromSnapshot")
        {
            testLearnFromSnapshot();
        }
        else if (testToRun == "LearnFromSingleSnapshot")
        {
            testLearnFromSingleSnapshot();
        }
        else if (testToRun == "LoadLTM")
        {
            testLoadLTM();
        }
        else if (testToRun == "Clustering")
        {
            testClustering();
        }
        else if (testToRun == "Merging")
        {
            testMerging();
        }
        else if (testToRun == "Aging")
        {
            testAging();
        }
        else if (testToRun == "Aging2")
        {
            testAging2();
        }
        else if (testToRun == "ClusteringBatch")
        {
            testClusteringBatch();
        }
        else if (testToRun == "GetClusterLocations")
        {
            testGetClusterLocations();
        }
 
        else
        {
            std::cout << "Unknown test name: " << testToRun << std::endl;
        }
    }
 
    void
    CommonPlacesTester::testLearnFromSnapshot()
    {
        const std::string snapshotName =
            getProperty<std::string>("SnapshotsToLearnFrom").getValue();
        const bool loadToWM = getProperty<bool>("LoadToWM").getValue();
        const int initDelay = getProperty<int>("InitialDelay").getValue();
        const int delay = getProperty<int>("DelayBetweenSnapshots").getValue();
 
        if (!snapshotName.empty())
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(initDelay));
 
            NameList snapNameList = armarx::Split(snapshotName, ",");
 
            for (NameList::const_iterator it = snapNameList.begin(); it != snapNameList.end(); ++it)
            {
                cpLearnerPrx->learnFromSnapshot(*it);
                ARMARX_INFO << "Snapshot succcessfully processed by CommonPlacesLearner: " << *it;
 
                if (loadToWM)
                {
                    testLoadLTM();
                }
 
                std::this_thread::sleep_for(std::chrono::milliseconds(delay));
            }
        }
        else
        {
            ARMARX_ERROR << "SnapshotsToLearnFrom parameter must be specified! ";
        }
    }
 
    void
    CommonPlacesTester::testLearnFromSingleSnapshot()
    {
        const std::string snapshotName =
            getProperty<std::string>("SnapshotsToLearnFrom").getValue();
        const bool loadToWM = getProperty<bool>("LoadToWM").getValue();
        const int initDelay = getProperty<int>("InitialDelay").getValue();
        const int delay = getProperty<int>("DelayBetweenSnapshots").getValue();
 
        std::this_thread::sleep_for(std::chrono::milliseconds(initDelay));
 
        WorkingMemorySnapshotInterfacePrx snapshot =
            longtermMemoryPrx->openWorkingMemorySnapshot(snapshotName);
 
        if (!snapshot)
        {
            ARMARX_ERROR << "Snapshot not found in LTM: " << snapshotName;
            return;
        }
 
        const std::string segmentName = "objectInstances";
        PersistentEntitySegmentBasePrx segInstances = snapshot->getSegment(segmentName);
 
        if (!segInstances)
        {
            ARMARX_ERROR << "Segment not found in snapshot: " << segmentName;
            return;
        }
 
        EntityIdList ids = segInstances->getAllEntityIds();
 
        // sort ids
        struct EntityIdComparator
        {
            static bool
            compare(const std::string& i, const std::string& j)
            {
                return (std::stoi(i) < std::stoi(j));
            }
        };
 
        std::sort(ids.begin(), ids.end(), EntityIdComparator::compare);
 
        for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
        {
            EntityBasePtr snapEntity = segInstances->getEntityById(*it);
            ObjectInstancePtr snapInstance = ObjectInstancePtr::dynamicCast(snapEntity);
            cpLearnerPrx->learnFromObject(snapInstance);
            ARMARX_INFO << "Object processed by CommonPlacesLearner: " << *it;
 
            if (loadToWM)
            //           testLoadLTM();
            {
                testClustering();
            }
 
 
            std::this_thread::sleep_for(std::chrono::milliseconds(delay));
        }
    }
 
    void
    CommonPlacesTester::testLoadLTM()
    {
        ObjectInstanceMemorySegmentBasePrx wmObjectsSeg = memoryPrx->getObjectInstancesSegment();
        PersistentEntitySegmentBasePrx ltmObjectsSeg =
            longtermMemoryPrx->getCustomInstancesSegment(ltmSegmentName, false);
 
        if (ltmObjectsSeg)
        {
            if (!objectToLoad.empty())
            {
                EntityBasePtr obj = ltmObjectsSeg->getEntityByName(objectToLoad);
                EntityBasePtr oldWMObj = wmObjectsSeg->getEntityByName(objectToLoad);
 
                if (obj)
                {
                    if (oldWMObj)
                    {
                        wmObjectsSeg->updateEntity(oldWMObj->getId(), obj);
                    }
                    else
                    {
                        wmObjectsSeg->addEntity(obj);
                    }
 
                    std::cout << "Loaded from LTM: " << ObjectInstancePtr::dynamicCast(obj)
                              << std::endl;
                }
                else
                {
                    ARMARX_ERROR << "Entity not found in LTM: " << objectToLoad;
                }
            }
            else
            {
                EntityIdList ids = ltmObjectsSeg->getAllEntityIds();
 
                for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
                {
                    wmObjectsSeg->addEntity(ltmObjectsSeg->getEntityById(*it));
                }
            }
        }
        else
        {
            ARMARX_ERROR << "LTM segment not found: " << ltmSegmentName;
        }
    }
 
    void
    CommonPlacesTester::testClustering()
    {
        const bool loadToWM = getProperty<bool>("LoadToWM").getValue();
 
        if (loadToWM)
        {
            testLoadLTM();
        }
 
        float maxDeviation = getProperty<float>("ClusterMaxDeviation").getValue();
        std::string deviationTypeStr = getProperty<std::string>("ClusterDeviationType").getValue();
        DeviationType devType = getDeviationTypeFromString(deviationTypeStr);
 
        //    Cluster3DList posClusters = cpLearnerPrx->getPositionClustersByMaxDeviation(objectToLoad, maxDeviation, devType);
        //    std::cout << "Reduced to clusters: " << std::endl;
        //    int i = 1;
        //    for (Cluster3DList::const_iterator it = posClusters.begin(); it != posClusters.end(); ++it)
        //    {
        //        std::cout << "Cluster " << i <<
        //                ": center = (" << it->center.x << ", " << it->center.y << ", " << it->center.z <<
        //                ")  weight = "  << it->weight << std::endl;
        //        ++i;
        //    }
 
        ObjectInstanceMemorySegmentBasePrx wmObjectsSeg = memoryPrx->getObjectInstancesSegment();
 
        mstimer.time_start();
        GaussianMixtureDistributionPtr posDistr = GaussianMixtureDistributionPtr::dynamicCast(
            cpLearnerPrx->getPositionReducedByMaxDeviation(objectToLoad, maxDeviation, devType));
        ARMARX_INFO << "Elapsed time for reduction: " << mstimer.time_stop() << " ms" << std::endl;
 
        if (posDistr)
        {
            ARMARX_INFO << "Reduced to: " << posDistr->output() << std::endl;
 
            EntityBasePtr oldWMObj = wmObjectsSeg->getEntityByName(objectToLoad + "GaussPos");
 
            if (oldWMObj)
            {
                wmObjectsSeg->removeEntity(oldWMObj->getId());
            }
 
            ObjectInstancePtr obj = new ObjectInstance(objectToLoad + "GaussPos");
            obj->setClass(objectToLoad, 1.f);
            NormalDistributionPtr mode =
                NormalDistributionPtr::dynamicCast(posDistr->getModalComponent().gaussian);
            armarx::FramedPositionPtr curPos = new armarx::FramedPosition(
                (Eigen::Vector3f)mode->toEigenMean(), armarx::GlobalFrame, "");
            obj->setPosition(curPos);
            obj->getPositionAttribute()->setValueWithUncertainty(
                obj->getPositionAttribute()->getValue(), posDistr);
 
            Eigen::Quaternionf quat(0., 0., 0., 1.);
            armarx::FramedOrientationBasePtr orient =
                new armarx::FramedOrientation(quat.toRotationMatrix(), armarx::GlobalFrame, "");
            obj->setOrientation(orient);
 
            obj->putAttribute("UncertaintyColorMap", "eHot");
            obj->putAttribute("UncertaintyTransparency", 0.7f);
 
            wmObjectsSeg->addEntity(obj);
        }
        else
        {
            ARMARX_INFO << "Unable to get object pos gaussian! " << std::endl;
        }
    }
 
    void
    CommonPlacesTester::testMerging()
    {
        const std::string snapshotName =
            getProperty<std::string>("SnapshotsToLearnFrom").getValue();
 
        //const int sampleSize = getProperty<int>("SampleSize").getValue();
        const int itCount = getProperty<int>("IterationCount").getValue();
 
        const int THRESHOLD_COUNT = 11;
        const float thresholds[THRESHOLD_COUNT] = {0., 1., 2., 3., 4., 5., 6., 7., 8., 9., 10.};
        float grandTotalDensity[THRESHOLD_COUNT];
        float grandTotalKL[THRESHOLD_COUNT];
        float grandTotalISD[THRESHOLD_COUNT];
        float grandReductionRate[THRESHOLD_COUNT];
 
        for (int i = 0; i < THRESHOLD_COUNT; ++i)
        {
            grandTotalDensity[i] = 0;
            grandTotalKL[i] = 0;
            grandTotalISD[i] = 0;
            grandReductionRate[i] = 0;
        }
 
        //    RunnallsKLDistance klDistance;
        ISDDistance isdDistance;
 
        NameList snapNameList = armarx::Split(snapshotName, ",");
 
        for (NameList::const_iterator it = snapNameList.begin(); it != snapNameList.end(); ++it)
        {
            WorkingMemorySnapshotInterfacePrx snapshot =
                longtermMemoryPrx->openWorkingMemorySnapshot(*it);
 
            if (!snapshot)
            {
                ARMARX_ERROR << "Snapshot not found in LTM: " << snapshotName;
                return;
            }
 
            const std::string segmentName = "objectInstances";
            PersistentEntitySegmentBasePrx segInstances = snapshot->getSegment(segmentName);
 
            if (!segInstances)
            {
                ARMARX_ERROR << "Segment not found in snapshot: " << segmentName;
                return;
            }
 
            EntityIdList ids = segInstances->getAllEntityIds();
 
            std::ofstream snfile;
            const std::string fname = "m_" + (*it) + ".csv";
            snfile.open(fname.c_str());
 
            for (int t = 0; t < THRESHOLD_COUNT; ++t)
            {
                cpLearnerPrx->setMergingThreshold(thresholds[t]);
 
                float totalDensity = 0., totalKL = 0., totalISD = 0.;
                int origComp = 0, mergedComp = 0;
 
                for (int i = 0; i < itCount; ++i)
                {
                    //        EntityIdList ids;
                    //        getRandomIds(allIds, ids, sampleSize);
 
                    std::random_shuffle(ids.begin(), ids.end());
 
                    longtermMemoryPrx->getCustomInstancesSegment(ltmSegmentName, false)->clear();
 
                    for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
                    {
                        EntityBasePtr snapEntity = segInstances->getEntityById(*it);
                        ObjectInstancePtr snapInstance = ObjectInstancePtr::dynamicCast(snapEntity);
                        GaussianMixtureDistributionPtr gmm =
                            GaussianMixtureDistributionPtr::dynamicCast(
                                cpLearnerPrx->getPositionFull(
                                    snapInstance->getMostProbableClass()));
 
                        if (gmm)
                        {
                            totalDensity += gmm->getDensity(snapInstance->getPosition()->toEigen());
                            //                        totalKL += klDistance.getDistance(gmm, GaussianMixtureDistribution::FromProbabilityMeasure(snapInstance->getPositionUncertainty()));
                            //                        totalISD += isdDistance.getDistance(gmm, GaussianMixtureDistribution::FromProbabilityMeasure(snapInstance->getPositionUncertainty()), true);
                        }
 
                        cpLearnerPrx->learnFromObject(snapInstance);
                        //        ARMARX_INFO << "Object processed by CommonPlacesLearner: " << *it;
                    }
 
                    GaussianMixtureDistributionPtr mergedGMM =
                        GaussianMixtureDistributionPtr::dynamicCast(
                            cpLearnerPrx->getPositionFull(objectToLoad));
 
                    // learn w/o merging
                    cpLearnerPrx->setLTMSegmentName(ltmSegmentName + "2");
                    longtermMemoryPrx->getCustomInstancesSegment(ltmSegmentName + "2", false)
                        ->clear();
                    cpLearnerPrx->setMergingThreshold(0.);
 
                    for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
                    {
                        EntityBasePtr snapEntity = segInstances->getEntityById(*it);
                        ObjectInstancePtr snapInstance = ObjectInstancePtr::dynamicCast(snapEntity);
                        cpLearnerPrx->learnFromObject(snapInstance);
                    }
 
                    GaussianMixtureDistributionPtr origGMM =
                        GaussianMixtureDistributionPtr::dynamicCast(
                            cpLearnerPrx->getPositionFull(objectToLoad));
                    origComp += origGMM->size();
                    mergedComp += mergedGMM->size();
                    totalISD += isdDistance.getDistance(mergedGMM, origGMM, true);
                    cpLearnerPrx->setLTMSegmentName(ltmSegmentName);
                }
 
                totalISD /= itCount;
                const float redRate = float(mergedComp) / float(origComp);
 
                // write snap file
                snfile << thresholds[t] << "," << totalDensity << "," << totalISD << ","
                       << float(origComp) / itCount << "," << float(mergedComp) / itCount << ","
                       << float(mergedComp) / float(origComp) << std::endl;
 
                grandTotalDensity[t] += totalDensity;
                grandTotalISD[t] += totalISD;
                grandTotalKL[t] += totalKL;
                grandReductionRate[t] += redRate;
                //            ARMARX_INFO << "Total density: " <<  totalDensity / itCount;
            }
 
            snfile.close();
        } // end snapshots
 
        // write total file
        std::ofstream totfile;
        totfile.open("m_totals.csv");
 
        for (int t = 0; t < THRESHOLD_COUNT; ++t)
            totfile << thresholds[t] << "," << (grandTotalDensity[t] / snapNameList.size()) << ","
                    << (grandTotalISD[t] / snapNameList.size()) << ","
                    << (grandReductionRate[t] / snapNameList.size()) << std::endl;
 
        totfile.close();
    }
 
    void
    CommonPlacesTester::testAging()
    {
        const std::string snapshotName =
            getProperty<std::string>("SnapshotsToLearnFrom").getValue();
 
        const int itCount = getProperty<int>("IterationCount").getValue();
 
        const int COEFF_COUNT = 10;
        const float thresholds[COEFF_COUNT] = {1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1};
        float grandTotalMah[COEFF_COUNT];
        int grandTotalCorrect[COEFF_COUNT];
        int totalObjects = 0;
 
        for (int i = 0; i < COEFF_COUNT; ++i)
        {
            grandTotalMah[i] = 0;
            grandTotalCorrect[i] = 0;
        }
 
        //    RunnallsKLDistance klDistance;
        ISDDistance isdDistance;
        MahalanobisDistance mahDistance;
 
        NameList snapNameList = armarx::Split(snapshotName, ",");
 
        for (NameList::const_iterator it = snapNameList.begin(); it != snapNameList.end(); ++it)
        {
            WorkingMemorySnapshotInterfacePrx snapshot =
                longtermMemoryPrx->openWorkingMemorySnapshot(*it);
 
            if (!snapshot)
            {
                ARMARX_ERROR << "Snapshot not found in LTM: " << *it;
                return;
            }
 
            const std::string segmentName = "objectInstances";
            PersistentEntitySegmentBasePrx segInstances = snapshot->getSegment(segmentName);
 
            if (!segInstances)
            {
                ARMARX_ERROR << "Segment not found in snapshot: " << segmentName;
                return;
            }
 
            ARMARX_INFO << "Processing snapshot: " << *it;
 
            EntityIdList ids = segInstances->getAllEntityIds();
 
            totalObjects += ids.size();
 
            for (int t = 0; t < COEFF_COUNT; ++t)
            {
                cpLearnerPrx->setAgingFactor(thresholds[t]);
 
                float totalMah = 0.; //, totalKL = 0., totalISD = 0.;
                int corrClusters = 0;
 
                for (int i = 0; i < itCount; ++i)
                {
                    //        EntityIdList ids;
                    //        getRandomIds(allIds, ids, sampleSize);
 
                    std::random_shuffle(ids.begin(), ids.end());
 
                    longtermMemoryPrx->getCustomInstancesSegment(ltmSegmentName, false)->clear();
 
                    for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
                    {
                        EntityBasePtr snapEntity = segInstances->getEntityById(*it);
                        ObjectInstancePtr snapInstance = ObjectInstancePtr::dynamicCast(snapEntity);
                        GaussianMixtureDistributionPtr gmm =
                            GaussianMixtureDistributionPtr::dynamicCast(
                                cpLearnerPrx->getPositionReducedByMaxDeviation(
                                    objectToLoad, 150, eDevAABB));
 
                        if (gmm)
                        {
                            totalMah += mahDistance.getWeigtedDistance(
                                gmm, snapInstance->getPositionUncertainty());
 
                            // discrete criterion: cluster is "correct", if observation mean within 3 sigma from cluster mean
                            GaussianMixtureComponent c = gmm->getComponent(0);
                            NormalDistributionPtr g =
                                NormalDistributionPtr::dynamicCast(c.gaussian);
                            const Eigen::VectorXf obsMean = snapInstance->getPosition()->toEigen();
                            Eigen::Vector3f sigma;
                            sigma << sqrtf(g->getCovariance(0, 0)), sqrtf(g->getCovariance(1, 1)),
                                sqrtf(g->getCovariance(2, 2));
                            const Eigen::VectorXf triSigmaPoint = g->toEigenMean() + 3 * sigma;
 
                            if (g->getDensity(obsMean) > g->getDensity(triSigmaPoint))
                            {
                                corrClusters++;
                            }
                        }
 
                        cpLearnerPrx->learnFromObject(snapInstance);
                        //        ARMARX_INFO << "Object processed by CommonPlacesLearner: " << *it;
                    }
                }
 
                totalMah /= itCount;
                const float avgCorrect = float(corrClusters) / itCount;
 
                grandTotalMah[t] += totalMah;
                grandTotalCorrect[t] += avgCorrect;
                //            ARMARX_INFO << "Total density: " <<  totalDensity / itCount;
            }
 
        } // end snapshots
 
        // write total file
        std::ofstream totfile;
        totfile.open("a_totals.csv");
 
        for (int t = 0; t < COEFF_COUNT; ++t)
            totfile << thresholds[t] << "," << grandTotalMah[t] << // total mah dist
                "," << grandTotalMah[t] / totalObjects << // avg mah dist/obj
                "," << grandTotalCorrect[t] << // total correct clusters
                "," << float(grandTotalCorrect[t]) / totalObjects << // correct cluster rate
                std::endl;
 
        totfile.close();
    }
 
    void
    CommonPlacesTester::testAging2()
    {
        const std::string snapshotName =
            getProperty<std::string>("SnapshotsToLearnFrom").getValue();
 
        const int itCount = getProperty<int>("IterationCount").getValue();
 
        const int COEFF_COUNT = 10;
        const float thresholds[COEFF_COUNT] = {1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1};
        float grandTotalMah[COEFF_COUNT];
        int grandTotalCorrect[COEFF_COUNT];
        int totalObjects = 0;
 
        for (int i = 0; i < COEFF_COUNT; ++i)
        {
            grandTotalMah[i] = 0;
            grandTotalCorrect[i] = 0;
        }
 
        MahalanobisDistance mahDistance;
 
        NameList snapNameList = armarx::Split(snapshotName, ",");
 
        for (int t = 0; t < COEFF_COUNT; ++t)
        {
            cpLearnerPrx->setAgingFactor(thresholds[t]);
 
            float totalMah = 0.;
            int corrClusters = 0;
 
            for (int i = 0; i < itCount; ++i)
            {
                longtermMemoryPrx->getCustomInstancesSegment(ltmSegmentName, false)->clear();
 
                for (NameList::const_iterator it = snapNameList.begin(); it != snapNameList.end();
                     ++it)
                {
                    WorkingMemorySnapshotInterfacePrx snapshot =
                        longtermMemoryPrx->openWorkingMemorySnapshot(*it);
 
                    if (!snapshot)
                    {
                        ARMARX_ERROR << "Snapshot not found in LTM: " << *it;
                        return;
                    }
 
                    const std::string segmentName = "objectInstances";
                    PersistentEntitySegmentBasePrx segInstances = snapshot->getSegment(segmentName);
 
                    if (!segInstances)
                    {
                        ARMARX_ERROR << "Segment not found in snapshot: " << segmentName;
                        return;
                    }
 
                    ARMARX_INFO << "Processing snapshot: " << *it;
 
                    EntityIdList ids = segInstances->getAllEntityIds();
 
                    if (t == 0 && i == 0)
                    {
                        totalObjects += ids.size();
                    }
 
                    std::random_shuffle(ids.begin(), ids.end());
 
                    for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
                    {
                        EntityBasePtr snapEntity = segInstances->getEntityById(*it);
                        ObjectInstancePtr snapInstance = ObjectInstancePtr::dynamicCast(snapEntity);
                        GaussianMixtureDistributionPtr gmm =
                            GaussianMixtureDistributionPtr::dynamicCast(
                                cpLearnerPrx->getPositionReducedByMaxDeviation(
                                    objectToLoad, 150, eDevAABB));
 
                        if (gmm)
                        {
                            totalMah += mahDistance.getWeigtedDistance(
                                gmm, snapInstance->getPositionUncertainty());
 
                            // discrete criterion: cluster is "correct", if observation mean within 3 sigma from cluster mean
                            GaussianMixtureComponent c = gmm->getModalComponent();
                            NormalDistributionPtr g =
                                NormalDistributionPtr::dynamicCast(c.gaussian);
                            const Eigen::VectorXf obsMean = snapInstance->getPosition()->toEigen();
                            Eigen::Vector3f sigma;
                            sigma << sqrtf(g->getCovariance(0, 0)), sqrtf(g->getCovariance(1, 1)),
                                sqrtf(g->getCovariance(2, 2));
                            const Eigen::VectorXf triSigmaPoint = g->toEigenMean() + 3 * sigma;
 
                            if (g->getDensity(obsMean) > g->getDensity(triSigmaPoint))
                            {
                                corrClusters++;
                            }
                        }
 
                        cpLearnerPrx->learnFromObject(snapInstance);
                        //        ARMARX_INFO << "Object processed by CommonPlacesLearner: " << *it;
                    }
                } // end snapshots
 
                //            ARMARX_INFO << "Total density: " <<  totalDensity / itCount;
 
            } // end iterations
 
            totalMah /= itCount;
            const float avgCorrect = float(corrClusters) / itCount;
 
            grandTotalMah[t] += totalMah;
            grandTotalCorrect[t] += avgCorrect;
 
        } // end coefficients
 
        // write total file
        std::ofstream totfile;
        totfile.open("a_totals.csv");
 
        for (int t = 0; t < COEFF_COUNT; ++t)
            totfile << thresholds[t] << "," << grandTotalMah[t] << // total mah dist
                "," << grandTotalMah[t] / totalObjects << // avg mah dist/obj
                "," << grandTotalCorrect[t] << // total correct clusters
                "," << float(grandTotalCorrect[t]) / totalObjects << // correct cluster rate
                std::endl;
 
        totfile.close();
    }
 
    void
    CommonPlacesTester::testGetClusterLocations()
    {
        Cluster3DList clusterList =
            cpLearnerPrx->getPositionClustersByComponentCount(objectToLoad, 3);
 
        if (clusterList.size() > 0)
        {
            for (Cluster3DList::iterator it = clusterList.begin(); it != clusterList.end(); it++)
            {
                Cluster3D& entry = *it;
                ARMARX_IMPORTANT << "object center: x:" << entry.center.x
                                 << " y: " << entry.center.y << " with weight: " << entry.weight;
            }
        }
        else
        {
            ARMARX_WARNING << "no cluster found for object " << objectToLoad;
        }
    }
 
    void
    CommonPlacesTester::testClusteringBatch()
    {
        const std::string snapshotName =
            getProperty<std::string>("SnapshotsToLearnFrom").getValue();
 
        const int DEV_COUNT = 11;
        const float thresholds[DEV_COUNT] = {
            10., 30., 50., 75., 100., 150., 200., 400., 800., 1000., 2000.};
 
        //    const int DEV_COUNT = 1;
        //    const float thresholds[DEV_COUNT] = {150.};
 
        std::ofstream snfile;
        const std::string fname = "c_" + ltmSegmentName + "_" + objectToLoad + ".csv";
        snfile.open(fname.c_str());
 
        for (int t = 0; t < DEV_COUNT; ++t)
        {
            ARMARX_INFO << "Processing dev: " << thresholds[t];
 
            GaussianMixtureDistributionBasePtr gmmAABB =
                cpLearnerPrx->getPositionReducedByMaxDeviation(
                    objectToLoad, thresholds[t], eDevAABB);
            GaussianMixtureDistributionBasePtr gmmOBB =
                cpLearnerPrx->getPositionReducedByMaxDeviation(
                    objectToLoad, thresholds[t], eDevOrientedBBox);
            GaussianMixtureDistributionBasePtr gmmSphere =
                cpLearnerPrx->getPositionReducedByMaxDeviation(
                    objectToLoad, thresholds[t], eDevEqualSphere);
 
            // write snap file
            snfile << thresholds[t] << "," << gmmAABB->size() << "," << gmmOBB->size() << ","
                   << gmmSphere->size() << "," << std::endl;
 
            //            ARMARX_INFO << "Total density: " <<  totalDensity / itCount;
        }
 
        snfile.close();
        ARMARX_INFO << "Finished: ";
    }
 
    void
    CommonPlacesTester::getRandomIds(std::vector<std::string>& allIds,
                                     std::vector<std::string>& ids,
                                     int sampleSize)
    {
        if (sampleSize > (int)(allIds.size()))
        {
            ids.assign(allIds.begin(), allIds.end());
            return;
        }
 
        std::set<std::string> idset;
 
        while (idset.size() < (size_t)sampleSize)
        {
            const size_t ind =
                (size_t)truncf(((float)rand()) / float(RAND_MAX) * float(allIds.size()));
            std::cout << "index to add" << ind << std::flush;
            idset.insert(allIds[ind]);
        }
 
        ids.assign(idset.begin(), idset.end());
    }
 
    DeviationType
    CommonPlacesTester::getDeviationTypeFromString(const std::string& devTypeStr)
    {
 
        if (devTypeStr == "AABB")
        {
            return eDevAABB;
        }
        else if (devTypeStr == "OrientedBBox")
        {
            return eDevOrientedBBox;
        }
        else if (devTypeStr == "EqualSphere")
        {
            return eDevEqualSphere;
        }
        else
        {
            return eDevAABB;
        }
    }
} // namespace memoryx