MRPT  1.9.9
CPointsMapXYZI.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 #include "maps-precomp.h" // Precomp header
11 
12 #include <mrpt/core/bits_mem.h>
20 #include <mrpt/system/filesystem.h>
21 #include <mrpt/system/os.h>
22 #include <iostream>
23 
24 #include "CPointsMap_crtp_common.h"
25 
26 using namespace std;
27 using namespace mrpt;
28 using namespace mrpt::maps;
29 using namespace mrpt::obs;
30 using namespace mrpt::img;
31 using namespace mrpt::poses;
32 using namespace mrpt::system;
33 using namespace mrpt::math;
34 using namespace mrpt::config;
35 
36 // =========== Begin of Map definition ============
38 
39 CPointsMapXYZI::TMapDefinition::TMapDefinition() = default;
40 
41 void CPointsMapXYZI::TMapDefinition::loadFromConfigFile_map_specific(
42  const CConfigFileBase& source, const std::string& sectionNamePrefix)
43 {
44  insertionOpts.loadFromConfigFile(
45  source, sectionNamePrefix + string("_insertOpts"));
46  likelihoodOpts.loadFromConfigFile(
47  source, sectionNamePrefix + string("_likelihoodOpts"));
48 }
49 
50 void CPointsMapXYZI::TMapDefinition::dumpToTextStream_map_specific(
51  std::ostream& out) const
52 {
53  this->insertionOpts.dumpToTextStream(out);
54  this->likelihoodOpts.dumpToTextStream(out);
55 }
56 
57 mrpt::maps::CMetricMap* CPointsMapXYZI::internal_CreateFromMapDefinition(
59 {
61  *dynamic_cast<const CPointsMapXYZI::TMapDefinition*>(&_def);
62  auto* obj = new CPointsMapXYZI();
63  obj->insertionOptions = def.insertionOpts;
64  obj->likelihoodOptions = def.likelihoodOpts;
65  return obj;
66 }
67 // =========== End of Map definition Block =========
68 
70 
71 #if MRPT_HAS_PCL
72 #include <pcl/io/pcd_io.h>
73 #include <pcl/point_types.h>
74 #endif
75 
76 void CPointsMapXYZI::reserve(size_t newLength)
77 {
78  m_x.reserve(newLength);
79  m_y.reserve(newLength);
80  m_z.reserve(newLength);
81  m_intensity.reserve(newLength);
82 }
83 
84 // Resizes all point buffers so they can hold the given number of points: newly
85 // created points are set to default values,
86 // and old contents are not changed.
87 void CPointsMapXYZI::resize(size_t newLength)
88 {
89  m_x.resize(newLength, 0);
90  m_y.resize(newLength, 0);
91  m_z.resize(newLength, 0);
92  m_intensity.resize(newLength, 1);
93  mark_as_modified();
94 }
95 
96 // Resizes all point buffers so they can hold the given number of points,
97 // *erasing* all previous contents
98 // and leaving all points to default values.
99 void CPointsMapXYZI::setSize(size_t newLength)
100 {
101  m_x.assign(newLength, 0);
102  m_y.assign(newLength, 0);
103  m_z.assign(newLength, 0);
104  m_intensity.assign(newLength, 0);
105  mark_as_modified();
106 }
107 
108 void CPointsMapXYZI::impl_copyFrom(const CPointsMap& obj)
109 {
110  // This also does a ::resize(N) of all data fields.
111  CPointsMap::base_copyFrom(obj);
112 
113  const auto* pXYZI = dynamic_cast<const CPointsMapXYZI*>(&obj);
114  if (pXYZI) m_intensity = pXYZI->m_intensity;
115 }
116 
117 uint8_t CPointsMapXYZI::serializeGetVersion() const { return 0; }
118 void CPointsMapXYZI::serializeTo(mrpt::serialization::CArchive& out) const
119 {
120  uint32_t n = m_x.size();
121 
122  // First, write the number of points:
123  out << n;
124 
125  if (n > 0)
126  {
127  out.WriteBufferFixEndianness(&m_x[0], n);
128  out.WriteBufferFixEndianness(&m_y[0], n);
129  out.WriteBufferFixEndianness(&m_z[0], n);
130  out.WriteBufferFixEndianness(&m_intensity[0], n);
131  }
132  insertionOptions.writeToStream(out);
133  likelihoodOptions.writeToStream(out);
134 }
135 
136 void CPointsMapXYZI::serializeFrom(
137  mrpt::serialization::CArchive& in, uint8_t version)
138 {
139  switch (version)
140  {
141  case 0:
142  {
143  mark_as_modified();
144 
145  // Read the number of points:
146  uint32_t n;
147  in >> n;
148  this->resize(n);
149  if (n > 0)
150  {
151  in.ReadBufferFixEndianness(&m_x[0], n);
152  in.ReadBufferFixEndianness(&m_y[0], n);
153  in.ReadBufferFixEndianness(&m_z[0], n);
154  in.ReadBufferFixEndianness(&m_intensity[0], n);
155  }
156  insertionOptions.readFromStream(in);
157  likelihoodOptions.readFromStream(in);
158  }
159  break;
160  default:
162  };
163 }
164 
165 void CPointsMapXYZI::internal_clear()
166 {
167  vector_strong_clear(m_x);
168  vector_strong_clear(m_y);
169  vector_strong_clear(m_z);
170  vector_strong_clear(m_intensity);
171  mark_as_modified();
172 }
173 
174 void CPointsMapXYZI::setPointRGB(
175  size_t index, float x, float y, float z, float R, float G, float B)
176 {
177  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
178  m_x[index] = x;
179  m_y[index] = y;
180  m_z[index] = z;
181  m_intensity[index] = R;
182  mark_as_modified();
183 }
184 
185 void CPointsMapXYZI::setPointIntensity(size_t index, float I)
186 {
187  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
188  this->m_intensity[index] = I;
189  // mark_as_modified(); // No need to rebuild KD-trees, etc...
190 }
191 
192 void CPointsMapXYZI::insertPointFast(float x, float y, float z)
193 {
194  m_x.push_back(x);
195  m_y.push_back(y);
196  m_z.push_back(z);
197  m_intensity.push_back(0);
198  // mark_as_modified(); -> Fast
199 }
200 
201 void CPointsMapXYZI::insertPointRGB(
202  float x, float y, float z, float R_intensity, float G_ignored,
203  float B_ignored)
204 {
205  m_x.push_back(x);
206  m_y.push_back(y);
207  m_z.push_back(z);
208  m_intensity.push_back(R_intensity);
209  mark_as_modified();
210 }
211 
212 void CPointsMapXYZI::getAs3DObject(
213  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
214 {
215  ASSERT_(outObj);
216 
217  if (!genericMapParams.enableSaveAs3DObject) return;
218 
220 
221  obj->loadFromPointsMap(this);
222  obj->setColor(1, 1, 1, 1.0);
223  obj->setPointSize(this->renderOptions.point_size);
224 
225  outObj->insert(obj);
226 }
227 
228 void CPointsMapXYZI::getPointRGB(
229  size_t index, float& x, float& y, float& z, float& R, float& G,
230  float& B) const
231 {
232  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
233 
234  x = m_x[index];
235  y = m_y[index];
236  z = m_z[index];
237  R = G = B = m_intensity[index];
238 }
239 
240 float CPointsMapXYZI::getPointIntensity(size_t index) const
241 {
242  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
243  return m_intensity[index];
244 }
245 
246 bool CPointsMapXYZI::saveXYZI_to_text_file(const std::string& file) const
247 {
248  FILE* f = os::fopen(file.c_str(), "wt");
249  if (!f) return false;
250  for (unsigned int i = 0; i < m_x.size(); i++)
251  os::fprintf(f, "%f %f %f %f\n", m_x[i], m_y[i], m_z[i], m_intensity[i]);
252  os::fclose(f);
253  return true;
254 }
255 
256 bool CPointsMapXYZI::loadXYZI_from_text_file(const std::string& file)
257 {
258  MRPT_START
259 
260  // Clear current map:
261  mark_as_modified();
262  this->clear();
263 
264  std::ifstream f;
265  f.open(file);
266  if (!f.is_open()) return false;
267 
268  while (!f.eof())
269  {
270  std::string line;
271  std::getline(f, line);
272 
273  std::stringstream ss(line);
274 
275  float x, y, z, i;
276  if (!(ss >> x >> y >> z >> i))
277  {
278  break;
279  }
280 
281  insertPointFast(x, y, z);
282  m_intensity.push_back(i);
283  }
284 
285  return true;
286 
287  MRPT_END
288 }
289 
290 /*---------------------------------------------------------------
291 addFrom_classSpecific
292 ---------------------------------------------------------------*/
293 void CPointsMapXYZI::addFrom_classSpecific(
294  const CPointsMap& anotherMap, const size_t nPreviousPoints)
295 {
296  const size_t nOther = anotherMap.size();
297 
298  // Specific data for this class:
299  const auto* anotheMap_col =
300  dynamic_cast<const CPointsMapXYZI*>(&anotherMap);
301 
302  if (anotheMap_col)
303  {
304  for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
305  m_intensity[j] = anotheMap_col->m_intensity[i];
306  }
307 }
308 
309 /** Save the point cloud as a PCL PCD file, in either ASCII or binary format
310  * \return false on any error */
311 bool CPointsMapXYZI::savePCDFile(
312  const std::string& filename, bool save_as_binary) const
313 {
314 #if MRPT_HAS_PCL
315  pcl::PointCloud<pcl::PointXYZI> cloud;
316 
317  const size_t nThis = this->size();
318 
319  // Fill in the cloud data
320  cloud.width = nThis;
321  cloud.height = 1;
322  cloud.is_dense = false;
323  cloud.points.resize(cloud.width * cloud.height);
324 
325  for (size_t i = 0; i < nThis; ++i)
326  {
327  cloud.points[i].x = m_x[i];
328  cloud.points[i].y = m_y[i];
329  cloud.points[i].z = m_z[i];
330  cloud.points[i].intensity = this->m_intensity[i];
331  }
332 
333  return 0 == pcl::io::savePCDFile(filename, cloud, save_as_binary);
334 
335 #else
336  MRPT_UNUSED_PARAM(filename);
337  MRPT_UNUSED_PARAM(save_as_binary);
338  THROW_EXCEPTION("Operation not available: MRPT was built without PCL");
339 #endif
340 }
341 
342 namespace mrpt::maps::detail
343 {
345 
346 template <>
348 {
349  /** Helper method fot the generic implementation of
350  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
351  * points - this is the place to reserve memory in lric for extra working
352  * variables. */
354  CPointsMapXYZI& me,
356  {
357  // lric.fVars: not needed
358  }
359 
360  /** Helper method fot the generic implementation of
361  * CPointsMap::loadFromRangeScan(), to be called once per range data */
363  CPointsMapXYZI& me, const float gx, const float gy, const float gz,
365  {
366  MRPT_UNUSED_PARAM(gx);
367  MRPT_UNUSED_PARAM(gy);
368  MRPT_UNUSED_PARAM(gz);
369  }
370  /** Helper method fot the generic implementation of
371  * CPointsMap::loadFromRangeScan(), to be called after each
372  * "{x,y,z}.push_back(...);" */
374  CPointsMapXYZI& me,
376  {
377  float pI = 1.0f;
378  me.m_intensity.push_back(pI);
379  }
380 
381  /** Helper method fot the generic implementation of
382  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
383  * points - this is the place to reserve memory in lric for extra working
384  * variables. */
386  CPointsMapXYZI& me,
388  {
389  // Not used.
390  }
391 
392  /** Helper method fot the generic implementation of
393  * CPointsMap::loadFromRangeScan(), to be called once per range data */
395  CPointsMapXYZI& me, const float gx, const float gy, const float gz,
397  {
398  MRPT_UNUSED_PARAM(gx);
399  MRPT_UNUSED_PARAM(gy);
400  MRPT_UNUSED_PARAM(gz);
401  }
402 
403  /** Helper method fot the generic implementation of
404  * CPointsMap::loadFromRangeScan(), to be called after each
405  * "{x,y,z}.push_back(...);" */
407  CPointsMapXYZI& me,
409  {
410  const float pI = 1.0f;
411  me.m_intensity.push_back(pI);
412  }
413 
414  /** Helper method fot the generic implementation of
415  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
416  * end */
418  CPointsMapXYZI& me,
420  {
421  }
422 };
423 } // namespace mrpt::maps::detail
424 /** See CPointsMap::loadFromRangeScan() */
425 void CPointsMapXYZI::loadFromRangeScan(
426  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
427 {
429  CPointsMapXYZI>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
430 }
431 
432 /** See CPointsMap::loadFromRangeScan() */
433 void CPointsMapXYZI::loadFromRangeScan(
434  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
435 {
437  CPointsMapXYZI>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
438 }
439 
440 // ====PLY files import & export virtual methods
441 void CPointsMapXYZI::PLY_import_set_vertex_count(const size_t N)
442 {
443  this->setSize(N);
444 }
445 
446 void CPointsMapXYZI::PLY_import_set_vertex(
447  const size_t idx, const mrpt::math::TPoint3Df& pt, const TColorf* pt_color)
448 {
449  if (pt_color)
450  this->setPointRGB(
451  idx, pt.x, pt.y, pt.z, pt_color->R, pt_color->G, pt_color->B);
452  else
453  this->setPoint(idx, pt.x, pt.y, pt.z);
454 }
455 
456 void CPointsMapXYZI::PLY_export_get_vertex(
457  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
458  TColorf& pt_color) const
459 {
460  pt_has_color = true;
461 
462  pt.x = m_x[idx];
463  pt.y = m_y[idx];
464  pt.z = m_z[idx];
465  pt_color.R = pt_color.G = pt_color.B = m_intensity[idx];
466 }
467 
468 bool CPointsMapXYZI::loadFromKittiVelodyneFile(const std::string& filename)
469 {
470  try
471  {
474  mrpt::io::CStream* f = nullptr;
475 
476  if (std::string("gz") == mrpt::system::extractFileExtension(filename))
477  {
478  if (f_gz.open(filename)) f = &f_gz;
479  }
480  else
481  {
482  if (f_normal.open(filename)) f = &f_normal;
483  }
484  if (!f)
486  "Could not open thefile: `%s`", filename.c_str());
487 
488  this->clear();
489  this->reserve(10000);
490 
491  for (;;)
492  {
493  constexpr std::size_t nToRead = sizeof(float) * 4;
494  float xyzi[4];
495  std::size_t nRead = f->Read(&xyzi, nToRead);
496  if (nRead == 0)
497  break; // EOF
498  else if (nRead == nToRead)
499  {
500  m_x.push_back(xyzi[0]);
501  m_y.push_back(xyzi[1]);
502  m_z.push_back(xyzi[2]);
503  m_intensity.push_back(xyzi[3]);
504  }
505  else
506  throw std::runtime_error(
507  "Unexpected EOF at the middle of a XYZI record "
508  "(truncated or corrupted file?)");
509  }
510  this->mark_as_modified();
511  return true;
512  }
513  catch (const std::exception& e)
514  {
515  std::cerr << "[loadFromKittiVelodyneFile] " << e.what() << std::endl;
516  return false;
517  }
518 }
519 
520 bool CPointsMapXYZI::saveToKittiVelodyneFile(const std::string& filename) const
521 {
522  try
523  {
524  mrpt::io::CFileGZOutputStream f(filename);
525 
526  for (size_t i = 0; i < m_x.size(); i++)
527  {
528  const float xyzi[4] = {m_x[i], m_y[i], m_z[i], m_intensity[i]};
529  const auto toWrite = sizeof(float) * 4;
530  std::size_t nWr = f.Write(&xyzi, toWrite);
531  ASSERT_EQUAL_(nWr, toWrite);
532  }
533  return true;
534  }
535  catch (const std::exception& e)
536  {
537  std::cerr << "[saveToKittiVelodyneFile] " << e.what() << std::endl;
538  return false;
539  }
540 }
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
#define MRPT_START
Definition: exceptions.h:241
GLdouble GLdouble z
Definition: glext.h:3879
static Ptr Create(Args &&... args)
bool open(const std::string &fileName)
Open a file for reading.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
mrpt::maps::CPointsMap::TLikelihoodOptions likelihoodOpts
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
const double G
mrpt::aligned_std_vector< float > m_intensity
The intensity/reflectance data.
virtual size_t Read(void *Buffer, size_t Count)=0
Introduces a pure virtual method responsible for reading from the stream.
GLenum GLsizei n
Definition: glext.h:5136
static void internal_loadFromRangeScan3D_prepareOneRange(CPointsMapXYZI &me, const float gx, const float gy, const float gz, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
void WriteBufferFixEndianness(const T *ptr, size_t ElementCount)
Writes a sequence of elemental datatypes, taking care of reordering their bytes from the running arch...
Definition: CArchive.h:133
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
STL namespace.
static void internal_loadFromRangeScan3D_postOneRange(CPointsMapXYZI &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
static void internal_loadFromRangeScan3D_init(CPointsMapXYZI &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: io/CStream.h:28
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
bool open(const std::string &fileName, mrpt::optional_ref< std::string > error_msg=std::nullopt)
Opens the file for read.
static void internal_loadFromRangeScan2D_init(CPointsMapXYZI &me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
This CStream derived class allow using a file as a read-only, binary stream.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:65
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
Lightweight 3D point (float version).
Definition: TPoint3D.h:21
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
GLuint index
Definition: glext.h:4068
This namespace contains representation of robot actions and observations.
static void internal_loadFromRangeScan2D_postPushBack(CPointsMapXYZI &me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
std::string extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:98
static void internal_loadFromRangeScan3D_postPushBack(CPointsMapXYZI &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
GLsizei const GLchar ** string
Definition: glext.h:4116
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:410
A map of 3D points with reflectance/intensity (float).
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
void vector_strong_clear(VECTOR_T &v)
Like calling a std::vector<>&#39;s clear() method, but really forcing deallocating the memory...
Definition: bits_mem.h:18
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
#define MRPT_END
Definition: exceptions.h:245
A RGB color - floats in the range [0,1].
Definition: TColor.h:77
size_t Write(const void *Buffer, size_t Count) override
Introduces a pure virtual method responsible for writing to the stream.
GLuint in
Definition: glext.h:7391
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
Definition: CPointsMap.h:95
GLenum GLint GLint y
Definition: glext.h:3542
Transparently opens a compressed "gz" file and reads uncompressed data from it.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
static void internal_loadFromRangeScan2D_prepareOneRange(CPointsMapXYZI &me, const float gx, const float gy, const float gz, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
GLsizeiptr size
Definition: glext.h:3934
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
GLenum GLint x
Definition: glext.h:3542
Saves data to a file and transparently compress the data using the given compression level...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:422
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:182
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
Definition: CPointsMap.h:77
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: fdd0b82cd Wed Oct 16 15:54:23 2019 +0200 at miƩ oct 16 16:00:10 CEST 2019