CColouredPointsMap.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 "maps-precomp.h"  // Precomp header

#include <mrpt/maps/CColouredPointsMap.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/utils/color_maps.h>
#include <mrpt/system/os.h>
#include <mrpt/opengl/CPointCloudColoured.h>
#include <mrpt/utils/CStream.h>

#include "CPointsMap_crtp_common.h"

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

//  =========== Begin of Map definition ============
MAP_DEFINITION_REGISTER(
        "CColouredPointsMap,colourPointsMap", mrpt::maps::CColouredPointsMap)

CColouredPointsMap::TMapDefinition::TMapDefinition() {}
void CColouredPointsMap::TMapDefinition::loadFromConfigFile_map_specific(
        const mrpt::utils::CConfigFileBase& source,
        const std::string& sectionNamePrefix)
{
        insertionOpts.loadFromConfigFile(
                source, sectionNamePrefix + string("_insertOpts"));
        likelihoodOpts.loadFromConfigFile(
                source, sectionNamePrefix + string("_likelihoodOpts"));
        colourOpts.loadFromConfigFile(
                source, sectionNamePrefix + string("_colorOpts"));
}

void CColouredPointsMap::TMapDefinition::dumpToTextStream_map_specific(
        mrpt::utils::CStream& out) const
{
        this->insertionOpts.dumpToTextStream(out);
        this->likelihoodOpts.dumpToTextStream(out);
        this->colourOpts.dumpToTextStream(out);
}

mrpt::maps::CMetricMap* CColouredPointsMap::internal_CreateFromMapDefinition(
        const mrpt::maps::TMetricMapInitializer& _def)
{
        const CColouredPointsMap::TMapDefinition& def =
                *dynamic_cast<const CColouredPointsMap::TMapDefinition*>(&_def);
        CColouredPointsMap* obj = new CColouredPointsMap();
        obj->insertionOptions = def.insertionOpts;
        obj->likelihoodOptions = def.likelihoodOpts;
        obj->colorScheme = def.colourOpts;
        return obj;
}
//  =========== End of Map definition Block =========

IMPLEMENTS_SERIALIZABLE(CColouredPointsMap, CPointsMap, mrpt::maps)

#if MRPT_HAS_PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
//#   include <pcl/registration/icp.h>
#endif

/*---------------------------------------------------------------
                                                Constructor
  ---------------------------------------------------------------*/
CColouredPointsMap::CColouredPointsMap() {}
/*---------------------------------------------------------------
                                                Destructor
  ---------------------------------------------------------------*/
CColouredPointsMap::~CColouredPointsMap() {}
/*---------------------------------------------------------------
                                reserve & resize methods
 ---------------------------------------------------------------*/
