COccupancyGridMap2D_io.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/system/os.h>
#include <mrpt/math/CMatrix.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/utils/CEnhancedMetaFile.h>
#include <mrpt/utils/round.h> // round()
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/random.h>

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


/*---------------------------------------------------------------
                                        saveAsBitmapFile
  ---------------------------------------------------------------*/
bool  COccupancyGridMap2D::saveAsBitmapFile(const std::string &file) const
{
        MRPT_START

        CImage                  img;
        getAsImage(img);
        return img.saveToFile(file);

        MRPT_END
}




/*---------------------------------------------------------------
                                        writeToStream
        Implements the writing to a CStream capability of
          CSerializable objects
  ---------------------------------------------------------------*/
void  COccupancyGridMap2D::writeToStream(mrpt::utils::CStream &out, int *version) const
{
        if (version)
                *version = 6;
        else
        {
                // Version 3: Change to log-odds. The only change is in the loader, when translating
                //   from older versions.

                // Version 2: Save OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS/16BITS
#ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
                out << uint8_t(8);
#else
                out << uint8_t(16);
#endif

                out << size_x << size_y << x_min << x_max << y_min << y_max << resolution;
                ASSERT_(size_x*size_y==map.size());

#ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
                out.WriteBuffer(&map[0], sizeof(map[0])*size_x*size_y);
#else
                out.WriteBufferFixEndianness(&map[0], size_x*size_y);
#endif

                // insertionOptions:
                out <<  insertionOptions.mapAltitude
                        <<      insertionOptions.useMapAltitude
                        <<      insertionOptions.maxDistanceInsertion
                        <<      insertionOptions.maxOccupancyUpdateCertainty
                        <<      insertionOptions.considerInvalidRangesAsFreeSpace
                        <<      insertionOptions.decimation
                        <<      insertionOptions.horizontalTolerance;

                // Likelihood:
                out     <<      (int32_t)likelihoodOptions.likelihoodMethod
                        <<      likelihoodOptions.LF_stdHit
                        <<      likelihoodOptions.LF_zHit
                        <<      likelihoodOptions.LF_zRandom
                        <<      likelihoodOptions.LF_maxRange
                        <<      likelihoodOptions.LF_decimation
                        <<      likelihoodOptions.LF_maxCorrsDistance
                        <<      likelihoodOptions.LF_alternateAverageMethod
                        <<      likelihoodOptions.MI_exponent
                        <<      likelihoodOptions.MI_skip_rays
                        <<      likelihoodOptions.MI_ratio_max_distance
                        <<      likelihoodOptions.rayTracing_useDistanceFilter
                        <<      likelihoodOptions.rayTracing_decimation
                        <<      likelihoodOptions.rayTracing_stdHit
                        <<      likelihoodOptions.consensus_takeEachRange
                        <<      likelihoodOptions.consensus_pow
                        <<      likelihoodOptions.OWA_weights
                        <<      likelihoodOptions.enableLikelihoodCache;

                // Insertion as 3D:
                out << genericMapParams; // v6

                // Version 4:
                out << insertionOptions.CFD_features_gaussian_size
                    << insertionOptions.CFD_features_median_size;

                // Version: 5;
                out << insertionOptions.wideningBeamsWithDistance;

        }
}

/*---------------------------------------------------------------
                                        readFromStream
  ---------------------------------------------------------------*/
