CMultiMetricMapPDF.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "slam-precomp.h"  // Precompiled headers

#include <mrpt/random.h>
#include <mrpt/utils/CTicTac.h>
#include <mrpt/utils/CFileStream.h>
#include <mrpt/system/os.h>

#include <mrpt/maps/CMultiMetricMapPDF.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFGrid.h>
#include <mrpt/obs/CObservationBeaconRanges.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/maps/CLandmarksMap.h>

#include <mrpt/slam/PF_aux_structs.h>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::slam;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::poses;
using namespace mrpt::random;
using namespace mrpt::utils;
using namespace mrpt::system;
using namespace std;

IMPLEMENTS_SERIALIZABLE(CMultiMetricMapPDF, CSerializable, mrpt::maps)
IMPLEMENTS_SERIALIZABLE(CRBPFParticleData, CSerializable, mrpt::maps)

/*---------------------------------------------------------------
                                Constructor
  ---------------------------------------------------------------*/
CMultiMetricMapPDF::CMultiMetricMapPDF(
        const bayes::CParticleFilter::TParticleFilterOptions& opts,
        const mrpt::maps::TSetOfMetricMapInitializers* mapsInitializers,
        const TPredictionParams* predictionOptions)
        : averageMap(mapsInitializers),
          averageMapIsUpdated(false),
          SFs(),
          SF2robotPath(),
          options(),
          newInfoIndex(0)
{
        m_poseParticles.m_particles.resize(opts.sampleSize);
        for (CParticleList::iterator it = m_poseParticles.m_particles.begin();
                 it != m_poseParticles.m_particles.end(); ++it)
        {
                it->log_w = 0;
                it->d.reset(new CRBPFParticleData(mapsInitializers));
        }

        // Initialize:
        const CPose3D nullPose(0, 0, 0);
        clear(nullPose);

        // If provided, copy the whole set of params now:
        if (predictionOptions != nullptr) options = *predictionOptions;
}

/*---------------------------------------------------------------
                                                clear
  ---------------------------------------------------------------*/
void CMultiMetricMapPDF::clear(const CPose2D& initialPose)
{
        CPose3D p(initialPose);
        clear(p);
}

/*---------------------------------------------------------------
                                                clear
  ---------------------------------------------------------------*/
void CMultiMetricMapPDF::clear(const CPose3D& initialPose)
{
        const size_t M = m_poseParticles.m_particles.size();
        for (size_t i = 0; i < M; i++)
        {
                m_poseParticles.m_particles[i].log_w = 0;

                m_poseParticles.m_particles[i].d->mapTillNow.clear();

                m_poseParticles.m_particles[i].d->robotPath.resize(1);
                m_poseParticles.m_particles[i].d->robotPath[0] = initialPose;
        }

        SFs.clear();
        SF2robotPath.clear();

        averageMapIsUpdated = false;
}

void CMultiMetricMapPDF::clear(
        const mrpt::maps::CSimpleMap& prevMap,
        const mrpt::poses::CPose3D& currentPose)
{
        const size_t nParts = m_poseParticles.m_particles.size(), nOldKeyframes = prevMap.size();
        if (nOldKeyframes == 0)
        {
                // prevMap is empty, so reset the map
                clear(currentPose);
                return;
        }
        for (size_t idxPart = 0; idxPart < nParts; idxPart++)
        {
                auto& p = m_poseParticles.m_particles[idxPart];
                p.log_w = 0;

                p.d->mapTillNow.clear();

                p.d->robotPath.resize(nOldKeyframes);
                for (size_t i = 0; i < nOldKeyframes; i++)
                {
                        CPose3DPDF::Ptr keyframe_pose;
                        CSensoryFrame::Ptr sfkeyframe_sf;
                        prevMap.get(i, keyframe_pose, sfkeyframe_sf);

                        // as pose, use: if the PDF is also a PF with the same number of
                        // samples, use those particles;
                        // otherwise, simply use the mean for all particles as an
                        // approximation (with loss of uncertainty).
                        mrpt::poses::CPose3D kf_pose;
                        bool kf_pose_set = false;
                        if (IS_CLASS(keyframe_pose, CPose3DPDFParticles))
                        {
                                const auto pdf_parts = dynamic_cast<const CPose3DPDFParticles*>(
                                        keyframe_pose.get());
                                ASSERT_(pdf_parts);
                                if (pdf_parts->particlesCount() == nParts)
                                {
                                        kf_pose = *pdf_parts->m_particles[idxPart].d;
                                        kf_pose_set = true;
                                }
                        }
                        if (!kf_pose_set)
                        {
                                kf_pose = keyframe_pose->getMeanVal();
                        }
                        p.d->robotPath[i] = kf_pose;
                        for (const auto& obs : *sfkeyframe_sf)
                        {
                                p.d->mapTillNow.insertObservation(&(*obs), &kf_pose);
                        }
                }
        }

        SFs = prevMap;  // copy
        SF2robotPath.clear();
        SF2robotPath.reserve(nOldKeyframes);
        for (size_t i = 0; i < nOldKeyframes; i++) SF2robotPath.push_back(i);

        averageMapIsUpdated = false;
}