void CColouredPointsMap::reserve(size_t newLength)
{
        newLength = mrpt::utils::length2length4N(newLength);

        x.reserve(newLength);
        y.reserve(newLength);
        z.reserve(newLength);
        m_color_R.reserve(newLength);
        m_color_G.reserve(newLength);
        m_color_B.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 CColouredPointsMap::resize(size_t newLength)
{
        this->reserve(newLength);  // to ensure 4N capacity

        x.resize(newLength, 0);
        y.resize(newLength, 0);
        z.resize(newLength, 0);
        m_color_R.resize(newLength, 1);
        m_color_G.resize(newLength, 1);
        m_color_B.resize(newLength, 1);
        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 CColouredPointsMap::setSize(size_t newLength)
{
        this->reserve(newLength);  // to ensure 4N capacity

        x.assign(newLength, 0);
        y.assign(newLength, 0);
        z.assign(newLength, 0);
        m_color_R.assign(newLength, 1);
        m_color_G.assign(newLength, 1);
        m_color_B.assign(newLength, 1);
        mark_as_modified();
}

/*---------------------------------------------------------------
                                                Copy constructor
  ---------------------------------------------------------------*/
void CColouredPointsMap::copyFrom(const CPointsMap& obj)
{
        CPointsMap::base_copyFrom(
                obj);  // This also does a ::resize(N) of all data fields.

        const CColouredPointsMap* pCol =
                dynamic_cast<const CColouredPointsMap*>(&obj);
        if (pCol)
        {
                m_color_R = pCol->m_color_R;
                m_color_G = pCol->m_color_G;
                m_color_B = pCol->m_color_B;
                // m_min_dist = pCol->m_min_dist;
        }
}

/*---------------------------------------------------------------
                                        writeToStream
   Implements the writing to a CStream capability of
         CSerializable objects
  ---------------------------------------------------------------*/
void CColouredPointsMap::writeToStream(
        mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 9;
        else
        {
                uint32_t n = x.size();

                // First, write the number of points:
                out << n;

                if (n > 0)
                {
                        out.WriteBufferFixEndianness(&x[0], n);
                        out.WriteBufferFixEndianness(&y[0], n);
                        out.WriteBufferFixEndianness(&z[0], n);
                }
                out << m_color_R << m_color_G << m_color_B;  // added in v4

                out << genericMapParams;  // v9
                insertionOptions.writeToStream(
                        out);  // version 9?: insert options are saved with its own method
                likelihoodOptions.writeToStream(out);  // Added in version 5
        }
}

/*---------------------------------------------------------------
                                        readFromStream
   Implements the reading from a CStream capability of
          CSerializable objects
  ---------------------------------------------------------------*/
void CColouredPointsMap::readFromStream(mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 8:
                case 9:
                {
                        mark_as_modified();

                        // Read the number of points:
                        uint32_t n;
                        in >> n;

                        this->resize(n);

                        if (n > 0)
                        {
                                in.ReadBufferFixEndianness(&x[0], n);
                                in.ReadBufferFixEndianness(&y[0], n);
                                in.ReadBufferFixEndianness(&z[0], n);
                        }
                        in >> m_color_R >> m_color_G >> m_color_B;

                        if (version >= 9)
                                in >> genericMapParams;
                        else
                        {
                                bool disableSaveAs3DObject;
                                in >> disableSaveAs3DObject;
                                genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
                        }
                        insertionOptions.readFromStream(in);
                        likelihoodOptions.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(&x[0], n);
                                in.ReadBufferFixEndianness(&y[0], n);
                                in.ReadBufferFixEndianness(&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 >= 4)  // Color data
                        {
                                in >> m_color_R >> m_color_G >> m_color_B;
                                if (version >= 7)
                                {
                                        // Removed: in >> m_min_dist;
                                }
                                else
                                {
                                        std::vector<float> dummy_dist;
                                        in >> dummy_dist;
                                }
                        }
                        else
                        {
                                m_color_R.assign(x.size(), 1.0f);
                                m_color_G.assign(x.size(), 1.0f);
                                m_color_B.assign(x.size(), 1.0f);
                                // m_min_dist.assign(x.size(),2000.0f);
                        }

                        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 CColouredPointsMap::internal_clear()
{
        // This swap() thing is the only way to really deallocate the memory.
        vector_strong_clear(x);
        vector_strong_clear(y);
        vector_strong_clear(z);

        vector_strong_clear(m_color_R);
        vector_strong_clear(m_color_G);
        vector_strong_clear(m_color_B);

        mark_as_modified();
}

/** Changes a given point from map. First index is 0.
 * \exception Throws std::exception on index out of bound.
 */
void CColouredPointsMap::setPoint(
        size_t index, float x, float y, float z, float R, float G, float B)
{
        if (index >= this->x.size()) THROW_EXCEPTION("Index out of bounds");
        this->x[index] = x;
        this->y[index] = y;
        this->z[index] = z;
        this->m_color_R[index] = R;
        this->m_color_G[index] = G;
        this->m_color_B[index] = B;
        mark_as_modified();
}

/** Changes just the color of a given point from the map. First index is 0.
 * \exception Throws std::exception on index out of bound.
 */
void CColouredPointsMap::setPointColor(size_t index, float R, float G, float B)
{
        if (index >= this->x.size()) THROW_EXCEPTION("Index out of bounds");
        this->m_color_R[index] = R;
        this->m_color_G[index] = G;
        this->m_color_B[index] = B;
        // mark_as_modified();  // No need to rebuild KD-trees, etc...
}

void CColouredPointsMap::insertPointFast(float x, float y, float z)
{
        this->x.push_back(x);
        this->y.push_back(y);
        this->z.push_back(z);
        m_color_R.push_back(1);
        m_color_G.push_back(1);
        m_color_B.push_back(1);

        // mark_as_modified(); -> Fast
}

void CColouredPointsMap::insertPoint(
        float x, float y, float z, float R, float G, float B)
{
        this->x.push_back(x);
        this->y.push_back(y);
        this->z.push_back(z);
        m_color_R.push_back(R);
        m_color_G.push_back(G);
        m_color_B.push_back(B);

        mark_as_modified();
}

/*---------------------------------------------------------------
                                        getAs3DObject
 ---------------------------------------------------------------*/
void CColouredPointsMap::getAs3DObject(
        mrpt::opengl::CSetOfObjects::Ptr& outObj) const
{
        ASSERT_(outObj);

        if (!genericMapParams.enableSaveAs3DObject) return;

        opengl::CPointCloudColoured::Ptr obj =
                mrpt::make_aligned_shared<opengl::CPointCloudColoured>();

        obj->loadFromPointsMap(this);
        obj->setColor(1, 1, 1, 1.0);

        obj->setPointSize(mrpt::global_settings::POINTSMAPS_3DOBJECT_POINTSIZE());

        outObj->insert(obj);
}

/*---------------------------------------------------------------
                                        TColourOptions
 ---------------------------------------------------------------*/
CColouredPointsMap::TColourOptions::TColourOptions()
        : scheme(cmFromHeightRelativeToSensor), z_min(-10), z_max(10), d_max(5)
{
}

void CColouredPointsMap::TColourOptions::loadFromConfigFile(
        const mrpt::utils::CConfigFileBase& source, const std::string& section)
{
        scheme = source.read_enum(section, "scheme", scheme);
        MRPT_LOAD_CONFIG_VAR(z_min, float, source, section)
        MRPT_LOAD_CONFIG_VAR(z_max, float, source, section)
        MRPT_LOAD_CONFIG_VAR(d_max, float, source, section)
}

void CColouredPointsMap::TColourOptions::dumpToTextStream(
        mrpt::utils::CStream& out) const
{
        out.printf(
                "\n----------- [CColouredPointsMap::TColourOptions] ------------ \n\n");

        out.printf(
                "scheme                                  = %u\n", (unsigned)scheme);
        out.printf("z_min                                   = %f\n", z_min);
        out.printf("z_max                                   = %f\n", z_max);
        out.printf("d_max                                   = %f\n", d_max);
}

/*---------------------------------------------------------------
                                        getPoint
 ---------------------------------------------------------------*/
unsigned long CColouredPointsMap::getPoint(
        size_t index, float& x, float& y, float& z) const
{
        if (index >= this->x.size()) THROW_EXCEPTION("Index out of bounds");

        x = this->x[index];
        y = this->y[index];
        z = this->z[index];

        return 1;  // Weight
}

/*---------------------------------------------------------------
                                        getPoint
 ---------------------------------------------------------------*/
void CColouredPointsMap::getPoint(
        size_t index, float& x, float& y, float& z, float& R, float& G,
        float& B) const
{
        if (index >= this->x.size()) THROW_EXCEPTION("Index out of bounds");

        x = this->x[index];
        y = this->y[index];
        z = this->z[index];

        R = m_color_R[index];
        G = m_color_G[index];
        B = m_color_B[index];
}

/** Retrieves a point color (colors range is [0,1])
  */
void CColouredPointsMap::getPointColor(
        size_t index, float& R, float& G, float& B) const
{
        if (index >= this->x.size()) THROW_EXCEPTION("Index out of bounds");

        R = m_color_R[index];
        G = m_color_G[index];
        B = m_color_B[index];
}

// This function is duplicated from
// "mrpt::vision::pinhole::projectPoint_with_distortion", to avoid
//  a dependency on mrpt-vision.
void aux_projectPoint_with_distortion(
        const mrpt::math::TPoint3D& P, const mrpt::utils::TCamera& params,
        mrpt::utils::TPixelCoordf& pixel, bool accept_points_behind)
{
        MRPT_UNUSED_PARAM(accept_points_behind);
        // Pinhole model:
        const double x = P.x / P.z;
        const double y = P.y / P.z;

        // Radial distortion:
        const double r2 = square(x) + square(y);
        const double r4 = square(r2);
        const double r6 = r2 * r4;

        pixel.x = params.cx() +
                          params.fx() * (x * (1 + params.dist[0] * r2 +
                                                                  params.dist[1] * r4 + params.dist[4] * r6) +
                                                         2 * params.dist[2] * x * y +
                                                         params.dist[3] * (r2 + 2 * square(x)));
        pixel.y = params.cy() +
                          params.fy() * (y * (1 + params.dist[0] * r2 +
                                                                  params.dist[1] * r4 + params.dist[4] * r6) +
                                                         2 * params.dist[3] * x * y +
                                                         params.dist[2] * (r2 + 2 * square(y)));
}

/*---------------------------------------------------------------
                                        getPoint
 ---------------------------------------------------------------*/
bool CColouredPointsMap::colourFromObservation(
        const CObservationImage& obs, const CPose3D& robotPose)
{
        // Check if image is not grayscale
        ASSERT_(obs.image.isColor());

        CPose3D cameraPoseR;  // Get Camera Pose on the Robot(B) (CPose3D)
        CPose3D cameraPoseW;  // World Camera Pose

        obs.getSensorPose(cameraPoseR);
        cameraPoseW = robotPose + cameraPoseR;  // Camera Global Coordinates

        // Image Information
        unsigned int imgW = obs.image.getWidth();
        unsigned int imgH = obs.image.getHeight();

        // Projection related variables
        std::vector<TPixelCoordf>
                projectedPoints;  // The set of projected points in the image
        std::vector<TPixelCoordf>::iterator
                itProPoints;  // Iterator for projectedPoints
        std::vector<size_t> p_idx;
        std::vector<float> p_dist;
        std::vector<unsigned int> p_proj;

        // Get the N closest points
        kdTreeNClosestPoint2DIdx(
                cameraPoseW.x(), cameraPoseW.y(),  // query point
                200000,  // number of points to search
                p_idx, p_dist);  // indexes and distances of the returned points

        // Fill p3D vector
        for (size_t k = 0; k < p_idx.size(); k++)
        {
                float d = sqrt(p_dist[k]);
                size_t idx = p_idx[k];
                if (d < colorScheme.d_max)  //  && d < m_min_dist[idx] )
                {
                        TPixelCoordf px;
                        aux_projectPoint_with_distortion(
                                TPoint3D(x[idx], y[idx], z[idx]), obs.cameraParams, px, true);
                        projectedPoints.push_back(px);
                        p_proj.push_back(k);
                }  // end if
        }  // end for

        // Get channel order
        unsigned int chR, chG, chB;
        if (obs.image.getChannelsOrder()[0] == 'B')
        {
                chR = 2;
                chG = 1;
                chB = 0;
        }
        else
        {
                chR = 0;
                chG = 1;
                chB = 2;
        }

        unsigned int n_proj = 0;
        const float factor = 1.0f / 255;  // Normalize pixels:

        // Get the colour of the projected points
        size_t k;
        for (itProPoints = projectedPoints.begin(), k = 0;
                 itProPoints != projectedPoints.end(); ++itProPoints, ++k)
        {
                // Only get the points projected inside the image
                if (itProPoints->x >= 0 && itProPoints->x < imgW &&
                        itProPoints->y > 0 && itProPoints->y < imgH)
                {
                        unsigned int ii = p_idx[p_proj[k]];
                        uint8_t* p = obs.image(
                                (unsigned int)itProPoints->x, (unsigned int)itProPoints->y);

                        m_color_R[ii] = p[chR] * factor;  // R
                        m_color_G[ii] = p[chG] * factor;  // G
                        m_color_B[ii] = p[chB] * factor;  // B
                        // m_min_dist[ii]       = p_dist[p_proj[k]];

                        n_proj++;
                }
        }  // end for

        return true;
}  // end colourFromObservation

void CColouredPointsMap::resetPointsMinDist(float defValue)
{
        MRPT_UNUSED_PARAM(defValue);
        // m_min_dist.assign(x.size(),defValue);
}

bool CColouredPointsMap::save3D_and_colour_to_text_file(
        const std::string& file) const
{
        FILE* f = os::fopen(file.c_str(), "wt");
        if (!f) return false;

        for (unsigned int i = 0; i < x.size(); i++)
                os::fprintf(
                        f, "%f %f %f %d %d %d\n", x[i], y[i], z[i],
                        (uint8_t)(255 * m_color_R[i]), (uint8_t)(255 * m_color_G[i]),
                        (uint8_t)(255 * m_color_B[i]));
        //      os::fprintf(f,"%f %f %f %f %f %f
        //%f\n",x[i],y[i],z[i],m_color_R[i],m_color_G[i],m_color_B[i],m_min_dist[i]);

        os::fclose(f);
        return true;
}

// ================================ PLY files import & export virtual methods
// ================================

/** In a base class, reserve memory to prepare subsequent calls to
 * PLY_import_set_vertex */
void CColouredPointsMap::PLY_import_set_vertex_count(const size_t N)
{
        this->setSize(N);
}

/** In a base class, will be called after PLY_import_set_vertex_count() once for
 * each loaded point.
  *  \param pt_color Will be nullptr if the loaded file does not provide color
 * info.
  */
void CColouredPointsMap::PLY_import_set_vertex(
        const size_t idx, const mrpt::math::TPoint3Df& pt,
        const mrpt::utils::TColorf* pt_color)
{
        if (pt_color)
                this->setPoint(
                        idx, pt.x, pt.y, pt.z, pt_color->R, pt_color->G, pt_color->B);
        else
                this->setPoint(idx, pt.x, pt.y, pt.z);
}

/** In a base class, will be called after PLY_export_get_vertex_count() once for
 * each exported point.
  *  \param pt_color Will be nullptr if the loaded file does not provide color
 * info.
  */
void CColouredPointsMap::PLY_export_get_vertex(
        const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
        mrpt::utils::TColorf& pt_color) const
{
        pt_has_color = true;

        pt.x = x[idx];
        pt.y = y[idx];
        pt.z = z[idx];

        pt_color.R = m_color_R[idx];
        pt_color.G = m_color_G[idx];
        pt_color.B = m_color_B[idx];
}

/*---------------------------------------------------------------
                                                addFrom_classSpecific
 ---------------------------------------------------------------*/
void CColouredPointsMap::addFrom_classSpecific(
        const CPointsMap& anotherMap, const size_t nPreviousPoints)
{
        const size_t nOther = anotherMap.size();

        // Specific data for this class:
        const CColouredPointsMap* anotheMap_col =
                dynamic_cast<const CColouredPointsMap*>(&anotherMap);

        if (anotheMap_col)
        {
                for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
                {
                        m_color_R[j] = anotheMap_col->m_color_R[i];
                        m_color_G[j] = anotheMap_col->m_color_G[i];
                        m_color_B[j] = anotheMap_col->m_color_B[i];
                }
        }
}

/** Save the point cloud as a PCL PCD file, in either ASCII or binary format
 * \return false on any error */
bool CColouredPointsMap::savePCDFile(
        const std::string& filename, bool save_as_binary) const
{
#if MRPT_HAS_PCL
        pcl::PointCloud<pcl::PointXYZRGB> cloud;

        const size_t nThis = this->size();

        // Fill in the cloud data
        cloud.width = nThis;
        cloud.height = 1;
        cloud.is_dense = false;
        cloud.points.resize(cloud.width * cloud.height);

        const float f = 255.f;

        union myaux_t {
                uint8_t rgb[4];
                float f;
        } aux_val;

        for (size_t i = 0; i < nThis; ++i)
        {
                cloud.points[i].x = this->x[i];
                cloud.points[i].y = this->y[i];
                cloud.points[i].z = this->z[i];

                aux_val.rgb[0] = static_cast<uint8_t>(this->m_color_B[i] * f);
                aux_val.rgb[1] = static_cast<uint8_t>(this->m_color_G[i] * f);
                aux_val.rgb[2] = static_cast<uint8_t>(this->m_color_R[i] * f);

                cloud.points[i].rgb = aux_val.f;
        }

        return 0 == pcl::io::savePCDFile(filename, cloud, save_as_binary);

#else
        MRPT_UNUSED_PARAM(filename);
        MRPT_UNUSED_PARAM(save_as_binary);
        THROW_EXCEPTION("Operation not available: MRPT was built without PCL")
#endif
}

namespace mrpt
{
namespace maps
{
namespace detail
{
using mrpt::maps::CColouredPointsMap;

template <>
struct pointmap_traits<CColouredPointsMap>
{
        /** 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(
                CColouredPointsMap& me,
                mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
        {
                // Vars:
                //  [0] -> pR
                //  [1] -> pG
                //  [2] -> pB
                //  [3] -> Az_1_color
                lric.fVars.resize(4);

                ASSERT_NOT_EQUAL_(me.colorScheme.z_max, me.colorScheme.z_min)
                lric.fVars[3] = 1.0 / (me.colorScheme.z_max - me.colorScheme.z_min);
        }

        /** Helper method fot the generic implementation of
         * CPointsMap::loadFromRangeScan(), to be called once per range data */
        inline static void internal_loadFromRangeScan2D_prepareOneRange(
                CColouredPointsMap& me, const float gx, const float gy, const float gz,
                mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
        {
                MRPT_UNUSED_PARAM(gx);
                MRPT_UNUSED_PARAM(gy);
                // Relative height of the point wrt the sensor:
                const float rel_z = gz - lric.HM.get_unsafe(2, 3);  // m23;

                // Variable renaming:
                float& pR = lric.fVars[0];
                float& pG = lric.fVars[1];
                float& pB = lric.fVars[2];
                const float& Az_1_color = lric.fVars[3];

                // Compute color:
                switch (me.colorScheme.scheme)
                {
                        // case cmFromHeightRelativeToSensor:
                        case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
                        case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
                        {
                                float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
                                if (q < 0)
                                        q = 0;
                                else if (q > 1)
                                        q = 1;

                                if (me.colorScheme.scheme ==
                                        CColouredPointsMap::cmFromHeightRelativeToSensorGray)
                                {
                                        pR = pG = pB = q;
                                }
                                else
                                {
                                        mrpt::utils::jet2rgb(q, pR, pG, pB);
                                }
                        }
                        break;
                        case CColouredPointsMap::cmFromIntensityImage:
                        {
                                // In 2D scans we don't have this channel.
                                pR = pG = pB = 1.0;
                        }
                        break;
                        default:
                                THROW_EXCEPTION("Unknown color scheme");
                }
        }
        /** 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(
                CColouredPointsMap& me,
                mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
        {
                float& pR = lric.fVars[0];
                float& pG = lric.fVars[1];
                float& pB = lric.fVars[2];
                me.m_color_R.push_back(pR);
                me.m_color_G.push_back(pG);
                me.m_color_B.push_back(pB);
                // m_min_dist.push_back(1e4);
        }

        /** 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(
                CColouredPointsMap& me,
                mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
        {
                // Vars:
                //  [0] -> pR
                //  [1] -> pG
                //  [2] -> pB
                //  [3] -> Az_1_color
                //  [4] -> K_8u
                //  [5] -> cx
                //  [6] -> cy
                //  [7] -> fx
                //  [8] -> fy
                lric.fVars.resize(9);
                float& cx = lric.fVars[5];
                float& cy = lric.fVars[6];
                float& fx = lric.fVars[7];
                float& fy = lric.fVars[8];

                // unsigned int vars:
                //  [0] -> imgW
                //  [1] -> imgH
                //  [2] -> img_idx_x
                //  [3] -> img_idx_y
                lric.uVars.resize(4);
                unsigned int& imgW = lric.uVars[0];
                unsigned int& imgH = lric.uVars[1];
                unsigned int& img_idx_x = lric.uVars[2];
                unsigned int& img_idx_y = lric.uVars[3];

                // bool vars:
                //  [0] -> hasValidIntensityImage
                //  [1] -> hasColorIntensityImg
                //  [2] -> simple_3d_to_color_relation
                lric.bVars.resize(3);
                uint8_t& hasValidIntensityImage = lric.bVars[0];
                uint8_t& hasColorIntensityImg = lric.bVars[1];
                uint8_t& simple_3d_to_color_relation = lric.bVars[2];

                ASSERT_NOT_EQUAL_(me.colorScheme.z_max, me.colorScheme.z_min)
                lric.fVars[3] = 1.0 / (me.colorScheme.z_max -
                                                           me.colorScheme.z_min);  // Az_1_color = ...
                lric.fVars[4] = 1.0f / 255;  // K_8u

                hasValidIntensityImage = false;
                imgW = 0;
                imgH = 0;

                if (lric.rangeScan.hasIntensityImage)
                {
                        // assure the size matches:
                        if (lric.rangeScan.points3D_x.size() ==
                                lric.rangeScan.intensityImage.getWidth() *
                                        lric.rangeScan.intensityImage.getHeight())
                        {
                                hasValidIntensityImage = true;
                                imgW = lric.rangeScan.intensityImage.getWidth();
                                imgH = lric.rangeScan.intensityImage.getHeight();
                        }
                }

                hasColorIntensityImg =
                        hasValidIntensityImage && lric.rangeScan.intensityImage.isColor();

                // running indices for the image pixels for the gray levels:
                // If both range & intensity images coincide (e.g. SwissRanger), then we
                // can just
                // assign 3D points to image pixels one-to-one, but that's not the case
                // if
                //
                simple_3d_to_color_relation =
                        (std::abs(lric.rangeScan.relativePoseIntensityWRTDepth.norm()) <
                         1e-5);
                img_idx_x = 0;
                img_idx_y = 0;

                // Will be used just if simple_3d_to_color_relation=false
                cx = lric.rangeScan.cameraParamsIntensity.cx();
                cy = lric.rangeScan.cameraParamsIntensity.cy();
                fx = lric.rangeScan.cameraParamsIntensity.fx();
                fy = lric.rangeScan.cameraParamsIntensity.fy();
        }

        /** Helper method fot the generic implementation of
         * CPointsMap::loadFromRangeScan(), to be called once per range data */
        inline static void internal_loadFromRangeScan3D_prepareOneRange(
                CColouredPointsMap& me, const float gx, const float gy, const float gz,
                mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
        {
                MRPT_UNUSED_PARAM(gx);
                MRPT_UNUSED_PARAM(gy);
                // Rename variables:
                float& pR = lric.fVars[0];
                float& pG = lric.fVars[1];
                float& pB = lric.fVars[2];
                float& Az_1_color = lric.fVars[3];
                float& K_8u = lric.fVars[4];
                float& cx = lric.fVars[5];
                float& cy = lric.fVars[6];
                float& fx = lric.fVars[7];
                float& fy = lric.fVars[8];

                unsigned int& imgW = lric.uVars[0];
                unsigned int& imgH = lric.uVars[1];
                unsigned int& img_idx_x = lric.uVars[2];
                unsigned int& img_idx_y = lric.uVars[3];

                const uint8_t& hasValidIntensityImage = lric.bVars[0];
                const uint8_t& hasColorIntensityImg = lric.bVars[1];
                const uint8_t& simple_3d_to_color_relation = lric.bVars[2];

                // Relative height of the point wrt the sensor:
                const float rel_z = gz - lric.HM.get_unsafe(2, 3);  // m23;

                // Compute color:
                switch (me.colorScheme.scheme)
                {
                        // case cmFromHeightRelativeToSensor:
                        case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
                        case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
                        {
                                float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
                                if (q < 0)
                                        q = 0;
                                else if (q > 1)
                                        q = 1;

                                if (me.colorScheme.scheme ==
                                        CColouredPointsMap::cmFromHeightRelativeToSensorGray)
                                {
                                        pR = pG = pB = q;
                                }
                                else
                                {
                                        mrpt::utils::jet2rgb(q, pR, pG, pB);
                                }
                        }
                        break;
                        case CColouredPointsMap::cmFromIntensityImage:
                        {
                                // Do we have to project the 3D point into the image plane??
                                bool hasValidColor = false;
                                if (simple_3d_to_color_relation)
                                {
                                        hasValidColor = true;
                                }
                                else
                                {
                                        // Do projection:
                                        TPoint3D pt;  // pt_wrt_colorcam;
                                        lric.rangeScan.relativePoseIntensityWRTDepth
                                                .inverseComposePoint(
                                                        lric.scan_x, lric.scan_y, lric.scan_z, pt.x, pt.y,
                                                        pt.z);

                                        // Project to image plane:
                                        if (pt.z)
                                        {
                                                img_idx_x = cx + fx * pt.x / pt.z;
                                                img_idx_y = cy + fy * pt.y / pt.z;

                                                hasValidColor = img_idx_x < imgW &&  // img_idx_x>=0
                                                                                // isn't needed for
                                                                                // unsigned.
                                                                                img_idx_y < imgH;
                                        }
                                }

                                if (hasValidColor && hasColorIntensityImg)
                                {
                                        const uint8_t* c = lric.rangeScan.intensityImage.get_unsafe(
                                                img_idx_x, img_idx_y, 0);
                                        pR = c[2] * K_8u;
                                        pG = c[1] * K_8u;
                                        pB = c[0] * K_8u;
                                }
                                else if (hasValidColor && hasValidIntensityImage)
                                {
                                        uint8_t c = *lric.rangeScan.intensityImage.get_unsafe(
                                                img_idx_x, img_idx_y, 0);
                                        pR = pG = pB = c * K_8u;
                                }
                                else
                                {
                                        pR = pG = pB = 1.0;
                                }
                        }
                        break;
                        default:
                                THROW_EXCEPTION("Unknown color scheme");
                }
        }

        /** 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(
                CColouredPointsMap& me,
                mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
        {
                float& pR = lric.fVars[0];
                float& pG = lric.fVars[1];
                float& pB = lric.fVars[2];

                me.m_color_R.push_back(pR);
                me.m_color_G.push_back(pG);
                me.m_color_B.push_back(pB);

                // m_min_dist.push_back(1e4);
        }

        /** 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(
                CColouredPointsMap& me,
                mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
        {
                MRPT_UNUSED_PARAM(me);
                unsigned int& imgW = lric.uVars[0];
                unsigned int& img_idx_x = lric.uVars[2];
                unsigned int& img_idx_y = lric.uVars[3];

                const uint8_t& hasValidIntensityImage = lric.bVars[0];
                const uint8_t& simple_3d_to_color_relation = lric.bVars[2];

                // Advance the color pointer (just for simple cases, e.g. SwissRanger,
                // not for Kinect)
                if (simple_3d_to_color_relation && hasValidIntensityImage)
                {
                        if (++img_idx_x >= imgW)
                        {
                                img_idx_y++;
                                img_idx_x = 0;
                        }
                }
        }
};
}
}
}

/** See CPointsMap::loadFromRangeScan() */
void CColouredPointsMap::loadFromRangeScan(
        const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
{
        mrpt::maps::detail::loadFromRangeImpl<CColouredPointsMap>::
		templ_loadFromRangeScan(*this, rangeScan, robotPose);
}

/** See CPointsMap::loadFromRangeScan() */
void CColouredPointsMap::loadFromRangeScan(
        const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
{
        mrpt::maps::detail::loadFromRangeImpl<CColouredPointsMap>::
		templ_loadFromRangeScan(*this, rangeScan, robotPose);
}