Main MRPT website > C++ reference for MRPT 1.5.7
PbMap.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 /* Plane-based Map (PbMap) library
11  * Construction of plane-based maps and localization in it from RGBD Images.
12  * Writen by Eduardo Fernandez-Moral. See docs for <a href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
13  */
14 #include "pbmap-precomp.h" // Precompiled headers
15 
16 #include <mrpt/pbmap.h>
17 
18 #include <mrpt/utils/CStream.h>
19 #include <pcl/io/io.h>
20 #include <pcl/io/pcd_io.h>
21 
22 using namespace std;
23 using namespace mrpt::utils;
24 using namespace mrpt::pbmap;
25 
26 
28 
29 /*---------------------------------------------------------------
30  Constructor
31  ---------------------------------------------------------------*/
33  FloorPlane(-1),
34  globalMapPtr( new pcl::PointCloud<pcl::PointXYZRGBA>() ),
35  edgeCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>),
36  outEdgeCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>)
37 {
38 }
39 
40 /*---------------------------------------------------------------
41  writeToStream
42  ---------------------------------------------------------------*/
43 void PbMap::writeToStream(mrpt::utils::CStream &out, int *out_Version) const
44 {
45 //cout << "Write PbMap. Version " << *out_Version << endl;
46  if (out_Version){//cout << "There is version\n";
47  *out_Version = 0;}
48  else
49  {
50  // Write label
51  out << label;
52 
53  // The data
54  uint32_t n = uint32_t( vPlanes.size() );
55  out << n;
56 // cout << "Write " << n << " planes\n";
57  for (uint32_t i=0; i < n; i++)
58  out << vPlanes[i];
59  }
60 //cout << "Exit Write PbMap. " << endl;
61 }
62 
63 /*---------------------------------------------------------------
64  readFromStream
65  ---------------------------------------------------------------*/
66 void PbMap::readFromStream(mrpt::utils::CStream &in, int version)
67 {
68  switch(version)
69  {
70  case 0:
71  {
72 // cout << "Read planes\n";
73 
74  // Read label
75  in >> label;
76 // cout << "PbMap label " << label << endl;
77 
78  // Delete previous content:
79  vPlanes.clear();
80 
81  // The data
82  // First, write the number of planes:
83  uint32_t n;
84  in >> n;
85  vPlanes.resize(n);
86  for (uint32_t i=0; i < n; i++)
87  {
88 // cout << "plane\n";
89 
90  Plane pl;
91  pl.id = i;
92  in >> pl;
93  vPlanes[i] = pl;
94  }
95 // cout << "Finish reading planes\n";
96  } break;
97  default:
99  };
100 }
101 
102 void PbMap::savePbMap(string filePath)
103 {
104 // boost::mutex::scoped_lock lock (mtx_pbmap_busy);
105 
106 //cout << "PbMap::savePbMap\n";
107  // Serialize PbMap
108  mrpt::utils::CFileGZOutputStream serialize_pbmap(filePath + "/planes.pbmap");
109  serialize_pbmap << *this;
110  serialize_pbmap.close();
111 
112 //cout << "PbMap::save cloud\n";
113  // Save reconstructed point cloud
114  pcl::io::savePCDFile(filePath + "/cloud.pcd", *this->globalMapPtr);
115 }
116 
117 void PbMap::loadPbMap(std::string filePath)
118 {
119  // Read in the cloud data
120  pcl::PCDReader reader;
121  string PbMapFile = filePath;
122  reader.read (PbMapFile.append("/cloud.pcd"), *(this->globalMapPtr));
123 // cout << "Size " << globalMapPtr->size() << " " << globalMapPtr->empty() << endl;
124 
125  // Load Previous Map
126  PbMapFile = filePath;
127  mrpt::utils::CFileGZInputStream serialized_pbmap;
128  if (serialized_pbmap.open(PbMapFile.append("/planes.pbmap")))
129  {
130  serialized_pbmap >> *this;
131  }
132  else
133  cout << "Error: cannot open " << PbMapFile << "\n";
134  serialized_pbmap.close();
135 
136 // std::cout << "Load PbMap from " << filePath << "\n";
137 }
138 
139 
140 // Merge two pbmaps.
141 void PbMap::MergeWith(PbMap &pbm, Eigen::Matrix4f &T)
142 {
143  // Rotate and translate PbMap
144  for(size_t i = 0; i < pbm.vPlanes.size(); i++)
145  {
146  Plane plane = pbm.vPlanes[i];
147 // Plane plane = &pbm.vPlanes[i]; //Warning: It modifies the source!!!
148 
149  // Transform normal and ppal direction
150  plane.v3normal = T.block(0,0,3,3) * plane.v3normal;
151  plane.v3PpalDir = T.block(0,0,3,3) * plane.v3PpalDir;
152 
153  // Transform centroid
154  plane.v3center = T.block(0,0,3,3) * plane.v3center + T.block(0,3,3,1);
155 
156  // Transform convex hull points
157  pcl::transformPointCloud(*plane.polygonContourPtr, *plane.polygonContourPtr, T);
158 
159  pcl::transformPointCloud(*plane.planePointCloudPtr, *plane.planePointCloudPtr, T);
160 
161  plane.id = vPlanes.size();
162 
163  vPlanes.push_back(plane);
164  }
165 
166  // Rotate and translate the point cloud
167  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr alignedPointCloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
168  pcl::transformPointCloud(*pbm.globalMapPtr,*alignedPointCloud,T);
169 
170  *globalMapPtr += *alignedPointCloud;
171 
172 }
173 
174 #include <fstream>
175 // Print PbMap content to a text file
176 void PbMap::printPbMap(string txtFilePbm)
177 {
178 cout << "PbMap 0.2\n\n";
179 
180  ofstream pbm;
181  pbm.open(txtFilePbm.c_str());
182  pbm << "PbMap 0.2\n\n";
183  pbm << "MapPlanes " << vPlanes.size() << endl;
184  for(unsigned i=0; i < vPlanes.size(); i++)
185  {
186  pbm << " ID " << vPlanes[i].id << " obs " << vPlanes[i].numObservations;
187  pbm << " areaVoxels " << vPlanes[i].areaVoxels << " areaHull " << vPlanes[i].areaHull;
188  pbm << " ratioXY " << vPlanes[i].elongation << " structure " << vPlanes[i].bFromStructure << " label " << vPlanes[i].label;
189  pbm << "\n normal\n" << vPlanes[i].v3normal << "\n center\n" << vPlanes[i].v3center;
190  pbm << "\n PpalComp\n" << vPlanes[i].v3PpalDir << "\n RGB\n" << vPlanes[i].v3colorNrgb;
191  pbm << "\n Neighbors (" << vPlanes[i].neighborPlanes.size() << "): ";
192  for(map<unsigned,unsigned>::iterator it=vPlanes[i].neighborPlanes.begin(); it != vPlanes[i].neighborPlanes.end(); it++)
193  pbm << it->first << " ";
194  pbm << "\n CommonObservations: ";
195  for(map<unsigned,unsigned>::iterator it=vPlanes[i].neighborPlanes.begin(); it != vPlanes[i].neighborPlanes.end(); it++)
196  pbm << it->second << " ";
197  pbm << "\n ConvexHull (" << vPlanes[i].polygonContourPtr->size() << "): \n";
198  for(unsigned j=0; j < vPlanes[i].polygonContourPtr->size(); j++)
199  pbm << "\t" << vPlanes[i].polygonContourPtr->points[j].x << " " << vPlanes[i].polygonContourPtr->points[j].y << " " << vPlanes[i].polygonContourPtr->points[j].z << endl;
200  pbm << endl;
201  }
202  pbm.close();
203 }
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr polygonContourPtr
Definition: Plane.h:167
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
A class used to store a planar feature (Plane for short).
Definition: Plane.h:48
Eigen::Vector3f v3PpalDir
Definition: Plane.h:136
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
bool open(const std::string &fileName)
Opens the file for read.
GLenum GLsizei n
Definition: glext.h:4618
Scalar * iterator
Definition: eigen_plugins.h:23
STL namespace.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Eigen::Vector3f v3normal
Definition: Plane.h:132
Transparently opens a compressed "gz" file and reads uncompressed data from it.
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr planePointCloudPtr
Definition: Plane.h:169
int version
Definition: mrpt_jpeglib.h:898
GLsizei const GLchar ** string
Definition: glext.h:3919
pcl::PointCloud< PointT >::Ptr globalMapPtr
Definition: pbmap/PbMap.h:64
#define CFileGZOutputStream
Saves data to a file and transparently compress the data using the given compression level...
unsigned id
! Parameters to allow the plane-based representation of the map by a graph
Definition: Plane.h:117
GLuint in
Definition: glext.h:6301
Eigen::Vector3f v3center
! Geometric description
Definition: Plane.h:131
std::vector< Plane > vPlanes
Definition: pbmap/PbMap.h:55
unsigned __int32 uint32_t
Definition: rptypes.h:49
A class used to store a Plane-based Map (PbMap).
Definition: pbmap/PbMap.h:45



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019