CHeightGridMap2D.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/CHeightGridMap2D.h>
#include <mrpt/utils/CConfigFileBase.h>  // MRPT_LOAD_CONFIG_VAR()
#include <mrpt/poses/CPose3D.h>
#include <mrpt/utils/stl_serialization.h>
#include <mrpt/opengl/CMesh.h>
#include <mrpt/opengl/CPointCloudColoured.h>

//#include <mrpt/system/os.h>
//#include <mrpt/utils/color_maps.h>
#include <mrpt/utils/CStream.h>

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

//  =========== Begin of Map definition ============
MAP_DEFINITION_REGISTER(
        "CHeightGridMap2D,heightMap,dem", mrpt::maps::CHeightGridMap2D)

CHeightGridMap2D::TMapDefinition::TMapDefinition()
        : min_x(-2),
          max_x(2),
          min_y(-2),
          max_y(2),
          resolution(0.10f),
          mapType(CHeightGridMap2D::mrSimpleAverage)
{
}

void CHeightGridMap2D::TMapDefinition::loadFromConfigFile_map_specific(
        const mrpt::utils::CConfigFileBase& source,
        const std::string& sectionNamePrefix)
{
        // [<sectionNamePrefix>+"_creationOpts"]
        const std::string sSectCreation =
                sectionNamePrefix + string("_creationOpts");
        MRPT_LOAD_CONFIG_VAR(min_x, double, source, sSectCreation);
        MRPT_LOAD_CONFIG_VAR(max_x, double, source, sSectCreation);
        MRPT_LOAD_CONFIG_VAR(min_y, double, source, sSectCreation);
        MRPT_LOAD_CONFIG_VAR(max_y, double, source, sSectCreation);
        MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation);
        mapType = source.read_enum<CHeightGridMap2D::TMapRepresentation>(
                sSectCreation, "mapType", mapType);

        insertionOpts.loadFromConfigFile(
                source, sectionNamePrefix + string("_insertOpts"));
}

void CHeightGridMap2D::TMapDefinition::dumpToTextStream_map_specific(
        mrpt::utils::CStream& out) const
{
        LOADABLEOPTS_DUMP_VAR(min_x, double);
        LOADABLEOPTS_DUMP_VAR(max_x, double);
        LOADABLEOPTS_DUMP_VAR(min_y, double);
        LOADABLEOPTS_DUMP_VAR(max_y, double);
        LOADABLEOPTS_DUMP_VAR(resolution, double);
        out.printf(
                "MAP TYPE                                  = %s\n",
                mrpt::utils::TEnumType<
                        CHeightGridMap2D::TMapRepresentation>::value2name(mapType)
                        .c_str());

        this->insertionOpts.dumpToTextStream(out);
}

mrpt::maps::CMetricMap* CHeightGridMap2D::internal_CreateFromMapDefinition(
        const mrpt::maps::TMetricMapInitializer& _def)
{
        const CHeightGridMap2D::TMapDefinition& def =
                *dynamic_cast<const CHeightGridMap2D::TMapDefinition*>(&_def);
        CHeightGridMap2D* obj = new CHeightGridMap2D(
                def.mapType, def.min_x, def.max_x, def.min_y, def.max_y,
                def.resolution);
        obj->insertionOptions = def.insertionOpts;
        return obj;
}
//  =========== End of Map definition Block =========

IMPLEMENTS_SERIALIZABLE(CHeightGridMap2D, CMetricMap, mrpt::maps)

bool HEIGHTGRIDMAP_EXPORT3D_AS_MESH_value = true;

bool mrpt::global_settings::HEIGHTGRIDMAP_EXPORT3D_AS_MESH()
{
        return HEIGHTGRIDMAP_EXPORT3D_AS_MESH_value;
}
void mrpt::global_settings::HEIGHTGRIDMAP_EXPORT3D_AS_MESH(bool value)
{
        HEIGHTGRIDMAP_EXPORT3D_AS_MESH_value = value;
}

/*---------------------------------------------------------------
                                                Constructor
  ---------------------------------------------------------------*/
CHeightGridMap2D::CHeightGridMap2D(
        TMapRepresentation mapType, double x_min, double x_max, double y_min,
        double y_max, double resolution)
        : CDynamicGrid<THeightGridmapCell>(x_min, x_max, y_min, y_max, resolution),
          insertionOptions(),
          m_mapType(mapType)
{
}

/*---------------------------------------------------------------
                                                clear
  ---------------------------------------------------------------*/
void CHeightGridMap2D::internal_clear() { fill(THeightGridmapCell()); }
/*---------------------------------------------------------------
                                                isEmpty
  ---------------------------------------------------------------*/