void  COccupancyGridMap2D::readFromStream(mrpt::utils::CStream &in, int version)
{
        m_is_empty = false;

        switch(version)
        {
        case 0:
        case 1:
        case 2:
        case 3:
        case 4:
        case 5:
        case 6:
                {
#                       ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
                                const uint8_t   MyBitsPerCell = 8;
#                       else
                                const uint8_t   MyBitsPerCell = 16;
#                       endif

                        uint8_t         bitsPerCellStream;

                        // Version 2: OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS/16BITS
                        if (version>=2)
                                        in >> bitsPerCellStream;
                        else    bitsPerCellStream = MyBitsPerCell;  // Old versinons: hope it's the same...

                        uint32_t        new_size_x,new_size_y;
                        float           new_x_min,new_x_max,new_y_min,new_y_max;
                        float                   new_resolution;

                        //resetFeaturesCache();

                        in >> new_size_x >> new_size_y >> new_x_min >> new_x_max >> new_y_min >> new_y_max >> new_resolution;

                        setSize(new_x_min,new_x_max,new_y_min,new_y_max,new_resolution,0.5);

                        ASSERT_(size_x*size_y==map.size());

                        if (bitsPerCellStream==MyBitsPerCell)
                        {
                                // Perfect:
                        #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
                                in.ReadBuffer(&map[0], sizeof(map[0])*map.size());
                        #else
                                in.ReadBufferFixEndianness(&map[0], map.size());
                        #endif
                        }
                        else
                        {
                                // We must do a conversion...
#                       ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
                                // We are 8-bit, stream is 16-bit
                                ASSERT_(bitsPerCellStream==16);
                                std::vector<uint16_t>    auxMap( map.size() );
                                in.ReadBuffer(&auxMap[0], sizeof(auxMap[0])*auxMap.size());

                                size_t  i, N = map.size();
                                uint8_t         *ptrTrg = (uint8_t*)&map[0];
                                const uint16_t  *ptrSrc = (const uint16_t*)&auxMap[0];
                                for (i=0;i<N;i++)
                                        *ptrTrg++ = (*ptrSrc++) >> 8;
#                       else
                                // We are 16-bit, stream is 8-bit
                                ASSERT_(bitsPerCellStream==8);
                                std::vector<uint8_t>    auxMap( map.size() );
                                in.ReadBuffer(&auxMap[0], sizeof(auxMap[0])*auxMap.size());

                                size_t  i, N = map.size();
                                uint16_t       *ptrTrg = (uint16_t*)&map[0];
                                const uint8_t  *ptrSrc = (const uint8_t*)&auxMap[0];
                                for (i=0;i<N;i++)
                                        *ptrTrg++ = (*ptrSrc++) << 8;
#                       endif
                        }

                        // If we are converting an old dump, convert from probabilities to log-odds:
                        if (version<3)
                        {
                                size_t  i, N = map.size();
                                cellType  *ptr = &map[0];
                                for (i=0;i<N;i++)
                                {
                                        double p = cellTypeUnsigned(*ptr) * (1.0f/0xFF);
                                        if (p<0)
                                                p=0;
                                        if (p>1)
                                                p=1;
                                        *ptr++ = p2l( p );
                                }
                        }

                        // For the precomputed likelihood trick:
                        precomputedLikelihoodToBeRecomputed = true;

                        if (version>=1)
                        {
                                // insertionOptions:
                                in  >>  insertionOptions.mapAltitude
                                        >>      insertionOptions.useMapAltitude
                                        >>      insertionOptions.maxDistanceInsertion
                                        >>      insertionOptions.maxOccupancyUpdateCertainty
                                        >>      insertionOptions.considerInvalidRangesAsFreeSpace
                                        >>      insertionOptions.decimation
                                        >>      insertionOptions.horizontalTolerance;

                                // Likelihood:
                                int32_t         i;
                                in      >>      i; likelihoodOptions.likelihoodMethod = static_cast<TLikelihoodMethod>(i);
                                in      >>      likelihoodOptions.LF_stdHit
                                        >>      likelihoodOptions.LF_zHit
                                        >>      likelihoodOptions.LF_zRandom
                                        >>      likelihoodOptions.LF_maxRange
                                        >>      likelihoodOptions.LF_decimation
                                        >>      likelihoodOptions.LF_maxCorrsDistance
                                        >>      likelihoodOptions.LF_alternateAverageMethod
                                        >>      likelihoodOptions.MI_exponent
                                        >>      likelihoodOptions.MI_skip_rays
                                        >>      likelihoodOptions.MI_ratio_max_distance
                                        >>      likelihoodOptions.rayTracing_useDistanceFilter
                                        >>      likelihoodOptions.rayTracing_decimation
                                        >>      likelihoodOptions.rayTracing_stdHit
                                        >>      likelihoodOptions.consensus_takeEachRange
                                        >>      likelihoodOptions.consensus_pow
                                        >>      likelihoodOptions.OWA_weights
                                        >>      likelihoodOptions.enableLikelihoodCache;

                                // Insertion as 3D:
                                if (version>=6)
                                        in >> genericMapParams;
                                else 
                                {
                                        bool disableSaveAs3DObject;
                                        in >> disableSaveAs3DObject;
                                        genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
                                }
                        }

                        if (version>=4)
                        {
                                in >> insertionOptions.CFD_features_gaussian_size
                                   >> insertionOptions.CFD_features_median_size;
                        }

                        if (version>=5)
                        {
                                in >> insertionOptions.wideningBeamsWithDistance;
                        }

                } break;
        default:
                MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)

        };
}

