MRPT  1.9.9
CColouredPointsMap.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>
13 #include <mrpt/img/color_maps.h>
19 #include <mrpt/system/os.h>
20 
21 #include "CPointsMap_crtp_common.h"
22 
23 using namespace std;
24 using namespace mrpt;
25 using namespace mrpt::maps;
26 using namespace mrpt::obs;
27 using namespace mrpt::img;
28 using namespace mrpt::poses;
29 using namespace mrpt::system;
30 using namespace mrpt::math;
31 using namespace mrpt::config;
32 
33 // =========== Begin of Map definition ============
35  "CColouredPointsMap,colourPointsMap", mrpt::maps::CColouredPointsMap)
36 
37 CColouredPointsMap::TMapDefinition::TMapDefinition() = default;
38 void CColouredPointsMap::TMapDefinition::loadFromConfigFile_map_specific(
39  const CConfigFileBase& source, const std::string& sectionNamePrefix)
40 {
41  insertionOpts.loadFromConfigFile(
42  source, sectionNamePrefix + string("_insertOpts"));
43  likelihoodOpts.loadFromConfigFile(
44  source, sectionNamePrefix + string("_likelihoodOpts"));
45  colourOpts.loadFromConfigFile(
46  source, sectionNamePrefix + string("_colorOpts"));
47 }
48 
49 void CColouredPointsMap::TMapDefinition::dumpToTextStream_map_specific(
50  std::ostream& out) const
51 {
52  this->insertionOpts.dumpToTextStream(out);
53  this->likelihoodOpts.dumpToTextStream(out);
54  this->colourOpts.dumpToTextStream(out);
55 }
56 
57 mrpt::maps::CMetricMap* CColouredPointsMap::internal_CreateFromMapDefinition(
59 {
61  *dynamic_cast<const CColouredPointsMap::TMapDefinition*>(&_def);
62  auto* obj = new CColouredPointsMap();
63  obj->insertionOptions = def.insertionOpts;
64  obj->likelihoodOptions = def.likelihoodOpts;
65  obj->colorScheme = def.colourOpts;
66  return obj;
67 }
68 // =========== End of Map definition Block =========
69 
71 
72 #if MRPT_HAS_PCL
73 #include <pcl/io/pcd_io.h>
74 #include <pcl/point_types.h>
75 //# include <pcl/registration/icp.h>
76 #endif
77 
78 void CColouredPointsMap::reserve(size_t newLength)
79 {
80  m_x.reserve(newLength);
81  m_y.reserve(newLength);
82  m_z.reserve(newLength);
83  m_color_R.reserve(newLength);
84  m_color_G.reserve(newLength);
85  m_color_B.reserve(newLength);
86 }
87 
88 // Resizes all point buffers so they can hold the given number of points: newly
89 // created points are set to default values,
90 // and old contents are not changed.
91 void CColouredPointsMap::resize(size_t newLength)
92 {
93  m_x.resize(newLength, 0);
94  m_y.resize(newLength, 0);
95  m_z.resize(newLength, 0);
96  m_color_R.resize(newLength, 1);
97  m_color_G.resize(newLength, 1);
98  m_color_B.resize(newLength, 1);
99  mark_as_modified();
100 }
101 
102 // Resizes all point buffers so they can hold the given number of points,
103 // *erasing* all previous contents
104 // and leaving all points to default values.
105 void CColouredPointsMap::setSize(size_t newLength)
106 {
107  m_x.assign(newLength, 0);
108  m_y.assign(newLength, 0);
109  m_z.assign(newLength, 0);
110  m_color_R.assign(newLength, 1);
111  m_color_G.assign(newLength, 1);
112  m_color_B.assign(newLength, 1);
113  mark_as_modified();
114 }
115 
116 void CColouredPointsMap::impl_copyFrom(const CPointsMap& obj)
117 {
118  // This also does a ::resize(N) of all data fields.
119  CPointsMap::base_copyFrom(obj);
120 
121  const auto* pCol = dynamic_cast<const CColouredPointsMap*>(&obj);
122  if (pCol)
123  {
124  m_color_R = pCol->m_color_R;
125  m_color_G = pCol->m_color_G;
126  m_color_B = pCol->m_color_B;
127  }
128 }
129 
130 uint8_t CColouredPointsMap::serializeGetVersion() const { return 9; }
131 void CColouredPointsMap::serializeTo(mrpt::serialization::CArchive& out) const
132 {
133  uint32_t n = m_x.size();
134 
135  // First, write the number of points:
136  out << n;
137 
138  if (n > 0)
139  {
140  out.WriteBufferFixEndianness(&m_x[0], n);
141  out.WriteBufferFixEndianness(&m_y[0], n);
142  out.WriteBufferFixEndianness(&m_z[0], n);
143  }
144  out << m_color_R << m_color_G << m_color_B; // added in v4
145 
146  out << genericMapParams; // v9
147  insertionOptions.writeToStream(
148  out); // version 9?: insert options are saved with its own method
149  likelihoodOptions.writeToStream(out); // Added in version 5
150 }
151 
152 void CColouredPointsMap::serializeFrom(
154 {
155  switch (version)
156  {
157  case 8:
158  case 9:
159  {
160  mark_as_modified();
161 
162  // Read the number of points:
163  uint32_t n;
164  in >> n;
165 
166  this->resize(n);
167 
168  if (n > 0)
169  {
170  in.ReadBufferFixEndianness(&m_x[0], n);
171  in.ReadBufferFixEndianness(&m_y[0], n);
172  in.ReadBufferFixEndianness(&m_z[0], n);
173  }
174  in >> m_color_R >> m_color_G >> m_color_B;
175 
176  if (version >= 9)
177  in >> genericMapParams;
178  else
179  {
180  bool disableSaveAs3DObject;
181  in >> disableSaveAs3DObject;
182  genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
183  }
184  insertionOptions.readFromStream(in);
185  likelihoodOptions.readFromStream(in);
186  }
187  break;
188 
189  case 0:
190  case 1:
191  case 2:
192  case 3:
193  case 4:
194  case 5:
195  case 6:
196  case 7:
197  {
198  mark_as_modified();
199 
200  // Read the number of points:
201  uint32_t n;
202  in >> n;
203 
204  this->resize(n);
205 
206  if (n > 0)
207  {
208  in.ReadBufferFixEndianness(&m_x[0], n);
209  in.ReadBufferFixEndianness(&m_y[0], n);
210  in.ReadBufferFixEndianness(&m_z[0], n);
211 
212  // Version 1: weights are also stored:
213  // Version 4: Type becomes long int -> uint32_t for
214  // portability!!
215  if (version >= 1)
216  {
217  if (version >= 4)
218  {
219  if (version >= 7)
220  {
221  // Weights were removed from this class in v7 (MRPT
222  // 0.9.5),
223  // so nothing else to do.
224  }
225  else
226  {
227  // Go on with old serialization format, but discard
228  // weights:
229  std::vector<uint32_t> dummy_pointWeight(n);
230  in.ReadBufferFixEndianness(
231  &dummy_pointWeight[0], n);
232  }
233  }
234  else
235  {
236  std::vector<uint32_t> dummy_pointWeight(n);
237  in.ReadBufferFixEndianness(&dummy_pointWeight[0], n);
238  }
239  }
240  }
241 
242  if (version >= 2)
243  {
244  // version 2: options saved too
245  in >> insertionOptions.minDistBetweenLaserPoints >>
246  insertionOptions.addToExistingPointsMap >>
247  insertionOptions.also_interpolate >>
248  insertionOptions.disableDeletion >>
249  insertionOptions.fuseWithExisting >>
250  insertionOptions.isPlanarMap;
251 
252  if (version < 6)
253  {
254  bool old_matchStaticPointsOnly;
255  in >> old_matchStaticPointsOnly;
256  }
257 
258  in >> insertionOptions.maxDistForInterpolatePoints;
259  {
260  bool disableSaveAs3DObject;
261  in >> disableSaveAs3DObject;
262  genericMapParams.enableSaveAs3DObject =
263  !disableSaveAs3DObject;
264  }
265  }
266 
267  if (version >= 3)
268  {
269  in >> insertionOptions.horizontalTolerance;
270  }
271 
272  if (version >= 4) // Color data
273  {
274  in >> m_color_R >> m_color_G >> m_color_B;
275  if (version >= 7)
276  {
277  // Removed: in >> m_min_dist;
278  }
279  else
280  {
281  std::vector<float> dummy_dist;
282  in >> dummy_dist;
283  }
284  }
285  else
286  {
287  m_color_R.assign(m_x.size(), 1.0f);
288  m_color_G.assign(m_x.size(), 1.0f);
289  m_color_B.assign(m_x.size(), 1.0f);
290  // m_min_dist.assign(x.size(),2000.0f);
291  }
292 
293  if (version >= 5) // version 5: added likelihoodOptions
294  likelihoodOptions.readFromStream(in);
295 
296  if (version >= 8) // version 8: added insertInvalidPoints
297  in >> insertionOptions.insertInvalidPoints;
298  }
299  break;
300  default:
302  };
303 }
304 
305 /*---------------------------------------------------------------
306 Clear
307 ---------------------------------------------------------------*/
308 void CColouredPointsMap::internal_clear()
309 {
310  // This swap() thing is the only way to really deallocate the memory.
311  vector_strong_clear(m_x);
312  vector_strong_clear(m_y);
313  vector_strong_clear(m_z);
314 
315  vector_strong_clear(m_color_R);
316  vector_strong_clear(m_color_G);
317  vector_strong_clear(m_color_B);
318 
319  mark_as_modified();
320 }
321 
322 /** Changes a given point from map. First index is 0.
323  * \exception Throws std::exception on index out of bound.
324  */
325 void CColouredPointsMap::setPointRGB(
326  size_t index, float x, float y, float z, float R, float G, float B)
327 {
328  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
329  m_x[index] = x;
330  m_y[index] = y;
331  m_z[index] = z;
332  this->m_color_R[index] = R;
333  this->m_color_G[index] = G;
334  this->m_color_B[index] = B;
335  mark_as_modified();
336 }
337 
338 /** Changes just the color of a given point from the map. First index is 0.
339  * \exception Throws std::exception on index out of bound.
340  */
341 void CColouredPointsMap::setPointColor(size_t index, float R, float G, float B)
342 {
343  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
344  this->m_color_R[index] = R;
345  this->m_color_G[index] = G;
346  this->m_color_B[index] = B;
347  // mark_as_modified(); // No need to rebuild KD-trees, etc...
348 }
349 
350 void CColouredPointsMap::insertPointFast(float x, float y, float z)
351 {
352  m_x.push_back(x);
353  m_y.push_back(y);
354  m_z.push_back(z);
355  m_color_R.push_back(1);
356  m_color_G.push_back(1);
357  m_color_B.push_back(1);
358 
359  // mark_as_modified(); -> Fast
360 }
361 
362 void CColouredPointsMap::insertPointRGB(
363  float x, float y, float z, float R, float G, float B)
364 {
365  m_x.push_back(x);
366  m_y.push_back(y);
367  m_z.push_back(z);
368  m_color_R.push_back(R);
369  m_color_G.push_back(G);
370  m_color_B.push_back(B);
371 
372  mark_as_modified();
373 }
374 
375 /*---------------------------------------------------------------
376 getAs3DObject
377 ---------------------------------------------------------------*/
378 void CColouredPointsMap::getAs3DObject(
379  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
380 {
381  ASSERT_(outObj);
382 
383  if (!genericMapParams.enableSaveAs3DObject) return;
384 
386  std::make_shared<opengl::CPointCloudColoured>();
387 
388  obj->loadFromPointsMap(this);
389  obj->setColor(1, 1, 1, 1.0);
390 
391  obj->setPointSize(this->renderOptions.point_size);
392 
393  outObj->insert(obj);
394 }
395 
396 /*---------------------------------------------------------------
397 TColourOptions
398 ---------------------------------------------------------------*/
399 CColouredPointsMap::TColourOptions::TColourOptions()
400 
401  = default;
402 
403 void CColouredPointsMap::TColourOptions::loadFromConfigFile(
404  const CConfigFileBase& source, const std::string& section)
405 {
406  scheme = source.read_enum(section, "scheme", scheme);
407  MRPT_LOAD_CONFIG_VAR(z_min, float, source, section)
408  MRPT_LOAD_CONFIG_VAR(z_max, float, source, section)
409  MRPT_LOAD_CONFIG_VAR(d_max, float, source, section)
410 }
411 
412 void CColouredPointsMap::TColourOptions::dumpToTextStream(
413  std::ostream& out) const
414 {
415  out << "\n----------- [CColouredPointsMap::TColourOptions] ------------ "
416  "\n\n";
417 
418  out << "scheme = " << scheme << endl;
419  out << "z_min = " << z_min << endl;
420  out << "z_max = " << z_max << endl;
421  out << "d_max = " << d_max << endl;
422 }
423 
424 void CColouredPointsMap::getPointRGB(
425  size_t index, float& x, float& y, float& z, float& R, float& G,
426  float& B) const
427 {
428  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
429 
430  x = m_x[index];
431  y = m_y[index];
432  z = m_z[index];
433  R = m_color_R[index];
434  G = m_color_G[index];
435  B = m_color_B[index];
436 }
437 
438 /** Retrieves a point color (colors range is [0,1])
439  */
440 void CColouredPointsMap::getPointColor(
441  size_t index, float& R, float& G, float& B) const
442 {
443  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
444 
445  R = m_color_R[index];
446  G = m_color_G[index];
447  B = m_color_B[index];
448 }
449 
450 // This function is duplicated from
451 // "mrpt::vision::pinhole::projectPoint_with_distortion", to avoid
452 // a dependency on mrpt-vision.
454  const mrpt::math::TPoint3D& P, const TCamera& params, TPixelCoordf& pixel,
455  bool accept_points_behind)
456 {
457  MRPT_UNUSED_PARAM(accept_points_behind);
458  // Pinhole model:
459  const double x = P.x / P.z;
460  const double y = P.y / P.z;
461 
462  // Radial distortion:
463  const double r2 = square(x) + square(y);
464  const double r4 = square(r2);
465  const double r6 = r2 * r4;
466 
467  pixel.x = params.cx() +
468  params.fx() * (x * (1 + params.dist[0] * r2 +
469  params.dist[1] * r4 + params.dist[4] * r6) +
470  2 * params.dist[2] * x * y +
471  params.dist[3] * (r2 + 2 * square(x)));
472  pixel.y = params.cy() +
473  params.fy() * (y * (1 + params.dist[0] * r2 +
474  params.dist[1] * r4 + params.dist[4] * r6) +
475  2 * params.dist[3] * x * y +
476  params.dist[2] * (r2 + 2 * square(y)));
477 }
478 
479 /*---------------------------------------------------------------
480 getPoint
481 ---------------------------------------------------------------*/
482 bool CColouredPointsMap::colourFromObservation(
483  const CObservationImage& obs, const CPose3D& robotPose)
484 {
485  // Check if image is not grayscale
486  ASSERT_(obs.image.isColor());
487 
488  CPose3D cameraPoseR; // Get Camera Pose on the Robot(B) (CPose3D)
489  CPose3D cameraPoseW; // World Camera Pose
490 
491  obs.getSensorPose(cameraPoseR);
492  cameraPoseW = robotPose + cameraPoseR; // Camera Global Coordinates
493 
494  // Image Information
495  unsigned int imgW = obs.image.getWidth();
496  unsigned int imgH = obs.image.getHeight();
497 
498  // Projection related variables
499  std::vector<TPixelCoordf>
500  projectedPoints; // The set of projected points in the image
501  std::vector<TPixelCoordf>::iterator
502  itProPoints; // Iterator for projectedPoints
503  std::vector<size_t> p_idx;
504  std::vector<float> p_dist;
505  std::vector<unsigned int> p_proj;
506 
507  // Get the N closest points
508  kdTreeNClosestPoint2DIdx(
509  cameraPoseW.x(), cameraPoseW.y(), // query point
510  200000, // number of points to search
511  p_idx, p_dist); // indexes and distances of the returned points
512 
513  // Fill p3D vector
514  for (size_t k = 0; k < p_idx.size(); k++)
515  {
516  float d = sqrt(p_dist[k]);
517  size_t idx = p_idx[k];
518  if (d < colorScheme.d_max) // && d < m_min_dist[idx] )
519  {
520  TPixelCoordf px;
522  TPoint3D(m_x[idx], m_y[idx], m_z[idx]), obs.cameraParams, px,
523  true);
524  projectedPoints.push_back(px);
525  p_proj.push_back(k);
526  } // end if
527  } // end for
528 
529  // Get channel order
530  unsigned int chR, chG, chB;
531  if (obs.image.getChannelsOrder()[0] == 'B')
532  {
533  chR = 2;
534  chG = 1;
535  chB = 0;
536  }
537  else
538  {
539  chR = 0;
540  chG = 1;
541  chB = 2;
542  }
543 
544  unsigned int n_proj = 0;
545  const float factor = 1.0f / 255; // Normalize pixels:
546 
547  // Get the colour of the projected points
548  size_t k;
549  for (itProPoints = projectedPoints.begin(), k = 0;
550  itProPoints != projectedPoints.end(); ++itProPoints, ++k)
551  {
552  // Only get the points projected inside the image
553  if (itProPoints->x >= 0 && itProPoints->x < imgW &&
554  itProPoints->y > 0 && itProPoints->y < imgH)
555  {
556  unsigned int ii = p_idx[p_proj[k]];
557  uint8_t* p = obs.image(
558  (unsigned int)itProPoints->x, (unsigned int)itProPoints->y);
559 
560  m_color_R[ii] = p[chR] * factor; // R
561  m_color_G[ii] = p[chG] * factor; // G
562  m_color_B[ii] = p[chB] * factor; // B
563  // m_min_dist[ii] = p_dist[p_proj[k]];
564 
565  n_proj++;
566  }
567  } // end for
568 
569  return true;
570 } // end colourFromObservation
571 
572 void CColouredPointsMap::resetPointsMinDist(float defValue)
573 {
574  MRPT_UNUSED_PARAM(defValue);
575  // m_min_dist.assign(x.size(),defValue);
576 }
577 
578 bool CColouredPointsMap::save3D_and_colour_to_text_file(
579  const std::string& file) const
580 {
581  FILE* f = os::fopen(file.c_str(), "wt");
582  if (!f) return false;
583 
584  for (unsigned int i = 0; i < m_x.size(); i++)
585  os::fprintf(
586  f, "%f %f %f %d %d %d\n", m_x[i], m_y[i], m_z[i],
587  (uint8_t)(255 * m_color_R[i]), (uint8_t)(255 * m_color_G[i]),
588  (uint8_t)(255 * m_color_B[i]));
589  // os::fprintf(f,"%f %f %f %f %f %f
590  //%f\n",x[i],y[i],z[i],m_color_R[i],m_color_G[i],m_color_B[i],m_min_dist[i]);
591 
592  os::fclose(f);
593  return true;
594 }
595 
596 // ================================ PLY files import & export virtual methods
597 // ================================
598 
599 /** In a base class, reserve memory to prepare subsequent calls to
600  * PLY_import_set_vertex */
601 void CColouredPointsMap::PLY_import_set_vertex_count(const size_t N)
602 {
603  this->setSize(N);
604 }
605 
606 /** In a base class, will be called after PLY_import_set_vertex_count() once for
607  * each loaded point.
608  * \param pt_color Will be nullptr if the loaded file does not provide color
609  * info.
610  */
611 void CColouredPointsMap::PLY_import_set_vertex(
612  const size_t idx, const mrpt::math::TPoint3Df& pt, const TColorf* pt_color)
613 {
614  if (pt_color)
615  this->setPointRGB(
616  idx, pt.x, pt.y, pt.z, pt_color->R, pt_color->G, pt_color->B);
617  else
618  this->setPoint(idx, pt.x, pt.y, pt.z);
619 }
620 
621 /** In a base class, will be called after PLY_export_get_vertex_count() once for
622  * each exported point.
623  * \param pt_color Will be nullptr if the loaded file does not provide color
624  * info.
625  */
626 void CColouredPointsMap::PLY_export_get_vertex(
627  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
628  TColorf& pt_color) const
629 {
630  pt_has_color = true;
631 
632  pt.x = m_x[idx];
633  pt.y = m_y[idx];
634  pt.z = m_z[idx];
635 
636  pt_color.R = m_color_R[idx];
637  pt_color.G = m_color_G[idx];
638  pt_color.B = m_color_B[idx];
639 }
640 
641 /*---------------------------------------------------------------
642 addFrom_classSpecific
643 ---------------------------------------------------------------*/
644 void CColouredPointsMap::addFrom_classSpecific(
645  const CPointsMap& anotherMap, const size_t nPreviousPoints)
646 {
647  const size_t nOther = anotherMap.size();
648 
649  // Specific data for this class:
650  const auto* anotheMap_col =
651  dynamic_cast<const CColouredPointsMap*>(&anotherMap);
652 
653  if (anotheMap_col)
654  {
655  for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
656  {
657  m_color_R[j] = anotheMap_col->m_color_R[i];
658  m_color_G[j] = anotheMap_col->m_color_G[i];
659  m_color_B[j] = anotheMap_col->m_color_B[i];
660  }
661  }
662 }
663 
664 /** Save the point cloud as a PCL PCD file, in either ASCII or binary format
665  * \return false on any error */
666 bool CColouredPointsMap::savePCDFile(
667  const std::string& filename, bool save_as_binary) const
668 {
669 #if MRPT_HAS_PCL
670  pcl::PointCloud<pcl::PointXYZRGB> cloud;
671 
672  const size_t nThis = this->size();
673 
674  // Fill in the cloud data
675  cloud.width = nThis;
676  cloud.height = 1;
677  cloud.is_dense = false;
678  cloud.points.resize(cloud.width * cloud.height);
679 
680  const float f = 255.f;
681 
682  union myaux_t {
683  uint8_t rgb[4];
684  float f;
685  } aux_val;
686 
687  for (size_t i = 0; i < nThis; ++i)
688  {
689  cloud.points[i].x = m_x[i];
690  cloud.points[i].y = m_y[i];
691  cloud.points[i].z = m_z[i];
692 
693  aux_val.rgb[0] = static_cast<uint8_t>(this->m_color_B[i] * f);
694  aux_val.rgb[1] = static_cast<uint8_t>(this->m_color_G[i] * f);
695  aux_val.rgb[2] = static_cast<uint8_t>(this->m_color_R[i] * f);
696 
697  cloud.points[i].rgb = aux_val.f;
698  }
699 
700  return 0 == pcl::io::savePCDFile(filename, cloud, save_as_binary);
701 
702 #else
703  MRPT_UNUSED_PARAM(filename);
704  MRPT_UNUSED_PARAM(save_as_binary);
705  THROW_EXCEPTION("Operation not available: MRPT was built without PCL");
706 #endif
707 }
708 
709 namespace mrpt::maps::detail
710 {
712 
713 template <>
715 {
716  /** Helper method fot the generic implementation of
717  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
718  * points - this is the place to reserve memory in lric for extra working
719  * variables. */
721  CColouredPointsMap& me,
723  {
724  // Vars:
725  // [0] -> pR
726  // [1] -> pG
727  // [2] -> pB
728  // [3] -> Az_1_color
729  lric.fVars.resize(4);
730 
732  lric.fVars[3] = 1.0 / (me.colorScheme.z_max - me.colorScheme.z_min);
733  }
734 
735  /** Helper method fot the generic implementation of
736  * CPointsMap::loadFromRangeScan(), to be called once per range data */
738  CColouredPointsMap& me, const float gx, const float gy, const float gz,
740  {
741  MRPT_UNUSED_PARAM(gx);
742  MRPT_UNUSED_PARAM(gy);
743  // Relative height of the point wrt the sensor:
744  const float rel_z = gz - lric.HM(2, 3); // m23;
745 
746  // Variable renaming:
747  float& pR = lric.fVars[0];
748  float& pG = lric.fVars[1];
749  float& pB = lric.fVars[2];
750  const float& Az_1_color = lric.fVars[3];
751 
752  // Compute color:
753  switch (me.colorScheme.scheme)
754  {
755  // case cmFromHeightRelativeToSensor:
756  case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
757  case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
758  {
759  float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
760  if (q < 0)
761  q = 0;
762  else if (q > 1)
763  q = 1;
764 
765  if (me.colorScheme.scheme ==
766  CColouredPointsMap::cmFromHeightRelativeToSensorGray)
767  {
768  pR = pG = pB = q;
769  }
770  else
771  {
772  jet2rgb(q, pR, pG, pB);
773  }
774  }
775  break;
776  case CColouredPointsMap::cmFromIntensityImage:
777  {
778  // In 2D scans we don't have this channel.
779  pR = pG = pB = 1.0;
780  }
781  break;
782  default:
783  THROW_EXCEPTION("Unknown color scheme");
784  }
785  }
786  /** Helper method fot the generic implementation of
787  * CPointsMap::loadFromRangeScan(), to be called after each
788  * "{x,y,z}.push_back(...);" */
790  CColouredPointsMap& me,
792  {
793  float& pR = lric.fVars[0];
794  float& pG = lric.fVars[1];
795  float& pB = lric.fVars[2];
796  me.m_color_R.push_back(pR);
797  me.m_color_G.push_back(pG);
798  me.m_color_B.push_back(pB);
799  // m_min_dist.push_back(1e4);
800  }
801 
802  /** Helper method fot the generic implementation of
803  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
804  * points - this is the place to reserve memory in lric for extra working
805  * variables. */
807  CColouredPointsMap& me,
809  {
810  // Vars:
811  // [0] -> pR
812  // [1] -> pG
813  // [2] -> pB
814  // [3] -> Az_1_color
815  // [4] -> K_8u
816  // [5] -> cx
817  // [6] -> cy
818  // [7] -> fx
819  // [8] -> fy
820  lric.fVars.resize(9);
821  float& cx = lric.fVars[5];
822  float& cy = lric.fVars[6];
823  float& fx = lric.fVars[7];
824  float& fy = lric.fVars[8];
825 
826  // unsigned int vars:
827  // [0] -> imgW
828  // [1] -> imgH
829  // [2] -> img_idx_x
830  // [3] -> img_idx_y
831  lric.uVars.resize(4);
832  unsigned int& imgW = lric.uVars[0];
833  unsigned int& imgH = lric.uVars[1];
834  unsigned int& img_idx_x = lric.uVars[2];
835  unsigned int& img_idx_y = lric.uVars[3];
836 
837  // bool vars:
838  // [0] -> hasValidIntensityImage
839  // [1] -> hasColorIntensityImg
840  // [2] -> simple_3d_to_color_relation
841  lric.bVars.resize(3);
842  uint8_t& hasValidIntensityImage = lric.bVars[0];
843  uint8_t& hasColorIntensityImg = lric.bVars[1];
844  uint8_t& simple_3d_to_color_relation = lric.bVars[2];
845 
847  lric.fVars[3] = 1.0 / (me.colorScheme.z_max -
848  me.colorScheme.z_min); // Az_1_color = ...
849  lric.fVars[4] = 1.0f / 255; // K_8u
850 
851  hasValidIntensityImage = false;
852  imgW = 0;
853  imgH = 0;
854 
855  if (lric.rangeScan.hasIntensityImage)
856  {
857  // assure the size matches:
858  if (lric.rangeScan.points3D_x.size() ==
861  {
862  hasValidIntensityImage = true;
863  imgW = lric.rangeScan.intensityImage.getWidth();
864  imgH = lric.rangeScan.intensityImage.getHeight();
865  }
866  }
867 
868  hasColorIntensityImg =
869  hasValidIntensityImage && lric.rangeScan.intensityImage.isColor();
870 
871  // running indices for the image pixels for the gray levels:
872  // If both range & intensity images coincide (e.g. SwissRanger), then we
873  // can just
874  // assign 3D points to image pixels one-to-one, but that's not the case
875  // if
876  //
877  simple_3d_to_color_relation =
878  (std::abs(lric.rangeScan.relativePoseIntensityWRTDepth.norm()) <
879  1e-5);
880  img_idx_x = 0;
881  img_idx_y = 0;
882 
883  // Will be used just if simple_3d_to_color_relation=false
884  cx = lric.rangeScan.cameraParamsIntensity.cx();
885  cy = lric.rangeScan.cameraParamsIntensity.cy();
886  fx = lric.rangeScan.cameraParamsIntensity.fx();
887  fy = lric.rangeScan.cameraParamsIntensity.fy();
888  }
889 
890  /** Helper method fot the generic implementation of
891  * CPointsMap::loadFromRangeScan(), to be called once per range data */
893  CColouredPointsMap& me, const float gx, const float gy, const float gz,
895  {
896  MRPT_UNUSED_PARAM(gx);
897  MRPT_UNUSED_PARAM(gy);
898  // Rename variables:
899  float& pR = lric.fVars[0];
900  float& pG = lric.fVars[1];
901  float& pB = lric.fVars[2];
902  float& Az_1_color = lric.fVars[3];
903  float& K_8u = lric.fVars[4];
904  float& cx = lric.fVars[5];
905  float& cy = lric.fVars[6];
906  float& fx = lric.fVars[7];
907  float& fy = lric.fVars[8];
908 
909  unsigned int& imgW = lric.uVars[0];
910  unsigned int& imgH = lric.uVars[1];
911  unsigned int& img_idx_x = lric.uVars[2];
912  unsigned int& img_idx_y = lric.uVars[3];
913 
914  const uint8_t& hasValidIntensityImage = lric.bVars[0];
915  const uint8_t& hasColorIntensityImg = lric.bVars[1];
916  const uint8_t& simple_3d_to_color_relation = lric.bVars[2];
917 
918  // Relative height of the point wrt the sensor:
919  const float rel_z = gz - lric.HM(2, 3); // m23;
920 
921  // Compute color:
922  switch (me.colorScheme.scheme)
923  {
924  // case cmFromHeightRelativeToSensor:
925  case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
926  case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
927  {
928  float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
929  if (q < 0)
930  q = 0;
931  else if (q > 1)
932  q = 1;
933 
934  if (me.colorScheme.scheme ==
935  CColouredPointsMap::cmFromHeightRelativeToSensorGray)
936  {
937  pR = pG = pB = q;
938  }
939  else
940  {
941  jet2rgb(q, pR, pG, pB);
942  }
943  }
944  break;
945  case CColouredPointsMap::cmFromIntensityImage:
946  {
947  // Do we have to project the 3D point into the image plane??
948  bool hasValidColor = false;
949  if (simple_3d_to_color_relation)
950  {
951  hasValidColor = true;
952  }
953  else
954  {
955  // Do projection:
956  TPoint3D pt; // pt_wrt_colorcam;
959  lric.scan_x, lric.scan_y, lric.scan_z, pt.x, pt.y,
960  pt.z);
961 
962  // Project to image plane:
963  if (pt.z)
964  {
965  img_idx_x = cx + fx * pt.x / pt.z;
966  img_idx_y = cy + fy * pt.y / pt.z;
967 
968  hasValidColor = img_idx_x < imgW && // img_idx_x>=0
969  // isn't needed for
970  // unsigned.
971  img_idx_y < imgH;
972  }
973  }
974 
975  const auto& ii = lric.rangeScan.intensityImage;
976  if (hasValidColor && hasColorIntensityImg)
977  {
978  const auto* c = ii.ptr<uint8_t>(img_idx_x, img_idx_y);
979  pR = c[2] * K_8u;
980  pG = c[1] * K_8u;
981  pB = c[0] * K_8u;
982  }
983  else if (hasValidColor && hasValidIntensityImage)
984  {
985  const auto c = ii.at<uint8_t>(img_idx_x, img_idx_y);
986  pR = pG = pB = c * K_8u;
987  }
988  else
989  {
990  pR = pG = pB = 1.0;
991  }
992  }
993  break;
994  default:
995  THROW_EXCEPTION("Unknown color scheme");
996  }
997  }
998 
999  /** Helper method fot the generic implementation of
1000  * CPointsMap::loadFromRangeScan(), to be called after each
1001  * "{x,y,z}.push_back(...);" */
1003  CColouredPointsMap& me,
1005  {
1006  float& pR = lric.fVars[0];
1007  float& pG = lric.fVars[1];
1008  float& pB = lric.fVars[2];
1009 
1010  me.m_color_R.push_back(pR);
1011  me.m_color_G.push_back(pG);
1012  me.m_color_B.push_back(pB);
1013 
1014  // m_min_dist.push_back(1e4);
1015  }
1016 
1017  /** Helper method fot the generic implementation of
1018  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
1019  * end */
1021  CColouredPointsMap& me,
1023  {
1024  MRPT_UNUSED_PARAM(me);
1025  unsigned int& imgW = lric.uVars[0];
1026  unsigned int& img_idx_x = lric.uVars[2];
1027  unsigned int& img_idx_y = lric.uVars[3];
1028 
1029  const uint8_t& hasValidIntensityImage = lric.bVars[0];
1030  const uint8_t& simple_3d_to_color_relation = lric.bVars[2];
1031 
1032  // Advance the color pointer (just for simple cases, e.g. SwissRanger,
1033  // not for Kinect)
1034  if (simple_3d_to_color_relation && hasValidIntensityImage)
1035  {
1036  if (++img_idx_x >= imgW)
1037  {
1038  img_idx_y++;
1039  img_idx_x = 0;
1040  }
1041  }
1042  }
1043 };
1044 } // namespace mrpt::maps::detail
1045 /** See CPointsMap::loadFromRangeScan() */
1046 void CColouredPointsMap::loadFromRangeScan(
1047  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
1048 {
1050  templ_loadFromRangeScan(*this, rangeScan, robotPose);
1051 }
1052 
1053 /** See CPointsMap::loadFromRangeScan() */
1054 void CColouredPointsMap::loadFromRangeScan(
1055  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
1056 {
1058  templ_loadFromRangeScan(*this, rangeScan, robotPose);
1059 }
static void internal_loadFromRangeScan3D_prepareOneRange(CColouredPointsMap &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...
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Declares a class derived from "CObservation" that encapsules an image from a camera, whose relative pose to robot is also stored.
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:161
const mrpt::obs::CObservation3DRangeScan & rangeScan
Definition: CPointsMap.h:104
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
const double G
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
GLenum GLsizei n
Definition: glext.h:5136
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
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:163
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.
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:877
mrpt::aligned_std_vector< float > m_color_R
The color data.
std::string getChannelsOrder() const
As of mrpt 2.0.0, this returns either "GRAY" or "BGR".
Definition: CImage.cpp:856
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
STL namespace.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
static void internal_loadFromRangeScan2D_prepareOneRange(CColouredPointsMap &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...
TColourOptions colorScheme
The options employed when inserting laser scans in the map.
unsigned char uint8_t
Definition: rptypes.h:44
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
T square(const T x)
Inline function for the square of a number.
mrpt::maps::CPointsMap::TLikelihoodOptions likelihoodOpts
#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.
mrpt::aligned_std_vector< float > m_color_B
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:846
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:159
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:646
Lightweight 3D point (float version).
Definition: TPoint3D.h:21
mrpt::img::CImage image
The image captured by the camera, that is, the main piece of information of this observation.
void jet2rgb(const float color_index, float &r, float &g, float &b)
Computes the RGB color components (range [0,1]) for the corresponding color index in the range [0...
Definition: color_maps.cpp:142
GLuint index
Definition: glext.h:4068
const GLubyte * c
Definition: glext.h:6406
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
Definition: CPointsMap.h:103
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
float scan_x
In internal_loadFromRangeScan3D_prepareOneRange, these are the local coordinates of the scan points b...
Definition: CPointsMap.h:107
static void internal_loadFromRangeScan2D_init(CColouredPointsMap &me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
This namespace contains representation of robot actions and observations.
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
Definition: CPointsMap.h:88
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:25
mrpt::img::TCamera cameraParams
Intrinsic and distortion parameters of the camera.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
static void templ_loadFromRangeScan(Derived &obj, const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose)
mrpt::maps::CColouredPointsMap::TColourOptions colourOpts
#define ASSERT_NOT_EQUAL_(__A, __B)
Definition: exceptions.h:143
static void internal_loadFromRangeScan3D_postPushBack(CColouredPointsMap &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
A map of 2D/3D points with individual colours (RGB).
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:256
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
GLsizei const GLchar ** string
Definition: glext.h:4116
mrpt::aligned_std_vector< float > m_color_G
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
Definition: CPointsMap.h:109
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:410
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:157
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
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
Definition: CImage.cpp:887
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
bool hasIntensityImage
true means the field intensityImage contains valid data
A RGB color - floats in the range [0,1].
Definition: TColor.h:77
const T * ptr(unsigned int col, unsigned int row, unsigned int channel=0) const
Returns a pointer to a given pixel, without checking for boundaries.
Definition: img/CImage.h:580
GLuint in
Definition: glext.h:7391
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
static void aux_projectPoint_with_distortion(const mrpt::math::TPoint3D &P, const TCamera &params, TPixelCoordf &pixel, bool accept_points_behind)
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
Definition: CPointsMap.h:95
GLenum GLint GLint y
Definition: glext.h:3542
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
GLsizeiptr size
Definition: glext.h:3934
static void internal_loadFromRangeScan2D_postPushBack(CColouredPointsMap &me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called after e...
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
Definition: TPoint3D.h:90
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
Definition: CPointsMap.h:85
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
GLfloat GLfloat p
Definition: glext.h:6398
GLenum const GLfloat * params
Definition: glext.h:3538
static void internal_loadFromRangeScan3D_postOneRange(CColouredPointsMap &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called once pe...
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
Definition: CPointsMap.h:77
static void internal_loadFromRangeScan3D_init(CColouredPointsMap &me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext &lric)
Helper method fot the generic implementation of CPointsMap::loadFromRangeScan(), to be called only on...
#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: 034c2ee2a Tue Aug 20 02:15:02 2019 +0200 at mar ago 20 02:20:10 CEST 2019