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