CSimplePointsMap.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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 "maps-precomp.h"  // Precomp header
 
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/core/bits_mem.h>
 
#include "CPointsMap_crtp_common.h"
 
using namespace std;
using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace mrpt::math;
 
//  =========== Begin of Map definition ============
MAP_DEFINITION_REGISTER(
        "CSimplePointsMap,pointsMap", mrpt::maps::CSimplePointsMap)
 
CSimplePointsMap::TMapDefinition::TMapDefinition() {}
void CSimplePointsMap::TMapDefinition::loadFromConfigFile_map_specific(
        const mrpt::config::CConfigFileBase& c,
        const std::string& s)
{
        insertionOpts.loadFromConfigFile(c, s + string("_insertOpts"));
        likelihoodOpts.loadFromConfigFile(c, s + string("_likelihoodOpts"));
        renderOpts.loadFromConfigFile(c, s + string("_renderOpts"));
}
 
void CSimplePointsMap::TMapDefinition::dumpToTextStream_map_specific(
        std::ostream& out) const
{
        this->insertionOpts.dumpToTextStream(out);
        this->likelihoodOpts.dumpToTextStream(out);
        this->renderOpts.dumpToTextStream(out);
}
 
mrpt::maps::CMetricMap* CSimplePointsMap::internal_CreateFromMapDefinition(
        const mrpt::maps::TMetricMapInitializer& _def)
{
        const CSimplePointsMap::TMapDefinition& def =
                *dynamic_cast<const CSimplePointsMap::TMapDefinition*>(&_def);
        CSimplePointsMap* obj = new CSimplePointsMap();
        obj->insertionOptions = def.insertionOpts;
        obj->likelihoodOptions = def.likelihoodOpts;
        obj->renderOptions = def.renderOpts;
        return obj;
}
//  =========== End of Map definition Block =========
 
IMPLEMENTS_SERIALIZABLE(CSimplePointsMap, CPointsMap, mrpt::maps)
 
CSimplePointsMap::CSimplePointsMap() { }
void CSimplePointsMap::reserve(size_t newLength)
{
        m_x.reserve(newLength);
        m_y.reserve(newLength);
        m_z.reserve(newLength);
}
 
// Resizes all point buffers so they can hold the given number of points: newly
// created points are set to default values,
//  and old contents are not changed.
void CSimplePointsMap::resize(size_t newLength)
{
        this->reserve(newLength);  // to ensure 4N capacity
        m_x.resize(newLength, 0);
        m_y.resize(newLength, 0);
        m_z.resize(newLength, 0);
        mark_as_modified();
}
 
// Resizes all point buffers so they can hold the given number of points,
// *erasing* all previous contents
//  and leaving all points to default values.
void CSimplePointsMap::setSize(size_t newLength)
{
        this->reserve(newLength);  // to ensure 4N capacity
        m_x.assign(newLength, 0);
        m_y.assign(newLength, 0);
        m_z.assign(newLength, 0);
        mark_as_modified();
}
 
void CSimplePointsMap::copyFrom(const CPointsMap& obj)
{
        CPointsMap::base_copyFrom(
                obj);  // This also does a ::resize(N) of all data fields.
}
 
uint8_t CSimplePointsMap::serializeGetVersion() const { return 10; }
void CSimplePointsMap::serializeTo(mrpt::serialization::CArchive& out) const
{
        uint32_t n = m_x.size();
 
        // First, write the number of points:
        out << n;
 
        if (n > 0)
        {
                out.WriteBufferFixEndianness(&m_x[0], n);
                out.WriteBufferFixEndianness(&m_y[0], n);
                out.WriteBufferFixEndianness(&m_z[0], n);
        }
        out << genericMapParams;  // v9
        insertionOptions.writeToStream(out);  // v9
        likelihoodOptions.writeToStream(out);  // v5
        renderOptions.writeToStream(out);  // v10
}
 
/*---------------------------------------------------------------
                                        readFromStream
   Implements the reading from a CStream capability of
          CSerializable objects
  ---------------------------------------------------------------*/