bool CHeightGridMap2D::isEmpty() const { return false; }
bool CHeightGridMap2D::insertIndividualPoint(
        const double x, const double y, const double z,
        const CHeightGridMap2D_Base::TPointInsertParams& params)
{
        THeightGridmapCell* cell = cellByPos(x, y);
        if (!cell)
                return false;  // Out of the map: Ignore if we've not resized before.

        if (!insertionOptions.filterByHeight ||
                (z >= insertionOptions.z_min && z <= insertionOptions.z_max))
        {
                cell->u += z;
                cell->v += z * z;
                if (!cell->w)
                {
                        cell->h = z;  // First observation
                        cell->w = 1;
                }
                else
                {
                        float W = cell->w++;  // W = N-1
                        cell->h = (cell->h * W + z) / cell->w;
                        if (W > 0)
                                cell->var = 1 / (W) * (cell->v - pow(cell->u, 2) / cell->w);
                }
        }  // end if really inserted
        return true;
}

bool CHeightGridMap2D::internal_insertObservation(
        const CObservation* obs, const CPose3D* robotPose)
{
        return dem_internal_insertObservation(obs, robotPose);
}

/*---------------------------------------------------------------
                                                computeObservationLikelihood
  ---------------------------------------------------------------*/
double CHeightGridMap2D::internal_computeObservationLikelihood(
        const CObservation* obs, const CPose3D& takenFrom)
{
        MRPT_UNUSED_PARAM(obs);
        MRPT_UNUSED_PARAM(takenFrom);

        THROW_EXCEPTION("Not implemented yet!");
}

/*---------------------------------------------------------------
  Implements the writing to a CStream capability of CSerializable objects
 ---------------------------------------------------------------*/
void CHeightGridMap2D::writeToStream(
        mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 3;
        else
        {
                dyngridcommon_writeToStream(out);

                // To assure compatibility: The size of each cell:
                uint32_t n = static_cast<uint32_t>(sizeof(THeightGridmapCell));
                out << n;

                // Save the map contents:
                n = static_cast<uint32_t>(m_map.size());
                out << n;
                for (vector<THeightGridmapCell>::const_iterator it = m_map.begin();
                         it != m_map.end(); ++it)
                        out << it->h
                                << it->w;  // This was removed in version 1: << it->history_Zs;

                // Save the insertion options:
                out << uint8_t(m_mapType);

                out << insertionOptions.filterByHeight << insertionOptions.z_min
                        << insertionOptions.z_max;

                out << genericMapParams;  // v2
        }
}

/*---------------------------------------------------------------
  Implements the reading from a CStream capability of CSerializable objects
 ---------------------------------------------------------------*/
