COccupancyGridMap2D_getAs.cpp

Go to the documentation of this file.

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

#include "maps-precomp.h"  // Precomp header

#include <mrpt/core/round.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/CTexturedPlane.h>
#include <mrpt/system/os.h>

using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace mrpt::img;
using namespace std;

/*---------------------------------------------------------------
                    getAsImage
  ---------------------------------------------------------------*/
void COccupancyGridMap2D::getAsImage(
    CImage& img, bool verticalFlip, bool forceRGB, bool tricolor) const
{
    if (!tricolor)
    {
        if (!forceRGB)
        {  // 8bit gray-scale
            img.resize(size_x, size_y, mrpt::img::CH_GRAY);
            const cellType* srcPtr = &map[0];
            unsigned char* destPtr;
            for (unsigned int y = 0; y < size_y; y++)
            {
                if (!verticalFlip)
                    destPtr = img(0, size_y - 1 - y);
                else
                    destPtr = img(0, y);
                for (unsigned int x = 0; x < size_x; x++)
                {
                    *destPtr++ = l2p_255(*srcPtr++);
                }
            }
        }
        else
        {  // 24bit RGB:
            img.resize(size_x, size_y, mrpt::img::CH_RGB);
            const cellType* srcPtr = &map[0];
            unsigned char* destPtr;
            for (unsigned int y = 0; y < size_y; y++)
            {
                if (!verticalFlip)
                    destPtr = img(0, size_y - 1 - y);
                else
                    destPtr = img(0, y);
                for (unsigned int x = 0; x < size_x; x++)
                {
                    uint8_t c = l2p_255(*srcPtr++);
                    *destPtr++ = c;
                    *destPtr++ = c;
                    *destPtr++ = c;
                }
            }
        }
    }
    else
    {
        // TRICOLOR: 0, 0.5, 1
        if (!forceRGB)
        {  // 8bit gray-scale
            img.resize(size_x, size_y, mrpt::img::CH_GRAY);
            const cellType* srcPtr = &map[0];
            unsigned char* destPtr;
            for (unsigned int y = 0; y < size_y; y++)
            {
                if (!verticalFlip)
                    destPtr = img(0, size_y - 1 - y);
                else
                    destPtr = img(0, y);
                for (unsigned int x = 0; x < size_x; x++)
                {
                    uint8_t c = l2p_255(*srcPtr++);
                    if (c < 120)
                        c = 0;
                    else if (c > 136)
                        c = 255;
                    else
                        c = 127;
                    *destPtr++ = c;
                }
            }
        }
        else
        {  // 24bit RGB:
            img.resize(size_x, size_y, mrpt::img::CH_RGB);
            const cellType* srcPtr = &map[0];
            unsigned char* destPtr;
            for (unsigned int y = 0; y < size_y; y++)
            {
                if (!verticalFlip)
                    destPtr = img(0, size_y - 1 - y);
                else
                    destPtr = img(0, y);
                for (unsigned int x = 0; x < size_x; x++)
                {
                    uint8_t c = l2p_255(*srcPtr++);
                    if (c < 120)
                        c = 0;
                    else if (c > 136)
                        c = 255;
                    else
                        c = 127;

                    *destPtr++ = c;
                    *destPtr++ = c;
                    *destPtr++ = c;
                }
            }
        }
    }
}

/*---------------------------------------------------------------
                    getAsImageFiltered
  ---------------------------------------------------------------*/
void COccupancyGridMap2D::getAsImageFiltered(
    CImage& img, bool verticalFlip, bool forceRGB) const
{
    getAsImage(img, verticalFlip, forceRGB);

    // Do filtering to improve the noisy peaks in grids:
    if (insertionOptions.CFD_features_gaussian_size != 0)
        img.filterGaussian(
            img, round(insertionOptions.CFD_features_gaussian_size));
    if (insertionOptions.CFD_features_median_size != 0)
        img.filterMedian(img, round(insertionOptions.CFD_features_median_size));
}

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

    MRPT_START

    auto outObj = mrpt::opengl::CTexturedPlane::Create();

    outObj->setPlaneCorners(x_min, x_max, y_min, y_max);

    outObj->setLocation(0, 0, insertionOptions.mapAltitude);

    // Create the color & transparecy (alpha) images:
    CImage imgColor(size_x, size_y, mrpt::img::CH_GRAY);
    CImage imgTrans(size_x, size_y, mrpt::img::CH_GRAY);

    const cellType* srcPtr = &map[0];

    for (unsigned int y = 0; y < size_y; y++)
    {
        unsigned char* destPtr_color = imgColor(0, y);
        unsigned char* destPtr_trans = imgTrans(0, y);
        for (unsigned int x = 0; x < size_x; x++)
        {
            uint8_t cell255 = l2p_255(*srcPtr++);
            *destPtr_color++ = cell255;

            int8_t auxC = (int8_t)((signed short)cell255) - 127;
            *destPtr_trans++ = auxC > 0 ? (auxC << 1) : ((-auxC) << 1);
        }
    }

    outObj->assignImage(imgColor, imgTrans);
    outSetOfObj->insert(outObj);

    MRPT_END
}

/** Get a point cloud with all (border) occupied cells as points */
void COccupancyGridMap2D::getAsPointCloud(
    mrpt::maps::CSimplePointsMap& pm, const float occup_threshold) const
{
    pm.clear();
    pm.reserve(1000);

    // for all rows in the gridmap
    for (size_t i = 1; i + 1 < size_x; i++)
    {
        // for all columns in the gridmap
        for (size_t j = 1; j + 1 < size_y; j++)
        {
            // if there is an obstacle and *it is a borderline*:
            bool is_surrounded = true;
            for (int di = -1; di <= 1 && is_surrounded; di++)
                for (int dj = -1; dj <= 1 && is_surrounded; dj++)
                    if ((di != 0 || dj != 0) &&
                        getCell(i + di, j + dj) > occup_threshold)
                        is_surrounded = false;

            if (getCell(i, j) < occup_threshold && !is_surrounded)
                pm.insertPoint(idx2x(i), idx2y(j));
        }
    }
}