/*---------------------------------------------------------------
                                        loadFromBitmapFile
 Load a 8-bits, black & white bitmap file as a grid map. It will be loaded such as coordinates (0,0) falls just in the middle of map.
\param file The file to be loaded.
\param resolution The size of a pixel (cell), in meters. Recall cells are always squared, so just a dimension is needed.
\return False on any error.
 ---------------------------------------------------------------*/
bool  COccupancyGridMap2D::loadFromBitmapFile(
        const std::string       &file,
        float                   resolution,
    float                       origin_xPixel,
    float                       origin_yPixel)
{
        MRPT_START

        CImage          imgFl;
        if (!imgFl.loadFromFile(file,0))
                return false;

        m_is_empty = false;
        return loadFromBitmap(imgFl,resolution, origin_xPixel, origin_yPixel);

        MRPT_END
}

/*---------------------------------------------------------------
                                        loadFromBitmap
 ---------------------------------------------------------------*/
bool  COccupancyGridMap2D::loadFromBitmap(const mrpt::utils::CImage &imgFl, float resolution, float origin_xPixel, float origin_yPixel)
{
        MRPT_START

        // For the precomputed likelihood trick:
        precomputedLikelihoodToBeRecomputed = true;

        size_t bmpWidth = imgFl.getWidth();
        size_t bmpHeight = imgFl.getHeight();

        if (size_x!=bmpWidth || size_y!=bmpHeight)
        {
                // Middle of bitmap?
                if (origin_xPixel == std::numeric_limits<float>::max() ||
                    origin_yPixel == std::numeric_limits<float>::max())
                {
                        origin_xPixel = imgFl.getWidth() / 2.0f;
                        origin_yPixel = imgFl.getHeight() / 2.0f;
                }

                // Resize grid:
                float new_x_max = (imgFl.getWidth() - origin_xPixel) * resolution;
                float new_x_min = - origin_xPixel * resolution;
                float new_y_max = (imgFl.getHeight() - origin_yPixel) * resolution;
                float new_y_min = - origin_yPixel * resolution;

                setSize(new_x_min,new_x_max,new_y_min,new_y_max,resolution);
        }

        // And load cells content:
        for (size_t x=0;x<bmpWidth;x++)
                for (size_t y=0;y<bmpHeight;y++)
                {
                        float f = imgFl.getAsFloat(x,bmpHeight-1-y);
                        f = std::max(0.01f,f);
                        f = std::min(0.99f,f);
                        setCell(x,y,f);
                }

        m_is_empty = false;
        return true;

        MRPT_END
}



/*---------------------------------------------------------------
                                saveAsBitmapTwoMapsWithCorrespondences
  ---------------------------------------------------------------*/