/*---------------------------------------------------------------
                                                getEstimatedPosePDF
  Returns an estimate of the pose, (the mean, or mathematical expectation of the
 PDF), computed
                as a weighted average over all m_poseParticles.m_particles.
 ---------------------------------------------------------------*/
void CMultiMetricMapPDF::getEstimatedPosePDF(
        CPose3DPDFParticles& out_estimation) const
{
        ASSERT_(m_poseParticles.m_particles[0].d->robotPath.size() > 0);
        getEstimatedPosePDFAtTime(
                m_poseParticles.m_particles[0].d->robotPath.size() - 1, out_estimation);
}

/*---------------------------------------------------------------
                                                getEstimatedPosePDFAtTime
 ---------------------------------------------------------------*/
void CMultiMetricMapPDF::getEstimatedPosePDFAtTime(
        size_t timeStep, CPose3DPDFParticles& out_estimation) const
{
        // CPose3D      p;
        size_t i, n = m_poseParticles.m_particles.size();

        // Delete current content of "out_estimation":
        out_estimation.clearParticles();

        // Create new m_poseParticles.m_particles:
        out_estimation.m_particles.resize(n);
        for (i = 0; i < n; i++)
        {
                out_estimation.m_particles[i].d.reset(
                        new CPose3D(m_poseParticles.m_particles[i].d->robotPath[timeStep]));
                out_estimation.m_particles[i].log_w = m_poseParticles.m_particles[i].log_w;
        }
}

/*---------------------------------------------------------------
                                                writeToStream
  ---------------------------------------------------------------*/
void CRBPFParticleData::writeToStream(
        mrpt::utils::CStream& out, int* version) const
{
        MRPT_UNUSED_PARAM(out);
        MRPT_UNUSED_PARAM(version);
        THROW_EXCEPTION("Shouldn't arrive here")
}

/*---------------------------------------------------------------
                                                readFromStream
  ---------------------------------------------------------------*/
void CRBPFParticleData::readFromStream(mrpt::utils::CStream& in, int version)
{
        MRPT_UNUSED_PARAM(in);
        MRPT_UNUSED_PARAM(version);
        THROW_EXCEPTION("Shouldn't arrive here")
}

/*---------------------------------------------------------------
                                                writeToStream
  ---------------------------------------------------------------*/
void CMultiMetricMapPDF::writeToStream(
        mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 0;
        else
        {
                uint32_t i, n, j, m;

                // The data
                n = static_cast<uint32_t>(m_poseParticles.m_particles.size());
                out << n;
                for (i = 0; i < n; i++)
                {
                        out << m_poseParticles.m_particles[i].log_w << m_poseParticles.m_particles[i].d->mapTillNow;
                        m = static_cast<uint32_t>(m_poseParticles.m_particles[i].d->robotPath.size());
                        out << m;
                        for (j = 0; j < m; j++) out << m_poseParticles.m_particles[i].d->robotPath[j];
                }
                out << SFs << SF2robotPath;
        }
}