void CHeightGridMap2D::readFromStream(mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                case 1:
                case 2:
                case 3:
                {
                        dyngridcommon_readFromStream(in, version < 3);
                        // To ensure compatibility: The size of each cell:
                        uint32_t n;
                        in >> n;
                        ASSERT_(n == static_cast<uint32_t>(sizeof(THeightGridmapCell)));

                        // Save the map contents:
                        in >> n;
                        m_map.resize(n);
                        for (vector<THeightGridmapCell>::iterator it = m_map.begin();
                                 it != m_map.end(); ++it)
                        {
                                in >> it->h >> it->w;
                                // Data member in version 0:
                                if (version == 0)
                                {
                                        std::multimap<mrpt::system::TTimeStamp, float> history_Zs;
                                        in >> history_Zs;  // Discarded now...
                                }
                        }

                        // Insertion options:
                        uint8_t ty;
                        in >> ty;
                        m_mapType = TMapRepresentation(ty);

                        in >> insertionOptions.filterByHeight >> insertionOptions.z_min >>
                                insertionOptions.z_max;

                        if (version >= 2) in >> genericMapParams;
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}

/*---------------------------------------------------------------
                                        TInsertionOptions
 ---------------------------------------------------------------*/
CHeightGridMap2D::TInsertionOptions::TInsertionOptions()
        : filterByHeight(false), z_min(-0.5), z_max(0.5), colorMap(cmJET)
{
}

/*---------------------------------------------------------------
                                        dumpToTextStream
  ---------------------------------------------------------------*/
void CHeightGridMap2D::TInsertionOptions::dumpToTextStream(
        mrpt::utils::CStream& out) const
{
        out.printf(
                "\n----------- [CHeightGridMap2D::TInsertionOptions] ------------ "
                "\n\n");
        out.printf(
                "filterByHeight                          = %c\n",
                filterByHeight ? 'y' : 'n');
        out.printf("z_min                                   = %f\n", z_min);
        out.printf("z_max                                   = %f\n", z_max);
        out.printf(
                "colormap                                = %s\n",
                colorMap == cmJET ? "jet" : "grayscale");
        out.printf("\n");
}

/*---------------------------------------------------------------
                                        loadFromConfigFile
  ---------------------------------------------------------------*/
void CHeightGridMap2D::TInsertionOptions::loadFromConfigFile(
        const mrpt::utils::CConfigFileBase& iniFile, const std::string& section)
{
        MRPT_LOAD_CONFIG_VAR(filterByHeight, bool, iniFile, section)
        MRPT_LOAD_CONFIG_VAR(z_min, float, iniFile, section)
        MRPT_LOAD_CONFIG_VAR(z_max, float, iniFile, section)
        string aux = iniFile.read_string(section, "colorMap", "jet");

        if (strCmp(aux, "jet"))
                colorMap = cmJET;
        else if (strCmp(aux, "grayscale"))
                colorMap = cmGRAYSCALE;
}

/*---------------------------------------------------------------
                                        saveMetricMapRepresentationToFile
  ---------------------------------------------------------------*/
void CHeightGridMap2D::saveMetricMapRepresentationToFile(
        const std::string& filNamePrefix) const
{
        // Text matrix:
        saveToTextFile(filNamePrefix + std::string("_mean.txt"));
}

/*---------------------------------------------------------------
                                                getAs3DObject
---------------------------------------------------------------*/
void CHeightGridMap2D::getAs3DObject(
        mrpt::opengl::CSetOfObjects::Ptr& outObj) const
{
        if (!genericMapParams.enableSaveAs3DObject) return;

        if (HEIGHTGRIDMAP_EXPORT3D_AS_MESH_value)
        {
                opengl::CMesh::Ptr mesh = mrpt::make_aligned_shared<opengl::CMesh>();

                mesh->setGridLimits(m_x_min, m_x_max, m_y_min, m_y_max);

                mesh->setColor(0.4, 0.4, 0.4);

                mesh->enableWireFrame(true);
                mesh->enableColorFromZ(true, insertionOptions.colorMap /*cmJET*/);

                CMatrixFloat Z, mask;
                /*mesh->getZ(Z);

                mesh->getMask(mask);*/

                Z.setSize(m_size_x, m_size_y);
                mask.setSize(m_size_x, m_size_y);

                for (size_t x = 0; x < m_size_x; x++)
                {
                        for (size_t y = 0; y < m_size_y; y++)
                        {
                                const THeightGridmapCell* c = cellByIndex(x, y);
                                ASSERTDEB_(c);
                                Z.set_unsafe(x, y, c->h);
                                mask.set_unsafe(x, y, c->w ? 1 : 0);
                        }
                }
                mesh->setZ(Z);
                mesh->setMask(mask);

                outObj->insert(mesh);
        }
        else
        {
                // As points:
                mrpt::opengl::CPointCloudColoured::Ptr obj =
                        mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
                obj->setPointSize(2);

                // Find min/max:
                float z_min, z_max;
                float K;
                if (this->getMinMaxHeight(z_min, z_max))
                        K = 1.0f / (z_max - z_min);
                else
                        K = 1.0f;

                obj->reserve(m_size_x * m_size_y);
                for (size_t x = 0; x < m_size_x; x++)
                        for (size_t y = 0; y < m_size_y; y++)
                        {
                                const THeightGridmapCell* c = cellByIndex(x, y);
                                ASSERTDEB_(c)
                                if (c->w)
                                {
                                        float r, g, b;
                                        const float col_idx = (c->h - z_min) * K;
                                        colormap(
                                                insertionOptions.colorMap,  // cmJET, //cmGRAYSCALE,
                                                col_idx, r, g, b);
                                        obj->push_back(idx2x(x), idx2y(y), c->h, r, g, b);
                                }
                        }

                outObj->insert(obj);
        }
}

/** Return the number of cells with at least one height data inserted. */
size_t CHeightGridMap2D::countObservedCells() const
{
        switch (m_mapType)
        {
                case mrSimpleAverage:
                {
                        size_t obsCells = 0;
                        const size_t N = m_map.size();
                        for (size_t i = 0; i < N; i++)
                                if (m_map[i].w) obsCells++;
                        return obsCells;
                }
                break;
                default:
                        THROW_EXCEPTION(
                                "countObservedCells() not implemented for this mapType (!?)")
        };
}

double CHeightGridMap2D::dem_get_resolution() const { return m_resolution; }
size_t CHeightGridMap2D::dem_get_size_x() const { return m_size_x; }
size_t CHeightGridMap2D::dem_get_size_y() const { return m_size_y; }
bool CHeightGridMap2D::dem_get_z_by_cell(
        const size_t cx, const size_t cy, double& z_out) const
{
        const THeightGridmapCell* cell = cellByIndex(cx, cy);
        if (cell && cell->w)
        {
                z_out = cell->h;
                return true;
        }
        else
                return false;
}
bool CHeightGridMap2D::dem_get_z(
        const double x, const double y, double& z_out) const
{
        const THeightGridmapCell* cell = cellByPos(x, y);
        if (cell && cell->w)
        {
                z_out = cell->h;
                return true;
        }
        else
        {
                return false;
        }
}
void CHeightGridMap2D::dem_update_map()
{
        // Nothing to do in this class: estimate is always up-to-date
}

float CHeightGridMap2D::compute3DMatchingRatio(
        const mrpt::maps::CMetricMap* otherMap,
        const mrpt::poses::CPose3D& otherMapPose,
        const TMatchingRatioParams& params) const
{
        MRPT_UNUSED_PARAM(otherMap);
        MRPT_UNUSED_PARAM(otherMapPose);
        MRPT_UNUSED_PARAM(params);
        return 0;
}