void CSimplePointsMap::serializeFrom(
        mrpt::serialization::CArchive& in, uint8_t version)
{
        switch (version)
        {
                case 8:
                case 9:
                case 10:
                {
                        mark_as_modified();
 
                        // Read the number of points:
                        uint32_t n;
                        in >> n;
 
                        this->resize(n);
 
                        if (n > 0)
                        {
                                in.ReadBufferFixEndianness(&m_x[0], n);
                                in.ReadBufferFixEndianness(&m_y[0], n);
                                in.ReadBufferFixEndianness(&m_z[0], n);
                        }
                        if (version >= 9)
                                in >> genericMapParams;
                        else
                        {
                                bool disableSaveAs3DObject;
                                in >> disableSaveAs3DObject;
                                genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
                        }
                        insertionOptions.readFromStream(in);
                        likelihoodOptions.readFromStream(in);
                        if (version>=10) renderOptions.readFromStream(in);
                }
                break;
 
                case 0:
                case 1:
                case 2:
                case 3:
                case 4:
                case 5:
                case 6:
                case 7:
                {
                        mark_as_modified();
 
                        // Read the number of points:
                        uint32_t n;
                        in >> n;
 
                        this->resize(n);
 
                        if (n > 0)
                        {
                                in.ReadBufferFixEndianness(&m_x[0], n);
                                in.ReadBufferFixEndianness(&m_y[0], n);
                                in.ReadBufferFixEndianness(&m_z[0], n);
 
                                // Version 1: weights are also stored:
                                // Version 4: Type becomes long int -> uint32_t for
                                // portability!!
                                if (version >= 1)
                                {
                                        if (version >= 4)
                                        {
                                                if (version >= 7)
                                                {
                                                        // Weights were removed from this class in v7 (MRPT
                                                        // 0.9.5),
                                                        //  so nothing else to do.
                                                }
                                                else
                                                {
                                                        // Go on with old serialization format, but discard
                                                        // weights:
                                                        std::vector<uint32_t> dummy_pointWeight(n);
                                                        in.ReadBufferFixEndianness(
                                                                &dummy_pointWeight[0], n);
                                                }
                                        }
                                        else
                                        {
                                                std::vector<uint32_t> dummy_pointWeight(n);
                                                in.ReadBufferFixEndianness(&dummy_pointWeight[0], n);
                                        }
                                }
                        }
 
                        if (version >= 2)
                        {
                                // version 2: options saved too
                                in >> insertionOptions.minDistBetweenLaserPoints >>
                                        insertionOptions.addToExistingPointsMap >>
                                        insertionOptions.also_interpolate >>
                                        insertionOptions.disableDeletion >>
                                        insertionOptions.fuseWithExisting >>
                                        insertionOptions.isPlanarMap;
 
                                if (version < 6)
                                {
                                        bool old_matchStaticPointsOnly;
                                        in >> old_matchStaticPointsOnly;
                                }
 
                                in >> insertionOptions.maxDistForInterpolatePoints;
 
                                {
                                        bool disableSaveAs3DObject;
                                        in >> disableSaveAs3DObject;
                                        genericMapParams.enableSaveAs3DObject =
                                                !disableSaveAs3DObject;
                                }
                        }
 
                        if (version >= 3)
                        {
                                in >> insertionOptions.horizontalTolerance;
                        }
 
                        if (version >= 5)  // version 5: added likelihoodOptions
                                likelihoodOptions.readFromStream(in);
 
                        if (version >= 8)  // version 8: added insertInvalidPoints
                                in >> insertionOptions.insertInvalidPoints;
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}
 
/*---------------------------------------------------------------
                                        Clear
  ---------------------------------------------------------------*/
void CSimplePointsMap::internal_clear()
{
        // This swap() thing is the only way to really deallocate the memory.
        vector_strong_clear(m_x);
        vector_strong_clear(m_y);
        vector_strong_clear(m_z);
 
        mark_as_modified();
}
 
void CSimplePointsMap::setPointFast(size_t index, float x, float y, float z)
{
        m_x[index] = x;
        m_y[index] = y;
        m_z[index] = z;
}
 
void CSimplePointsMap::insertPointFast(float x, float y, float z)
{
        m_x.push_back(x);
        m_y.push_back(y);
        m_z.push_back(z);
}
 
namespace mrpt
{
namespace maps
{
namespace detail
{
using mrpt::maps::CSimplePointsMap;
 
template <>
struct pointmap_traits<CSimplePointsMap>
{
        /** Helper method fot the generic implementation of
         * CPointsMap::loadFromRangeScan(), to be called only once before inserting
         * points - this is the place to reserve memory in lric for extra working
         * variables. */
        inline static void internal_loadFromRangeScan2D_init(
                CSimplePointsMap& me,
                mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
        {
                MRPT_UNUSED_PARAM(me);
                MRPT_UNUSED_PARAM(lric);
        }
        /** Helper method fot the generic implementation of
         * CPointsMap::loadFromRangeScan(), to be called once per range data */
        inline static void internal_loadFromRangeScan2D_prepareOneRange(
                CSimplePointsMap& me, const float gx, const float gy, const float gz,
                mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
        {
                MRPT_UNUSED_PARAM(me);
                MRPT_UNUSED_PARAM(gx);
                MRPT_UNUSED_PARAM(gy);
                MRPT_UNUSED_PARAM(gz);
                MRPT_UNUSED_PARAM(lric);
        }
        /** Helper method fot the generic implementation of
         * CPointsMap::loadFromRangeScan(), to be called after each
         * "{x,y,z}.push_back(...);" */
        inline static void internal_loadFromRangeScan2D_postPushBack(
                CSimplePointsMap& me,
                mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
        {
                MRPT_UNUSED_PARAM(me);
                MRPT_UNUSED_PARAM(lric);
        }
 
        /** Helper method fot the generic implementation of
         * CPointsMap::loadFromRangeScan(), to be called only once before inserting
         * points - this is the place to reserve memory in lric for extra working
         * variables. */
        inline static void internal_loadFromRangeScan3D_init(
                CSimplePointsMap& me,
                mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
        {
                MRPT_UNUSED_PARAM(me);
                MRPT_UNUSED_PARAM(lric);
        }
        /** Helper method fot the generic implementation of
         * CPointsMap::loadFromRangeScan(), to be called once per range data */
        inline static void internal_loadFromRangeScan3D_prepareOneRange(
                CSimplePointsMap& me, const float gx, const float gy, const float gz,
                mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
        {
                MRPT_UNUSED_PARAM(me);
                MRPT_UNUSED_PARAM(gx);
                MRPT_UNUSED_PARAM(gy);
                MRPT_UNUSED_PARAM(gz);
                MRPT_UNUSED_PARAM(lric);
        }
        /** Helper method fot the generic implementation of
         * CPointsMap::loadFromRangeScan(), to be called after each
         * "{x,y,z}.push_back(...);" */
        inline static void internal_loadFromRangeScan3D_postPushBack(
                CSimplePointsMap& me,
                mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
        {
                MRPT_UNUSED_PARAM(me);
                MRPT_UNUSED_PARAM(lric);
        }
        /** Helper method fot the generic implementation of
         * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
         * end */
        inline static void internal_loadFromRangeScan3D_postOneRange(
                CSimplePointsMap& me,
                mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
        {
                MRPT_UNUSED_PARAM(me);
                MRPT_UNUSED_PARAM(lric);
        }
};
}
}
}
 
/** See CPointsMap::loadFromRangeScan() */
void CSimplePointsMap::loadFromRangeScan(
        const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
{
        mrpt::maps::detail::loadFromRangeImpl<
                CSimplePointsMap>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
}
 
/** See CPointsMap::loadFromRangeScan() */
void CSimplePointsMap::loadFromRangeScan(
        const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
{
        mrpt::maps::detail::loadFromRangeImpl<
                CSimplePointsMap>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
}
 
// ================================ PLY files import & export virtual methods
// ================================
 
/** In a base class, reserve memory to prepare subsequent calls to
 * PLY_import_set_vertex */
void CSimplePointsMap::PLY_import_set_vertex_count(const size_t N)
{
        this->setSize(N);
}