/*---------------------------------------------------------------
                                                readFromStream
  ---------------------------------------------------------------*/
void CMultiMetricMapPDF::readFromStream(mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                {
                        uint32_t i, n, j, m;

                        // Delete current contents:
                        // --------------------------
                        m_poseParticles.clearParticles();
                        SFs.clear();
                        SF2robotPath.clear();

                        averageMapIsUpdated = false;

                        // Load the new data:
                        // --------------------
                        in >> n;

                        m_poseParticles.m_particles.resize(n);
                        for (i = 0; i < n; i++)
                        {
                                m_poseParticles.m_particles[i].d.reset(new CRBPFParticleData());

                                // Load
                                in >> m_poseParticles.m_particles[i].log_w >> m_poseParticles.m_particles[i].d->mapTillNow;

                                in >> m;
                                m_poseParticles.m_particles[i].d->robotPath.resize(m);
                                for (j = 0; j < m; j++) in >> m_poseParticles.m_particles[i].d->robotPath[j];
                        }

                        in >> SFs >> SF2robotPath;
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}

TPose3D CMultiMetricMapPDF::getLastPose(
        const size_t i, bool& is_valid_pose) const
{
        if (i >= m_poseParticles.m_particles.size())
                THROW_EXCEPTION("Particle index out of bounds!");

        if (m_poseParticles.m_particles[i].d->robotPath.empty())
        {
                is_valid_pose = false;
                return TPose3D(0, 0, 0, 0, 0, 0);
        }
        else
        {
                return *m_poseParticles.m_particles[i].d->robotPath.rbegin();
        }
}

const CMultiMetricMap* CMultiMetricMapPDF::getAveragedMetricMapEstimation()
{
        rebuildAverageMap();
        return &averageMap;
}

/*---------------------------------------------------------------
                                                getWeightedAveragedMap
 ---------------------------------------------------------------*/
void CMultiMetricMapPDF::rebuildAverageMap()
{
        float min_x = 1e6, max_x = -1e6, min_y = 1e6, max_y = -1e6;
        CParticleList::iterator part;

        if (averageMapIsUpdated) return;

        // ---------------------------------------------------------
        //                                      GRID
        // ---------------------------------------------------------
        for (part = m_poseParticles.m_particles.begin(); part != m_poseParticles.m_particles.end(); ++part)
        {
                ASSERT_(part->d->mapTillNow.m_gridMaps.size() > 0);

                min_x = min(min_x, part->d->mapTillNow.m_gridMaps[0]->getXMin());
                max_x = max(max_x, part->d->mapTillNow.m_gridMaps[0]->getXMax());
                min_y = min(min_y, part->d->mapTillNow.m_gridMaps[0]->getYMin());
                max_y = max(max_y, part->d->mapTillNow.m_gridMaps[0]->getYMax());
        }

        // Asure all maps have the same dimensions:
        for (part = m_poseParticles.m_particles.begin(); part != m_poseParticles.m_particles.end(); ++part)
                part->d->mapTillNow.m_gridMaps[0]->resizeGrid(
                        min_x, max_x, min_y, max_y, 0.5f, false);

        for (part = m_poseParticles.m_particles.begin(); part != m_poseParticles.m_particles.end(); ++part)
        {
                min_x = min(min_x, part->d->mapTillNow.m_gridMaps[0]->getXMin());
                max_x = max(max_x, part->d->mapTillNow.m_gridMaps[0]->getXMax());
                min_y = min(min_y, part->d->mapTillNow.m_gridMaps[0]->getYMin());
                max_y = max(max_y, part->d->mapTillNow.m_gridMaps[0]->getYMax());
        }

        // Prepare target map:
        ASSERT_(averageMap.m_gridMaps.size() > 0);
        averageMap.m_gridMaps[0]->setSize(
                min_x, max_x, min_y, max_y,
                m_poseParticles.m_particles[0].d->mapTillNow.m_gridMaps[0]->getResolution(), 0);

        // Compute the sum of weights:
        double sumLinearWeights = 0;
        for (part = m_poseParticles.m_particles.begin(); part != m_poseParticles.m_particles.end(); ++part)
                sumLinearWeights += exp(part->log_w);

        // CHECK:
        for (part = m_poseParticles.m_particles.begin(); part != m_poseParticles.m_particles.end(); ++part)
        {
                ASSERT_(
                        part->d->mapTillNow.m_gridMaps[0]->getSizeX() ==
                        averageMap.m_gridMaps[0]->getSizeX());
                ASSERT_(
                        part->d->mapTillNow.m_gridMaps[0]->getSizeY() ==
                        averageMap.m_gridMaps[0]->getSizeY());
        }

        {
                // ******************************************************
                //     Implementation WITHOUT the SSE Instructions Set
                // ******************************************************
                MRPT_START

                // Reserve a float grid-map, add weight all maps
                // -------------------------------------------------------------------------------------------
                std::vector<float> floatMap;
                floatMap.resize(averageMap.m_gridMaps[0]->map.size(), 0);

                // For each particle in the RBPF:
                double sumW = 0;
                for (part = m_poseParticles.m_particles.begin(); part != m_poseParticles.m_particles.end(); ++part)
                        sumW += exp(part->log_w);

                if (sumW == 0) sumW = 1;

                for (part = m_poseParticles.m_particles.begin(); part != m_poseParticles.m_particles.end(); ++part)
                {
                        // Variables:
                        std::vector<COccupancyGridMap2D::cellType>::iterator srcCell;
                        std::vector<COccupancyGridMap2D::cellType>::iterator firstSrcCell =
                                part->d->mapTillNow.m_gridMaps[0]->map.begin();
                        std::vector<COccupancyGridMap2D::cellType>::iterator lastSrcCell =
                                part->d->mapTillNow.m_gridMaps[0]->map.end();
                        std::vector<float>::iterator destCell;

                        // The weight of particle:
                        float w = exp(part->log_w) / sumW;

                        ASSERT_(
                                part->d->mapTillNow.m_gridMaps[0]->map.size() ==
                                floatMap.size());

                        // For each cell in individual maps:
                        for (srcCell = firstSrcCell, destCell = floatMap.begin();
                                 srcCell != lastSrcCell; srcCell++, destCell++)
                                (*destCell) += w * (*srcCell);
                }

                // Copy to fixed point map:
                std::vector<float>::iterator srcCell;
                std::vector<COccupancyGridMap2D::cellType>::iterator destCell =
                        averageMap.m_gridMaps[0]->map.begin();

                ASSERT_(averageMap.m_gridMaps[0]->map.size() == floatMap.size());

                for (srcCell = floatMap.begin(); srcCell != floatMap.end();
                         srcCell++, destCell++)
                        *destCell = static_cast<COccupancyGridMap2D::cellType>(*srcCell);

                MRPT_END
        }  // End of SSE not supported

        // Don't calculate again until really necesary.
        averageMapIsUpdated = true;
}

/*---------------------------------------------------------------
                                                insertObservation
 ---------------------------------------------------------------*/
bool CMultiMetricMapPDF::insertObservation(CSensoryFrame& sf)
{
        const size_t M = m_poseParticles.particlesCount();

        // Insert into SFs:
        CPose3DPDFParticles::Ptr posePDF =
                mrpt::make_aligned_shared<CPose3DPDFParticles>();
        getEstimatedPosePDF(*posePDF);

        // Insert it into the SFs and the SF2robotPath list:
        const uint32_t new_sf_id = SFs.size();
        SFs.insert(posePDF, CSensoryFrame::Ptr(new CSensoryFrame(sf)));
        SF2robotPath.resize(new_sf_id + 1);
        SF2robotPath[new_sf_id] = m_poseParticles.m_particles[0].d->robotPath.size() - 1;

        bool anymap = false;
        for (size_t i = 0; i < M; i++)
        {
                bool pose_is_valid;
                const CPose3D robotPose = CPose3D(getLastPose(i, pose_is_valid));
                // ASSERT_(pose_is_valid); // if not, use the default (0,0,0)
                const bool map_modified = sf.insertObservationsInto(
                        &m_poseParticles.m_particles[i].d->mapTillNow, &robotPose);
                anymap = anymap || map_modified;
        }

        averageMapIsUpdated = false;
        return anymap;
}

/*---------------------------------------------------------------
                                                getPath
 ---------------------------------------------------------------*/
void CMultiMetricMapPDF::getPath(
        size_t i, std::deque<math::TPose3D>& out_path) const
{
        if (i >= m_poseParticles.m_particles.size()) THROW_EXCEPTION("Index out of bounds");
        out_path = m_poseParticles.m_particles[i].d->robotPath;
}

/*---------------------------------------------------------------
                                        getCurrentEntropyOfPaths
  ---------------------------------------------------------------*/
double CMultiMetricMapPDF::getCurrentEntropyOfPaths()
{
        size_t i;
        size_t N =
                m_poseParticles.m_particles[0].d->robotPath.size();  // The poses count along the paths

        // Compute paths entropy:
        // ---------------------------
        double H_paths = 0;

        if (N)
        {
                // For each pose along the path:
                for (i = 0; i < N; i++)
                {
                        // Get pose est. as m_poseParticles.m_particles:
                        CPose3DPDFParticles posePDFParts;
                        getEstimatedPosePDFAtTime(i, posePDFParts);

                        // Approximate to gaussian and compute entropy of covariance:
                        H_paths += posePDFParts.getCovarianceEntropy();
                }
                H_paths /= N;
        }
        return H_paths;
}

/*---------------------------------------------------------------
                                        getCurrentJointEntropy
  ---------------------------------------------------------------*/
double CMultiMetricMapPDF::getCurrentJointEntropy()
{
        double H_joint, H_paths, H_maps;
        size_t i, M = m_poseParticles.m_particles.size();
        COccupancyGridMap2D::TEntropyInfo entropy;

        // Entropy of the paths:
        H_paths = getCurrentEntropyOfPaths();

        float min_x = 1e6, max_x = -1e6, min_y = 1e6, max_y = -1e6;
        CParticleList::iterator part;

        // ---------------------------------------------------------
        //                      ASSURE ALL THE GRIDS ARE THE SAME SIZE!
        // ---------------------------------------------------------
        for (part = m_poseParticles.m_particles.begin(); part != m_poseParticles.m_particles.end(); ++part)
        {
                ASSERT_(part->d->mapTillNow.m_gridMaps.size() > 0);

                min_x = min(min_x, part->d->mapTillNow.m_gridMaps[0]->getXMin());
                max_x = max(max_x, part->d->mapTillNow.m_gridMaps[0]->getXMax());
                min_y = min(min_y, part->d->mapTillNow.m_gridMaps[0]->getYMin());
                max_y = max(max_y, part->d->mapTillNow.m_gridMaps[0]->getYMax());
        }

        // Asure all maps have the same dimensions:
        for (part = m_poseParticles.m_particles.begin(); part != m_poseParticles.m_particles.end(); ++part)
                part->d->mapTillNow.m_gridMaps[0]->resizeGrid(
                        min_x, max_x, min_y, max_y, 0.5f, false);

        // Sum of linear weights:
        double sumLinearWeights = 0;
        for (i = 0; i < M; i++) sumLinearWeights += exp(m_poseParticles.m_particles[i].log_w);

        // Compute weighted maps entropy:
        // --------------------------------
        H_maps = 0;
        for (i = 0; i < M; i++)
        {
                ASSERT_(m_poseParticles.m_particles[i].d->mapTillNow.m_gridMaps.size() > 0);

                m_poseParticles.m_particles[i].d->mapTillNow.m_gridMaps[0]->computeEntropy(entropy);
                H_maps += exp(m_poseParticles.m_particles[i].log_w) * entropy.H / sumLinearWeights;
        }

        printf("H_paths=%e\n", H_paths);
        printf("H_maps=%e\n", H_maps);

        H_joint = H_paths + H_maps;
        return H_joint;
}

const CMultiMetricMap* CMultiMetricMapPDF::getCurrentMostLikelyMetricMap() const
{
        size_t i, max_i = 0, n = m_poseParticles.m_particles.size();
        double max_w = m_poseParticles.m_particles[0].log_w;

        for (i = 0; i < n; i++)
        {
                if (m_poseParticles.m_particles[i].log_w > max_w)
                {
                        max_w = m_poseParticles.m_particles[i].log_w;
                        max_i = i;
                }
        }

        // Return its map:
        return &m_poseParticles.m_particles[max_i].d->mapTillNow;
}

/*---------------------------------------------------------------
                                updateSensoryFrameSequence
  ---------------------------------------------------------------*/
void CMultiMetricMapPDF::updateSensoryFrameSequence()
{
        MRPT_START
        CPose3DPDFParticles posePartsPDF;
        CPose3DPDF::Ptr previousPosePDF;
        CSensoryFrame::Ptr dummy;

        for (size_t i = 0; i < SFs.size(); i++)
        {
                // Get last estimation:
                SFs.get(i, previousPosePDF, dummy);

                // Compute the new one:
                getEstimatedPosePDFAtTime(SF2robotPath[i], posePartsPDF);

                // Copy into SFs:
                previousPosePDF->copyFrom(posePartsPDF);
        }

        MRPT_END
}

/*---------------------------------------------------------------
                                saveCurrentPathEstimationToTextFile
  ---------------------------------------------------------------*/
void CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile(
        const std::string& fil)
{
        FILE* f = os::fopen(fil.c_str(), "wt");
        if (!f) return;

        for (CParticleList::iterator it = m_poseParticles.m_particles.begin();
                 it != m_poseParticles.m_particles.end(); ++it)
        {
                for (size_t i = 0; i < it->d->robotPath.size(); i++)
                {
                        const mrpt::math::TPose3D& p = it->d->robotPath[i];

                        os::fprintf(
                                f, "%.04f %.04f %.04f %.04f %.04f %.04f ", p.x, p.y, p.z, p.yaw,
                                p.pitch, p.roll);
                }
                os::fprintf(f, " %e\n", it->log_w);
        }

        os::fclose(f);
}

/*---------------------------------------------------------------
                                TPredictionParams
  ---------------------------------------------------------------*/
CMultiMetricMapPDF::TPredictionParams::TPredictionParams()
        : pfOptimalProposal_mapSelection(0),
          ICPGlobalAlign_MinQuality(0.70f),
          update_gridMapLikelihoodOptions(),
          KLD_params(),
          icp_params()
{
}

/*---------------------------------------------------------------
                                        dumpToTextStream
  ---------------------------------------------------------------*/
void CMultiMetricMapPDF::TPredictionParams::dumpToTextStream(
        mrpt::utils::CStream& out) const
{
        out.printf(
                "\n----------- [CMultiMetricMapPDF::TPredictionParams] ------------ "
                "\n\n");

        out.printf(
                "pfOptimalProposal_mapSelection          = %i\n",
                pfOptimalProposal_mapSelection);
        out.printf(
                "ICPGlobalAlign_MinQuality               = %f\n",
                ICPGlobalAlign_MinQuality);

        KLD_params.dumpToTextStream(out);
        icp_params.dumpToTextStream(out);
        out.printf("\n");
}

/*---------------------------------------------------------------
                                        loadFromConfigFile
  ---------------------------------------------------------------*/
void CMultiMetricMapPDF::TPredictionParams::loadFromConfigFile(
        const mrpt::utils::CConfigFileBase& iniFile, const std::string& section)
{
        pfOptimalProposal_mapSelection = iniFile.read_int(
                section, "pfOptimalProposal_mapSelection",
                pfOptimalProposal_mapSelection, true);

        MRPT_LOAD_CONFIG_VAR(ICPGlobalAlign_MinQuality, float, iniFile, section);

        KLD_params.loadFromConfigFile(iniFile, section);
        icp_params.loadFromConfigFile(iniFile, section);
}