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  "mrpt::maps::CPointsMapXYZI", mrpt::maps::CPointsMapXYZI)
39 
40 CPointsMapXYZI::TMapDefinition::TMapDefinition() = default;
41 
42 void CPointsMapXYZI::TMapDefinition::loadFromConfigFile_map_specific(
43  const CConfigFileBase& source, const std::string& sectionNamePrefix)
44 {
45  insertionOpts.loadFromConfigFile(
46  source, sectionNamePrefix + string("_insertOpts"));
47  likelihoodOpts.loadFromConfigFile(
48  source, sectionNamePrefix + string("_likelihoodOpts"));
49 }
50 
51 void CPointsMapXYZI::TMapDefinition::dumpToTextStream_map_specific(
52  std::ostream& out) const
53 {
54  this->insertionOpts.dumpToTextStream(out);
55  this->likelihoodOpts.dumpToTextStream(out);
56 }
57 
58 mrpt::maps::CMetricMap* CPointsMapXYZI::internal_CreateFromMapDefinition(
60 {
62  *dynamic_cast<const CPointsMapXYZI::TMapDefinition*>(&_def);
63  auto* obj = new CPointsMapXYZI();
64  obj->insertionOptions = def.insertionOpts;
65  obj->likelihoodOptions = def.likelihoodOpts;
66  return obj;
67 }
68 // =========== End of Map definition Block =========
69 
71 
72 void CPointsMapXYZI::reserve(size_t newLength)
73 {
74  m_x.reserve(newLength);
75  m_y.reserve(newLength);
76  m_z.reserve(newLength);
77  m_intensity.reserve(newLength);
78 }
79 
80 // Resizes all point buffers so they can hold the given number of points: newly
81 // created points are set to default values,
82 // and old contents are not changed.
83 void CPointsMapXYZI::resize(size_t newLength)
84 {
85  m_x.resize(newLength, 0);
86  m_y.resize(newLength, 0);
87  m_z.resize(newLength, 0);
88  m_intensity.resize(newLength, 1);
89  mark_as_modified();
90 }
91 
92 // Resizes all point buffers so they can hold the given number of points,
93 // *erasing* all previous contents
94 // and leaving all points to default values.
95 void CPointsMapXYZI::setSize(size_t newLength)
96 {
97  m_x.assign(newLength, 0);
98  m_y.assign(newLength, 0);
99  m_z.assign(newLength, 0);
100  m_intensity.assign(newLength, 0);
101  mark_as_modified();
102 }
103 
104 void CPointsMapXYZI::impl_copyFrom(const CPointsMap& obj)
105 {
106  // This also does a ::resize(N) of all data fields.
107  CPointsMap::base_copyFrom(obj);
108 
109  const auto* pXYZI = dynamic_cast<const CPointsMapXYZI*>(&obj);
110  if (pXYZI) m_intensity = pXYZI->m_intensity;
111 }
112 
113 uint8_t CPointsMapXYZI::serializeGetVersion() const { return 0; }
114 void CPointsMapXYZI::serializeTo(mrpt::serialization::CArchive& out) const
115 {
116  uint32_t n = m_x.size();
117 
118  // First, write the number of points:
119  out << n;
120 
121  if (n > 0)
122  {
123  out.WriteBufferFixEndianness(&m_x[0], n);
124  out.WriteBufferFixEndianness(&m_y[0], n);
125  out.WriteBufferFixEndianness(&m_z[0], n);
126  out.WriteBufferFixEndianness(&m_intensity[0], n);
127  }
128  insertionOptions.writeToStream(out);
129  likelihoodOptions.writeToStream(out);
130 }
131 
132 void CPointsMapXYZI::serializeFrom(
133  mrpt::serialization::CArchive& in, uint8_t version)
134 {
135  switch (version)
136  {
137  case 0:
138  {
139  mark_as_modified();
140 
141  // Read the number of points:
142  uint32_t n;
143  in >> n;
144  this->resize(n);
145  if (n > 0)
146  {
147  in.ReadBufferFixEndianness(&m_x[0], n);
148  in.ReadBufferFixEndianness(&m_y[0], n);
149  in.ReadBufferFixEndianness(&m_z[0], n);
150  in.ReadBufferFixEndianness(&m_intensity[0], n);
151  }
152  insertionOptions.readFromStream(in);
153  likelihoodOptions.readFromStream(in);
154  }
155  break;
156  default:
158  };
159 }
160 
161 void CPointsMapXYZI::internal_clear()
162 {
163  vector_strong_clear(m_x);
164  vector_strong_clear(m_y);
165  vector_strong_clear(m_z);
166  vector_strong_clear(m_intensity);
167  mark_as_modified();
168 }
169 
170 void CPointsMapXYZI::setPointRGB(
171  size_t index, float x, float y, float z, float R, float G, float B)
172 {
173  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
174  m_x[index] = x;
175  m_y[index] = y;
176  m_z[index] = z;
177  m_intensity[index] = R;
178  mark_as_modified();
179 }
180 
181 void CPointsMapXYZI::setPointIntensity(size_t index, float I)
182 {
183  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
184  this->m_intensity[index] = I;
185  // mark_as_modified(); // No need to rebuild KD-trees, etc...
186 }
187 
188 void CPointsMapXYZI::insertPointFast(float x, float y, float z)
189 {
190  m_x.push_back(x);
191  m_y.push_back(y);
192  m_z.push_back(z);
193  m_intensity.push_back(0);
194  // mark_as_modified(); -> Fast
195 }
196 
197 void CPointsMapXYZI::insertPointRGB(
198  float x, float y, float z, float R_intensity, float G_ignored,
199  float B_ignored)
200 {
201  m_x.push_back(x);
202  m_y.push_back(y);
203  m_z.push_back(z);
204  m_intensity.push_back(R_intensity);
205  mark_as_modified();
206 }
207 
208 void CPointsMapXYZI::getAs3DObject(
209  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
210 {
211  ASSERT_(outObj);
212 
213  if (!genericMapParams.enableSaveAs3DObject) return;
214 
216 
217  obj->loadFromPointsMap(this);
218  obj->setColor(1, 1, 1, 1.0);
219  obj->setPointSize(this->renderOptions.point_size);
220 
221  outObj->insert(obj);
222 }
223 
224 void CPointsMapXYZI::getPointRGB(
225  size_t index, float& x, float& y, float& z, float& R, float& G,
226  float& B) const
227 {
228  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
229 
230  x = m_x[index];
231  y = m_y[index];
232  z = m_z[index];
233  R = G = B = m_intensity[index];
234 }
235 
236 float CPointsMapXYZI::getPointIntensity(size_t index) const
237 {
238  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
239  return m_intensity[index];
240 }
241 
242 bool CPointsMapXYZI::saveXYZI_to_text_file(const std::string& file) const
243 {
244  FILE* f = os::fopen(file.c_str(), "wt");
245  if (!f) return false;
246  for (unsigned int i = 0; i < m_x.size(); i++)
247  os::fprintf(f, "%f %f %f %f\n", m_x[i], m_y[i], m_z[i], m_intensity[i]);
248  os::fclose(f);
249  return true;
250 }
251 
252 bool CPointsMapXYZI::loadXYZI_from_text_file(const std::string& file)
253 {
254  MRPT_START
255 
256  // Clear current map:
257  mark_as_modified();
258  this->clear();
259 
260  std::ifstream f;
261  f.open(file);
262  if (!f.is_open()) return false;
263 
264  while (!f.eof())
265  {
266  std::string line;
267  std::getline(f, line);
268 
269  std::stringstream ss(line);
270 
271  float x, y, z, i;
272  if (!(ss >> x >> y >> z >> i))
273  {
274  break;
275  }
276 
277  insertPointFast(x, y, z);
278  m_intensity.push_back(i);
279  }
280 
281  return true;
282 
283  MRPT_END
284 }
285 
286 /*---------------------------------------------------------------
287 addFrom_classSpecific
288 ---------------------------------------------------------------*/
289 void CPointsMapXYZI::addFrom_classSpecific(
290  const CPointsMap& anotherMap, const size_t nPreviousPoints)
291 {
292  const size_t nOther = anotherMap.size();
293 
294  // Specific data for this class:
295  const auto* anotheMap_col =
296  dynamic_cast<const CPointsMapXYZI*>(&anotherMap);
297 
298  if (anotheMap_col)
299  {
300  for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
301  m_intensity[j] = anotheMap_col->m_intensity[i];
302  }
303 }
304 
305 namespace mrpt::maps::detail
306 {
308 
309 template <>
311 {
312  /** Helper method fot the generic implementation of
313  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
314  * points - this is the place to reserve memory in lric for extra working
315  * variables. */
317  CPointsMapXYZI& me,
319  {
320  // lric.fVars: not needed
321  }
322 
323  /** Helper method fot the generic implementation of
324  * CPointsMap::loadFromRangeScan(), to be called once per range data */
326  CPointsMapXYZI& me, const float gx, const float gy, const float gz,
328  {
329  MRPT_UNUSED_PARAM(gx);
330  MRPT_UNUSED_PARAM(gy);
331  MRPT_UNUSED_PARAM(gz);
332  }
333  /** Helper method fot the generic implementation of
334  * CPointsMap::loadFromRangeScan(), to be called after each
335  * "{x,y,z}.push_back(...);" */
337  CPointsMapXYZI& me,
339  {
340  float pI = 1.0f;
341  me.m_intensity.push_back(pI);
342  }
343 
344  /** Helper method fot the generic implementation of
345  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
346  * points - this is the place to reserve memory in lric for extra working
347  * variables. */
349  CPointsMapXYZI& me,
351  {
352  // Not used.
353  }
354 
355  /** Helper method fot the generic implementation of
356  * CPointsMap::loadFromRangeScan(), to be called once per range data */
358  CPointsMapXYZI& me, const float gx, const float gy, const float gz,
360  {
361  MRPT_UNUSED_PARAM(gx);
362  MRPT_UNUSED_PARAM(gy);
363  MRPT_UNUSED_PARAM(gz);
364  }
365 
366  /** Helper method fot the generic implementation of
367  * CPointsMap::loadFromRangeScan(), to be called after each
368  * "{x,y,z}.push_back(...);" */
370  CPointsMapXYZI& me,
372  {
373  const float pI = 1.0f;
374  me.m_intensity.push_back(pI);
375  }
376 
377  /** Helper method fot the generic implementation of
378  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
379  * end */
381  CPointsMapXYZI& me,
383  {
384  }
385 };
386 } // namespace mrpt::maps::detail
387 /** See CPointsMap::loadFromRangeScan() */
388 void CPointsMapXYZI::loadFromRangeScan(
389  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
390 {
392  CPointsMapXYZI>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
393 }
394 
395 /** See CPointsMap::loadFromRangeScan() */
396 void CPointsMapXYZI::loadFromRangeScan(
397  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
398 {
400  CPointsMapXYZI>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
401 }
402 
403 // ====PLY files import & export virtual methods
404 void CPointsMapXYZI::PLY_import_set_vertex_count(const size_t N)
405 {
406  this->setSize(N);
407 }
408 
409 void CPointsMapXYZI::PLY_import_set_vertex(
410  const size_t idx, const mrpt::math::TPoint3Df& pt, const TColorf* pt_color)
411 {
412  if (pt_color)
413  this->setPointRGB(
414  idx, pt.x, pt.y, pt.z, pt_color->R, pt_color->G, pt_color->B);
415  else
416  this->setPoint(idx, pt.x, pt.y, pt.z);
417 }
418 
419 void CPointsMapXYZI::PLY_export_get_vertex(
420  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
421  TColorf& pt_color) const
422 {
423  pt_has_color = true;
424 
425  pt.x = m_x[idx];
426  pt.y = m_y[idx];
427  pt.z = m_z[idx];
428  pt_color.R = pt_color.G = pt_color.B = m_intensity[idx];
429 }
430 
431 bool CPointsMapXYZI::loadFromKittiVelodyneFile(const std::string& filename)
432 {
433  try
434  {
437  mrpt::io::CStream* f = nullptr;
438 
439  if (std::string("gz") == mrpt::system::extractFileExtension(filename))
440  {
441  if (f_gz.open(filename)) f = &f_gz;
442  }
443  else
444  {
445  if (f_normal.open(filename)) f = &f_normal;
446  }
447  if (!f)
449  "Could not open thefile: `%s`", filename.c_str());
450 
451  this->clear();
452  this->reserve(10000);
453 
454  for (;;)
455  {
456  constexpr std::size_t nToRead = sizeof(float) * 4;
457  float xyzi[4];
458  std::size_t nRead = f->Read(&xyzi, nToRead);
459  if (nRead == 0)
460  break; // EOF
461  else if (nRead == nToRead)
462  {
463  m_x.push_back(xyzi[0]);
464  m_y.push_back(xyzi[1]);
465  m_z.push_back(xyzi[2]);
466  m_intensity.push_back(xyzi[3]);
467  }
468  else
469  throw std::runtime_error(
470  "Unexpected EOF at the middle of a XYZI record "
471  "(truncated or corrupted file?)");
472  }
473  this->mark_as_modified();
474  return true;
475  }
476  catch (const std::exception& e)
477  {
478  std::cerr << "[loadFromKittiVelodyneFile] " << e.what() << std::endl;
479  return false;
480  }
481 }
482 
483 bool CPointsMapXYZI::saveToKittiVelodyneFile(const std::string& filename) const
484 {
485  try
486  {
487  mrpt::io::CFileGZOutputStream f(filename);
488 
489  for (size_t i = 0; i < m_x.size(); i++)
490  {
491  const float xyzi[4] = {m_x[i], m_y[i], m_z[i], m_intensity[i]};
492  const auto toWrite = sizeof(float) * 4;
493  std::size_t nWr = f.Write(&xyzi, toWrite);
494  ASSERT_EQUAL_(nWr, toWrite);
495  }
496  return true;
497  }
498  catch (const std::exception& e)
499  {
500  std::cerr << "[saveToKittiVelodyneFile] " << e.what() << std::endl;
501  return false;
502  }
503 }
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...
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:85
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245
A RGB color - floats in the range [0,1].
Definition: TColor.h:78
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
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
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...
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
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:441
images resize(NUM_IMGS)
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
Definition: CPointsMap.h:77
void reserve(size_t newLength) override
Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
#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: cb8dd5fc8 Sat Dec 7 21:55:39 2019 +0100 at sáb dic 7 22:00:13 CET 2019