PbMap.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 |
   +------------------------------------------------------------------------+ */

/*  Plane-based Map (PbMap) library
 *  Construction of plane-based maps and localization in it from RGBD Images.
 *  Writen by Eduardo Fernandez-Moral. See docs for <a
 * href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
 */
#include "pbmap-precomp.h"  // Precompiled headers

#include <mrpt/pbmap.h>

#include <mrpt/utils/CStream.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>

using namespace std;
using namespace mrpt::utils;
using namespace mrpt::pbmap;

IMPLEMENTS_SERIALIZABLE(PbMap, CSerializable, mrpt::pbmap)

/*---------------------------------------------------------------
        Constructor
  ---------------------------------------------------------------*/
PbMap::PbMap()
        : FloorPlane(-1),
          globalMapPtr(new pcl::PointCloud<pcl::PointXYZRGBA>()),
          edgeCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>),
          outEdgeCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>)
{
}

/*---------------------------------------------------------------
                                                writeToStream
 ---------------------------------------------------------------*/
void PbMap::writeToStream(mrpt::utils::CStream& out, int* out_Version) const
{
        // cout << "Write PbMap. Version " << *out_Version << endl;
        if (out_Version)
        {  // cout << "There is version\n";
                *out_Version = 0;
        }
        else
        {
                // Write label
                out << label;

                // The data
                uint32_t n = uint32_t(vPlanes.size());
                out << n;
                //  cout << "Write " << n << " planes\n";
                for (uint32_t i = 0; i < n; i++) out << vPlanes[i];
        }
        // cout << "Exit Write PbMap. " << endl;
}

/*---------------------------------------------------------------
                                                readFromStream
 ---------------------------------------------------------------*/
void PbMap::readFromStream(mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                {
                        //      cout << "Read planes\n";

                        // Read label
                        in >> label;
                        //      cout << "PbMap label " << label << endl;

                        // Delete previous content:
                        vPlanes.clear();

                        // The data
                        // First, write the number of planes:
                        uint32_t n;
                        in >> n;
                        vPlanes.resize(n);
                        for (uint32_t i = 0; i < n; i++)
                        {
                                //      cout << "plane\n";

                                Plane pl;
                                pl.id = i;
                                in >> pl;
                                vPlanes[i] = pl;
                        }
                        //        cout << "Finish reading planes\n";
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}

void PbMap::savePbMap(string filePath)
{
        //  boost::mutex::scoped_lock lock (mtx_pbmap_busy);

        // cout << "PbMap::savePbMap\n";
        // Serialize PbMap
        mrpt::utils::CFileGZOutputStream serialize_pbmap(
                filePath + "/planes.pbmap");
        serialize_pbmap << *this;
        serialize_pbmap.close();

        // cout << "PbMap::save cloud\n";
        // Save reconstructed point cloud
        pcl::io::savePCDFile(filePath + "/cloud.pcd", *this->globalMapPtr);
}

void PbMap::loadPbMap(std::string filePath)
{
        // Read in the cloud data
        pcl::PCDReader reader;
        string PbMapFile = filePath;
        reader.read(PbMapFile.append("/cloud.pcd"), *(this->globalMapPtr));
        //  cout << "Size " << globalMapPtr->size() << " " << globalMapPtr->empty()
        //  << endl;

        // Load Previous Map
        PbMapFile = filePath;
        mrpt::utils::CFileGZInputStream serialized_pbmap;
        if (serialized_pbmap.open(PbMapFile.append("/planes.pbmap")))
        {
                serialized_pbmap >> *this;
        }
        else
                cout << "Error: cannot open " << PbMapFile << "\n";
        serialized_pbmap.close();

        //  std::cout << "Load PbMap from " << filePath << "\n";
}

// Merge two pbmaps.
void PbMap::MergeWith(PbMap& pbm, Eigen::Matrix4f& T)
{
        // Rotate and translate PbMap
        for (size_t i = 0; i < pbm.vPlanes.size(); i++)
        {
                Plane plane = pbm.vPlanes[i];
                //    Plane plane = &pbm.vPlanes[i]; //Warning: It modifies the
                //    source!!!

                // Transform normal and ppal direction
                plane.v3normal = T.block(0, 0, 3, 3) * plane.v3normal;
                plane.v3PpalDir = T.block(0, 0, 3, 3) * plane.v3PpalDir;

                // Transform centroid
                plane.v3center =
                        T.block(0, 0, 3, 3) * plane.v3center + T.block(0, 3, 3, 1);

                // Transform convex hull points
                pcl::transformPointCloud(
                        *plane.polygonContourPtr, *plane.polygonContourPtr, T);

                pcl::transformPointCloud(
                        *plane.planePointCloudPtr, *plane.planePointCloudPtr, T);

                plane.id = vPlanes.size();

                vPlanes.push_back(plane);
        }

        // Rotate and translate the point cloud
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr alignedPointCloud(
                new pcl::PointCloud<pcl::PointXYZRGBA>);
        pcl::transformPointCloud(*pbm.globalMapPtr, *alignedPointCloud, T);

        *globalMapPtr += *alignedPointCloud;
}

#include <fstream>
// Print PbMap content to a text file
void PbMap::printPbMap(string txtFilePbm)
{
        cout << "PbMap 0.2\n\n";

        ofstream pbm;
        pbm.open(txtFilePbm.c_str());
        pbm << "PbMap 0.2\n\n";
        pbm << "MapPlanes " << vPlanes.size() << endl;
        for (unsigned i = 0; i < vPlanes.size(); i++)
        {
                pbm << " ID " << vPlanes[i].id << " obs " << vPlanes[i].numObservations;
                pbm << " areaVoxels " << vPlanes[i].areaVoxels << " areaHull "
                        << vPlanes[i].areaHull;
                pbm << " ratioXY " << vPlanes[i].elongation << " structure "
                        << vPlanes[i].bFromStructure << " label " << vPlanes[i].label;
                pbm << "\n normal\n"
                        << vPlanes[i].v3normal << "\n center\n"
                        << vPlanes[i].v3center;
                pbm << "\n PpalComp\n"
                        << vPlanes[i].v3PpalDir << "\n RGB\n"
                        << vPlanes[i].v3colorNrgb;
                pbm << "\n Neighbors (" << vPlanes[i].neighborPlanes.size() << "): ";
                for (map<unsigned, unsigned>::iterator it =
                                 vPlanes[i].neighborPlanes.begin();
                         it != vPlanes[i].neighborPlanes.end(); it++)
                        pbm << it->first << " ";
                pbm << "\n CommonObservations: ";
                for (map<unsigned, unsigned>::iterator it =
                                 vPlanes[i].neighborPlanes.begin();
                         it != vPlanes[i].neighborPlanes.end(); it++)
                        pbm << it->second << " ";
                pbm << "\n ConvexHull (" << vPlanes[i].polygonContourPtr->size()
                        << "): \n";
                for (unsigned j = 0; j < vPlanes[i].polygonContourPtr->size(); j++)
                        pbm << "\t" << vPlanes[i].polygonContourPtr->points[j].x << " "
                                << vPlanes[i].polygonContourPtr->points[j].y << " "
                                << vPlanes[i].polygonContourPtr->points[j].z << endl;
                pbm << endl;
        }
        pbm.close();
}