bool  COccupancyGridMap2D::saveAsBitmapTwoMapsWithCorrespondences(
        const std::string                                               &fileName,
        const COccupancyGridMap2D                               *m1,
        const COccupancyGridMap2D                               *m2,
        const TMatchingPairList         &corrs)
{
        MRPT_START

        CImage                  img1,img2;
        CImage                  img(10,10,3,true);
        unsigned int    i,n , Ay1, Ay2;
        unsigned int    px, py;

        // The individual maps:
        // ---------------------------------------------
        m1->getAsImage( img1, false );
        m2->getAsImage( img2, false );
        unsigned int lx1 = img1.getWidth();
        unsigned int ly1 = img1.getHeight();

        unsigned int lx2 = img2.getWidth();
        unsigned int ly2 = img2.getHeight();

        // The map with the lowest height has to be vertically aligned:
        if (ly1>ly2)
        {
                Ay1 = 0;
                Ay2 = (ly1-ly2)/2;
        }
        else
        {
                Ay2 = 0;
                Ay1 = (ly2-ly1)/2;
        }


        // Compute the size of the composite image:
        // ---------------------------------------------
        img.resize(lx1 + lx2 + 1, max(ly1,ly2), 3, true );
        img.filledRectangle(0,0,img.getWidth()-1,img.getHeight()-1, TColor::black );    // background: black
        img.drawImage(0,Ay1,img1);
        img.drawImage(lx1+1,Ay2,img2);

        // Draw the features:
        // ---------------------------------------------
        n = corrs.size();
        TColor  lineColor = TColor::black;
        for (i=0;i<n;i++)
        {
                // In M1:
                px = m1->x2idx( corrs[i].this_x );
                py = Ay1+ly1-1- m1->y2idx( corrs[i].this_y );
                img.rectangle(px-10,py-10,px+10,py+10,lineColor);
                img.rectangle(px-11,py-11,px+11,py+11,lineColor);

                // In M2:
                px = lx1+1 + m2->x2idx( corrs[i].other_x );
                py = Ay2+ly2-1- m2->y2idx( corrs[i].other_y );
                img.rectangle(px-10,py-10,px+10,py+10,lineColor);
                img.rectangle(px-11,py-11,px+11,py+11,lineColor);
        }

        // Draw the correspondences as lines:
        // ---------------------------------------------
        for (i=0;i<n;i++)
        {
                lineColor = TColor(
                        static_cast<long>(randomGenerator.drawUniform(0,255.0f)),
                        static_cast<long>(randomGenerator.drawUniform(0,255.0f)),
                        static_cast<long>(randomGenerator.drawUniform(0,255.0f)) );

                img.line(
                        m1->x2idx( corrs[i].this_x ),
//                              lx1+1+ m1->x2idx( corrs[i].this_x ),
                        Ay1+ly1-1- m1->y2idx( corrs[i].this_y ),
                        lx1+1+ m2->x2idx( corrs[i].other_x ),
                        Ay2+ly2-1-m2->y2idx( corrs[i].other_y ),
            lineColor);
        } // i

        return img.saveToFile(fileName.c_str() );

        MRPT_END
}

/*---------------------------------------------------------------
                                saveAsEMFTwoMapsWithCorrespondences
  ---------------------------------------------------------------*/
