MRPT  1.9.9
PbMap.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://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
13  * href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
14  */
15 #include "pbmap-precomp.h" // Precompiled headers
16 
17 #include <mrpt/pbmap.h>
18 
22 #include <pcl/io/io.h>
23 #include <pcl/io/pcd_io.h>
24 
25 using namespace std;
26 using namespace mrpt::pbmap;
27 
29 
30 /*---------------------------------------------------------------
31  Constructor
32  ---------------------------------------------------------------*/
34  : FloorPlane(-1),
35  globalMapPtr(new pcl::PointCloud<pcl::PointXYZRGBA>()),
36  edgeCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>),
37  outEdgeCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>)
38 {
39 }
40 
41 uint8_t PbMap::serializeGetVersion() const { return 0; }
42 void PbMap::serializeTo(mrpt::serialization::CArchive& out) const
43 {
44  // Write label
45  out << label;
46 
47  // The data
48  auto n = uint32_t(vPlanes.size());
49  out << n;
50  // cout << "Write " << n << " planes\n";
51  for (uint32_t i = 0; i < n; i++) out << vPlanes[i];
52 }
53 
54 void PbMap::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
55 {
56  switch (version)
57  {
58  case 0:
59  {
60  // Read label
61  in >> label;
62 
63  // Delete previous content:
64  vPlanes.clear();
65 
66  // The data
67  // First, write the number of planes:
68  uint32_t n;
69  in >> n;
70  vPlanes.resize(n);
71  for (uint32_t i = 0; i < n; i++)
72  {
73  Plane pl;
74  pl.id = i;
75  in >> pl;
76  vPlanes[i] = pl;
77  }
78  }
79  break;
80  default:
82  };
83 }
84 
85 void PbMap::savePbMap(string filePath)
86 {
87  // Serialize PbMap
88  mrpt::io::CFileGZOutputStream f(filePath + "/planes.pbmap");
89  auto serialize_pbmap = mrpt::serialization::archiveFrom(f);
90  serialize_pbmap << *this;
91  f.close();
92 
93  pcl::io::savePCDFile(filePath + "/cloud.pcd", *this->globalMapPtr);
94 }
95 
96 void PbMap::loadPbMap(std::string filePath)
97 {
98  // Read in the cloud data
99  pcl::PCDReader reader;
100  string PbMapFile = filePath;
101  reader.read(PbMapFile.append("/cloud.pcd"), *(this->globalMapPtr));
102 
103  // Load Previous Map
104  PbMapFile = filePath;
106  if (f.open(PbMapFile.append("/planes.pbmap")))
107  {
108  auto serialized_pbmap = mrpt::serialization::archiveFrom(f);
109  serialized_pbmap >> *this;
110  }
111  else
112  cout << "Error: cannot open " << PbMapFile << "\n";
113  f.close();
114 
115  // std::cout << "Load PbMap from " << filePath << "\n";
116 }
117 
118 // Merge two pbmaps.
119 void PbMap::MergeWith(PbMap& pbm, Eigen::Matrix4f& T)
120 {
121  // Rotate and translate PbMap
122  for (size_t i = 0; i < pbm.vPlanes.size(); i++)
123  {
124  Plane plane = pbm.vPlanes[i];
125  // Plane plane = &pbm.vPlanes[i]; //Warning: It modifies the
126  // source!!!
127 
128  // Transform normal and ppal direction
129  plane.v3normal = T.block<3, 3>(0, 0) * plane.v3normal;
130  plane.v3PpalDir = T.block<3, 3>(0, 0) * plane.v3PpalDir;
131 
132  // Transform centroid
133  plane.v3center =
134  T.block<3, 3>(0, 0) * plane.v3center + T.block(0, 3, 3, 1);
135 
136  // Transform convex hull points
137  pcl::transformPointCloud(
138  *plane.polygonContourPtr, *plane.polygonContourPtr, T);
139 
140  pcl::transformPointCloud(
141  *plane.planePointCloudPtr, *plane.planePointCloudPtr, T);
142 
143  plane.id = vPlanes.size();
144 
145  vPlanes.push_back(plane);
146  }
147 
148  // Rotate and translate the point cloud
149  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr alignedPointCloud(
150  new pcl::PointCloud<pcl::PointXYZRGBA>);
151  pcl::transformPointCloud(*pbm.globalMapPtr, *alignedPointCloud, T);
152 
153  *globalMapPtr += *alignedPointCloud;
154 }
155 
156 #include <fstream>
157 // Print PbMap content to a text file
158 void PbMap::printPbMap(string txtFilePbm)
159 {
160  cout << "PbMap 0.2\n\n";
161 
162  ofstream pbm;
163  pbm.open(txtFilePbm.c_str());
164  pbm << "PbMap 0.2\n\n";
165  pbm << "MapPlanes " << vPlanes.size() << endl;
166  for (unsigned i = 0; i < vPlanes.size(); i++)
167  {
168  pbm << " ID " << vPlanes[i].id << " obs " << vPlanes[i].numObservations;
169  pbm << " areaVoxels " << vPlanes[i].areaVoxels << " areaHull "
170  << vPlanes[i].areaHull;
171  pbm << " ratioXY " << vPlanes[i].elongation << " structure "
172  << vPlanes[i].bFromStructure << " label " << vPlanes[i].label;
173  pbm << "\n normal\n"
174  << vPlanes[i].v3normal << "\n center\n"
175  << vPlanes[i].v3center;
176  pbm << "\n PpalComp\n"
177  << vPlanes[i].v3PpalDir << "\n RGB\n"
178  << vPlanes[i].v3colorNrgb;
179  pbm << "\n Neighbors (" << vPlanes[i].neighborPlanes.size() << "): ";
180  for (auto it = vPlanes[i].neighborPlanes.begin();
181  it != vPlanes[i].neighborPlanes.end(); it++)
182  pbm << it->first << " ";
183  pbm << "\n CommonObservations: ";
184  for (auto it = vPlanes[i].neighborPlanes.begin();
185  it != vPlanes[i].neighborPlanes.end(); it++)
186  pbm << it->second << " ";
187  pbm << "\n ConvexHull (" << vPlanes[i].polygonContourPtr->size()
188  << "): \n";
189  for (unsigned j = 0; j < vPlanes[i].polygonContourPtr->size(); j++)
190  pbm << "\t" << vPlanes[i].polygonContourPtr->points[j].x << " "
191  << vPlanes[i].polygonContourPtr->points[j].y << " "
192  << vPlanes[i].polygonContourPtr->points[j].z << endl;
193  pbm << endl;
194  }
195  pbm.close();
196 }
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr polygonContourPtr
Definition: Plane.h:179
A class used to store a planar feature (Plane for short).
Definition: Plane.h:43
Eigen::Vector3f v3PpalDir
Definition: Plane.h:144
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
GLenum GLsizei n
Definition: glext.h:5136
STL namespace.
unsigned char uint8_t
Definition: rptypes.h:44
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:586
bool open(const std::string &fileName, mrpt::optional_ref< std::string > error_msg=std::nullopt)
Opens the file for read.
Eigen::Vector3f v3normal
Definition: Plane.h:139
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr planePointCloudPtr
Definition: Plane.h:183
GLsizei const GLchar ** string
Definition: glext.h:4116
pcl::PointCloud< PointT >::Ptr globalMapPtr
Definition: pbmap/PbMap.h:64
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:53
unsigned id
! Parameters to allow the plane-based representation of the map by a graph
Definition: Plane.h:124
void close()
Closes the file.
GLuint in
Definition: glext.h:7391
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Eigen::Vector3f v3center
! Geometric description
Definition: Plane.h:138
std::vector< Plane > vPlanes
Definition: pbmap/PbMap.h:54
Saves data to a file and transparently compress the data using the given compression level...
unsigned __int32 uint32_t
Definition: rptypes.h:50
A class used to store a Plane-based Map (PbMap).
Definition: pbmap/PbMap.h:44



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019