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-2020, 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, [[maybe_unused]] const float gx,
327  [[maybe_unused]] const float gy, [[maybe_unused]] const float gz,
329  {
330  }
331  /** Helper method fot the generic implementation of
332  * CPointsMap::loadFromRangeScan(), to be called after each
333  * "{x,y,z}.push_back(...);" */
335  CPointsMapXYZI& me,
337  {
338  float pI = 1.0f;
339  me.m_intensity.push_back(pI);
340  }
341 
342  /** Helper method fot the generic implementation of
343  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
344  * points - this is the place to reserve memory in lric for extra working
345  * variables. */
347  CPointsMapXYZI& me,
349  {
350  // Not used.
351  }
352 
353  /** Helper method fot the generic implementation of
354  * CPointsMap::loadFromRangeScan(), to be called once per range data */
356  CPointsMapXYZI& me, [[maybe_unused]] const float gx,
357  [[maybe_unused]] const float gy, [[maybe_unused]] const float gz,
359  {
360  }
361 
362  /** Helper method fot the generic implementation of
363  * CPointsMap::loadFromRangeScan(), to be called after each
364  * "{x,y,z}.push_back(...);" */
366  CPointsMapXYZI& me,
368  {
369  const float pI = 1.0f;
370  me.m_intensity.push_back(pI);
371  }
372 
373  /** Helper method fot the generic implementation of
374  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
375  * end */
377  CPointsMapXYZI& me,
379  {
380  }
381 };
382 } // namespace mrpt::maps::detail
383 /** See CPointsMap::loadFromRangeScan() */
384 void CPointsMapXYZI::loadFromRangeScan(
385  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
386 {
388  CPointsMapXYZI>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
389 }
390 
391 /** See CPointsMap::loadFromRangeScan() */
392 void CPointsMapXYZI::loadFromRangeScan(
393  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
394 {
396  CPointsMapXYZI>::templ_loadFromRangeScan(*this, rangeScan, robotPose);
397 }
398 
399 // ====PLY files import & export virtual methods
400 void CPointsMapXYZI::PLY_import_set_vertex_count(const size_t N)
401 {
402  this->setSize(N);
403 }
404 
405 void CPointsMapXYZI::PLY_import_set_vertex(
406  const size_t idx, const mrpt::math::TPoint3Df& pt, const TColorf* pt_color)
407 {
408  if (pt_color)
409  this->setPointRGB(
410  idx, pt.x, pt.y, pt.z, pt_color->R, pt_color->G, pt_color->B);
411  else
412  this->setPoint(idx, pt.x, pt.y, pt.z);
413 }
414 
415 void CPointsMapXYZI::PLY_export_get_vertex(
416  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
417  TColorf& pt_color) const
418 {
419  pt_has_color = true;
420 
421  pt.x = m_x[idx];
422  pt.y = m_y[idx];
423  pt.z = m_z[idx];
424  pt_color.R = pt_color.G = pt_color.B = m_intensity[idx];
425 }
426 
427 bool CPointsMapXYZI::loadFromKittiVelodyneFile(const std::string& filename)
428 {
429  try
430  {
433  mrpt::io::CStream* f = nullptr;
434 
435  if (std::string("gz") == mrpt::system::extractFileExtension(filename))
436  {
437  if (f_gz.open(filename)) f = &f_gz;
438  }
439  else
440  {
441  if (f_normal.open(filename)) f = &f_normal;
442  }
443  if (!f)
445  "Could not open thefile: `%s`", filename.c_str());
446 
447  this->clear();
448  this->reserve(10000);
449 
450  for (;;)
451  {
452  constexpr std::size_t nToRead = sizeof(float) * 4;
453  float xyzi[4];
454  std::size_t nRead = f->Read(&xyzi, nToRead);
455  if (nRead == 0)
456  break; // EOF
457  else if (nRead == nToRead)
458  {
459  m_x.push_back(xyzi[0]);
460  m_y.push_back(xyzi[1]);
461  m_z.push_back(xyzi[2]);
462  m_intensity.push_back(xyzi[3]);
463  }
464  else
465  throw std::runtime_error(
466  "Unexpected EOF at the middle of a XYZI record "
467  "(truncated or corrupted file?)");
468  }
469  this->mark_as_modified();
470  return true;
471  }
472  catch (const std::exception& e)
473  {
474  std::cerr << "[loadFromKittiVelodyneFile] " << e.what() << std::endl;
475  return false;
476  }
477 }
478 
479 bool CPointsMapXYZI::saveToKittiVelodyneFile(const std::string& filename) const
480 {
481  try
482  {
483  mrpt::io::CFileGZOutputStream f(filename);
484 
485  for (size_t i = 0; i < m_x.size(); i++)
486  {
487  const float xyzi[4] = {m_x[i], m_y[i], m_z[i], m_intensity[i]};
488  const auto toWrite = sizeof(float) * 4;
489  std::size_t nWr = f.Write(&xyzi, toWrite);
490  ASSERT_EQUAL_(nWr, toWrite);
491  }
492  return true;
493  }
494  catch (const std::exception& e)
495  {
496  std::cerr << "[saveToKittiVelodyneFile] " << e.what() << std::endl;
497  return false;
498  }
499 }
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
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.
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
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...
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.
static void internal_loadFromRangeScan2D_prepareOneRange(CPointsMapXYZI &me, [[maybe_unused]] const float gx, [[maybe_unused]] const float gy, [[maybe_unused]] const float gz, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
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...
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:408
A map of 3D points with reflectance/intensity (float).
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
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
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
size_t Write(const void *Buffer, size_t Count) override
Introduces a pure virtual method responsible for writing to the stream.
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
Definition: CPointsMap.h:95
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
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
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 ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
Definition: CArchive.h:94
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:440
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.
static void internal_loadFromRangeScan3D_prepareOneRange(CPointsMapXYZI &me, [[maybe_unused]] const float gx, [[maybe_unused]] const float gy, [[maybe_unused]] const float gz, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020