bool  COccupancyGridMap2D::saveAsEMFTwoMapsWithCorrespondences(
        const std::string                                               &fileName,
        const COccupancyGridMap2D                               *m1,
        const COccupancyGridMap2D                               *m2,
        const TMatchingPairList         &corrs)
{
        MRPT_START

        CEnhancedMetaFile                               emf(fileName,1);
        CImage                  img1,img2;
        TColor                  lineColor;
        unsigned int    i, Ay1, Ay2;
        unsigned int    px, py;


        lineColor = TColor::red;

        // The individual maps:
        // ---------------------------------------------
#ifdef MRPT_OS_WINDOWS
        m1->getAsImage( img1, true );
        m2->getAsImage( img2, true );
#else
        m1->getAsImage( img1, false );          // Linux: emulated EMF is different.
        m2->getAsImage( img2, false );
#endif
        unsigned int lx1 = img1.getWidth();
        unsigned int ly1 = img1.getHeight();
        //unsigned int lx2 = img2.getWidth();
        unsigned int ly2 = img2.getHeight();

        // The map with the lowest height has to be vertically aligned:
        if (ly1>ly2)
        {
                Ay1 = 0;
                Ay2 = (ly1-ly2)/2;
        }
        else
        {
                Ay2 = 0;
                Ay1 = (ly2-ly1)/2;
        }


        // Draw the pair of maps:
        // ---------------------------------------------
        emf.drawImage(0,Ay1,img1);
        emf.drawImage(lx1+1,Ay2,img2);

        // Draw the features:
        // ---------------------------------------------
        const unsigned int n = corrs.size();
        lineColor = TColor::black;
        for (i=0;i<n;i++)
        {
                // In M1:
                px = m1->x2idx( corrs[i].this_x );
                py = Ay1+ly1-1- m1->y2idx( corrs[i].this_y );
                emf.rectangle(px-10,py-10,px+10,py+10,lineColor);
                emf.rectangle(px-11,py-11,px+11,py+11,lineColor);

                // In M2:
                px = lx1+1 + m2->x2idx( corrs[i].other_x );
                py = Ay2+ly2-1- m2->y2idx( corrs[i].other_y );
                emf.rectangle(px-10,py-10,px+10,py+10,lineColor);
                emf.rectangle(px-11,py-11,px+11,py+11,lineColor);
        }

/** /
        // Draw the correspondences as lines:
        // ---------------------------------------------
        for (i=0;i<n;i++)
        {
                lineColor =
                        ((unsigned long)RandomUni(0,255.0f)) +
                        (((unsigned long)RandomUni(0,255.0f)) << 8 ) +
                        (((unsigned long)RandomUni(0,255.0f)) << 16 );

                emf.line(
                        m1->x2idx( corrs[i].this_x ),
                        Ay1+ly1-1- m1->y2idx( corrs[i].this_y ),
                        lx1+1+ m2->x2idx( corrs[i].other_x ),
                        Ay2+ly2-1-m2->y2idx( corrs[i].other_y ),
            lineColor);
        } // i
/ **/

        // Draw the correspondences as text labels:
        // ---------------------------------------------
        char    str[100];
        for (i=0;i<n;i++)
        {
                os::sprintf(str,100,"%i",i);

                emf.textOut(
                        m1->x2idx( corrs[i].this_x ) - 10 ,
                        Ay1+ly1-1- m1->y2idx( corrs[i].this_y ) - 25,
                        str, TColor::black );

                emf.textOut(
                        lx1+1+ m2->x2idx( corrs[i].other_x ) - 10,
                        Ay2+ly2-1-m2->y2idx( corrs[i].other_y ) - 25,
                        str,TColor::black );
        } // i

        return true;

        MRPT_END
}


/*---------------------------------------------------------------
                                        auxParticleFilterCleanUp
 ---------------------------------------------------------------*/
void  COccupancyGridMap2D::saveMetricMapRepresentationToFile(
        const std::string       &filNamePrefix
        ) const
{
        std::string             fil( filNamePrefix + std::string(".png") );
        saveAsBitmapFile( fil  );

        fil = filNamePrefix + std::string("_limits.txt");
        CMatrix LIMITS(1,4);
        LIMITS(0,0) = x_min;
        LIMITS(0,1) = x_max;
        LIMITS(0,2) = y_min;
        LIMITS(0,3) = y_max;
        LIMITS.saveToTextFile( fil, MATRIX_FORMAT_FIXED, false /* add mrpt header */, "% Grid limits: [x_min x_max y_min y_max]\n" );
}