MRPT  1.9.9
CColouredPointsMap.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
15 #include <mrpt/core/bits_mem.h>
16 #include <mrpt/img/color_maps.h>
17 #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 
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);
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 /*---------------------------------------------------------------
117 Copy constructor
118 ---------------------------------------------------------------*/
119 void CColouredPointsMap::copyFrom(const CPointsMap& obj)
120 {
121  CPointsMap::base_copyFrom(
122  obj); // This also does a ::resize(N) of all data fields.
123 
124  const CColouredPointsMap* pCol =
125  dynamic_cast<const CColouredPointsMap*>(&obj);
126  if (pCol)
127  {
128  m_color_R = pCol->m_color_R;
129  m_color_G = pCol->m_color_G;
130  m_color_B = pCol->m_color_B;
131  // m_min_dist = pCol->m_min_dist;
132  }
133 }
134 
135 uint8_t CColouredPointsMap::serializeGetVersion() const { return 9; }
136 void CColouredPointsMap::serializeTo(mrpt::serialization::CArchive& out) const
137 {
138  uint32_t n = m_x.size();
139 
140  // First, write the number of points:
141  out << n;
142 
143  if (n > 0)
144  {
145  out.WriteBufferFixEndianness(&m_x[0], n);
146  out.WriteBufferFixEndianness(&m_y[0], n);
147  out.WriteBufferFixEndianness(&m_z[0], n);
148  }
149  out << m_color_R << m_color_G << m_color_B; // added in v4
150 
151  out << genericMapParams; // v9
152  insertionOptions.writeToStream(
153  out); // version 9?: insert options are saved with its own method
154  likelihoodOptions.writeToStream(out); // Added in version 5
155 }
156 
157 void CColouredPointsMap::serializeFrom(
159 {
160  switch (version)
161  {
162  case 8:
163  case 9:
164  {
165  mark_as_modified();
166 
167  // Read the number of points:
168  uint32_t n;
169  in >> n;
170 
171  this->resize(n);
172 
173  if (n > 0)
174  {
175  in.ReadBufferFixEndianness(&m_x[0], n);
176  in.ReadBufferFixEndianness(&m_y[0], n);
177  in.ReadBufferFixEndianness(&m_z[0], n);
178  }
179  in >> m_color_R >> m_color_G >> m_color_B;
180 
181  if (version >= 9)
182  in >> genericMapParams;
183  else
184  {
185  bool disableSaveAs3DObject;
186  in >> disableSaveAs3DObject;
187  genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
188  }
189  insertionOptions.readFromStream(in);
190  likelihoodOptions.readFromStream(in);
191  }
192  break;
193 
194  case 0:
195  case 1:
196  case 2:
197  case 3:
198  case 4:
199  case 5:
200  case 6:
201  case 7:
202  {
203  mark_as_modified();
204 
205  // Read the number of points:
206  uint32_t n;
207  in >> n;
208 
209  this->resize(n);
210 
211  if (n > 0)
212  {
213  in.ReadBufferFixEndianness(&m_x[0], n);
214  in.ReadBufferFixEndianness(&m_y[0], n);
215  in.ReadBufferFixEndianness(&m_z[0], n);
216 
217  // Version 1: weights are also stored:
218  // Version 4: Type becomes long int -> uint32_t for
219  // portability!!
220  if (version >= 1)
221  {
222  if (version >= 4)
223  {
224  if (version >= 7)
225  {
226  // Weights were removed from this class in v7 (MRPT
227  // 0.9.5),
228  // so nothing else to do.
229  }
230  else
231  {
232  // Go on with old serialization format, but discard
233  // weights:
234  std::vector<uint32_t> dummy_pointWeight(n);
235  in.ReadBufferFixEndianness(
236  &dummy_pointWeight[0], n);
237  }
238  }
239  else
240  {
241  std::vector<uint32_t> dummy_pointWeight(n);
242  in.ReadBufferFixEndianness(&dummy_pointWeight[0], n);
243  }
244  }
245  }
246 
247  if (version >= 2)
248  {
249  // version 2: options saved too
250  in >> insertionOptions.minDistBetweenLaserPoints >>
251  insertionOptions.addToExistingPointsMap >>
252  insertionOptions.also_interpolate >>
253  insertionOptions.disableDeletion >>
254  insertionOptions.fuseWithExisting >>
255  insertionOptions.isPlanarMap;
256 
257  if (version < 6)
258  {
259  bool old_matchStaticPointsOnly;
260  in >> old_matchStaticPointsOnly;
261  }
262 
263  in >> insertionOptions.maxDistForInterpolatePoints;
264  {
265  bool disableSaveAs3DObject;
266  in >> disableSaveAs3DObject;
267  genericMapParams.enableSaveAs3DObject =
268  !disableSaveAs3DObject;
269  }
270  }
271 
272  if (version >= 3)
273  {
274  in >> insertionOptions.horizontalTolerance;
275  }
276 
277  if (version >= 4) // Color data
278  {
279  in >> m_color_R >> m_color_G >> m_color_B;
280  if (version >= 7)
281  {
282  // Removed: in >> m_min_dist;
283  }
284  else
285  {
286  std::vector<float> dummy_dist;
287  in >> dummy_dist;
288  }
289  }
290  else
291  {
292  m_color_R.assign(m_x.size(), 1.0f);
293  m_color_G.assign(m_x.size(), 1.0f);
294  m_color_B.assign(m_x.size(), 1.0f);
295  // m_min_dist.assign(x.size(),2000.0f);
296  }
297 
298  if (version >= 5) // version 5: added likelihoodOptions
299  likelihoodOptions.readFromStream(in);
300 
301  if (version >= 8) // version 8: added insertInvalidPoints
302  in >> insertionOptions.insertInvalidPoints;
303  }
304  break;
305  default:
307  };
308 }
309 
310 /*---------------------------------------------------------------
311 Clear
312 ---------------------------------------------------------------*/
313 void CColouredPointsMap::internal_clear()
314 {
315  // This swap() thing is the only way to really deallocate the memory.
316  vector_strong_clear(m_x);
317  vector_strong_clear(m_y);
318  vector_strong_clear(m_z);
319 
320  vector_strong_clear(m_color_R);
321  vector_strong_clear(m_color_G);
322  vector_strong_clear(m_color_B);
323 
324  mark_as_modified();
325 }
326 
327 /** Changes a given point from map. First index is 0.
328  * \exception Throws std::exception on index out of bound.
329  */
330 void CColouredPointsMap::setPoint(
331  size_t index, float x, float y, float z, float R, float G, float B)
332 {
333  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
334  m_x[index] = x;
335  m_y[index] = y;
336  m_z[index] = z;
337  this->m_color_R[index] = R;
338  this->m_color_G[index] = G;
339  this->m_color_B[index] = B;
340  mark_as_modified();
341 }
342 
343 /** Changes just the color of a given point from the map. First index is 0.
344  * \exception Throws std::exception on index out of bound.
345  */
346 void CColouredPointsMap::setPointColor(size_t index, float R, float G, float B)
347 {
348  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
349  this->m_color_R[index] = R;
350  this->m_color_G[index] = G;
351  this->m_color_B[index] = B;
352  // mark_as_modified(); // No need to rebuild KD-trees, etc...
353 }
354 
355 void CColouredPointsMap::insertPointFast(float x, float y, float z)
356 {
357  m_x.push_back(x);
358  m_y.push_back(y);
359  m_z.push_back(z);
360  m_color_R.push_back(1);
361  m_color_G.push_back(1);
362  m_color_B.push_back(1);
363 
364  // mark_as_modified(); -> Fast
365 }
366 
367 void CColouredPointsMap::insertPoint(
368  float x, float y, float z, float R, float G, float B)
369 {
370  m_x.push_back(x);
371  m_y.push_back(y);
372  m_z.push_back(z);
373  m_color_R.push_back(R);
374  m_color_G.push_back(G);
375  m_color_B.push_back(B);
376 
377  mark_as_modified();
378 }
379 
380 /*---------------------------------------------------------------
381 getAs3DObject
382 ---------------------------------------------------------------*/
383 void CColouredPointsMap::getAs3DObject(
384  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
385 {
386  ASSERT_(outObj);
387 
388  if (!genericMapParams.enableSaveAs3DObject) return;
389 
391  mrpt::make_aligned_shared<opengl::CPointCloudColoured>();
392 
393  obj->loadFromPointsMap(this);
394  obj->setColor(1, 1, 1, 1.0);
395 
396  obj->setPointSize(this->renderOptions.point_size);
397 
398  outObj->insert(obj);
399 }
400 
401 /*---------------------------------------------------------------
402 TColourOptions
403 ---------------------------------------------------------------*/
404 CColouredPointsMap::TColourOptions::TColourOptions()
405  : scheme(cmFromHeightRelativeToSensor), z_min(-10), z_max(10), d_max(5)
406 {
407 }
408 
410  const CConfigFileBase& source, const std::string& section)
411 {
412  scheme = source.read_enum(section, "scheme", scheme);
413  MRPT_LOAD_CONFIG_VAR(z_min, float, source, section)
414  MRPT_LOAD_CONFIG_VAR(z_max, float, source, section)
415  MRPT_LOAD_CONFIG_VAR(d_max, float, source, section)
416 }
417 
419  std::ostream& out) const
420 {
421  out << "\n----------- [CColouredPointsMap::TColourOptions] ------------ "
422  "\n\n";
423 
424  out << "scheme = " << scheme << endl;
425  out << "z_min = " << z_min << endl;
426  out << "z_max = " << z_max << endl;
427  out << "d_max = " << d_max << endl;
428 }
429 
431  size_t index, float& x, float& y, float& z) const
432 {
433  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
434 
435  x = m_x[index];
436  y = m_y[index];
437  z = m_z[index];
438 
439  return 1; // Weight
440 }
441 
442 /*---------------------------------------------------------------
443 getPoint
444 ---------------------------------------------------------------*/
446  size_t index, float& x, float& y, float& z, float& R, float& G,
447  float& B) const
448 {
449  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
450 
451  x = m_x[index];
452  y = m_y[index];
453  z = m_z[index];
454 
455  R = m_color_R[index];
456  G = m_color_G[index];
457  B = m_color_B[index];
458 }
459 
460 /** Retrieves a point color (colors range is [0,1])
461  */
463  size_t index, float& R, float& G, float& B) const
464 {
465  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
466 
467  R = m_color_R[index];
468  G = m_color_G[index];
469  B = m_color_B[index];
470 }
471 
472 // This function is duplicated from
473 // "mrpt::vision::pinhole::projectPoint_with_distortion", to avoid
474 // a dependency on mrpt-vision.
476  const mrpt::math::TPoint3D& P, const TCamera& params, TPixelCoordf& pixel,
477  bool accept_points_behind)
478 {
479  MRPT_UNUSED_PARAM(accept_points_behind);
480  // Pinhole model:
481  const double x = P.x / P.z;
482  const double y = P.y / P.z;
483 
484  // Radial distortion:
485  const double r2 = square(x) + square(y);
486  const double r4 = square(r2);
487  const double r6 = r2 * r4;
488 
489  pixel.x = params.cx() +
490  params.fx() * (x * (1 + params.dist[0] * r2 +
491  params.dist[1] * r4 + params.dist[4] * r6) +
492  2 * params.dist[2] * x * y +
493  params.dist[3] * (r2 + 2 * square(x)));
494  pixel.y = params.cy() +
495  params.fy() * (y * (1 + params.dist[0] * r2 +
496  params.dist[1] * r4 + params.dist[4] * r6) +
497  2 * params.dist[3] * x * y +
498  params.dist[2] * (r2 + 2 * square(y)));
499 }
500 
501 /*---------------------------------------------------------------
502 getPoint
503 ---------------------------------------------------------------*/
505  const CObservationImage& obs, const CPose3D& robotPose)
506 {
507  // Check if image is not grayscale
508  ASSERT_(obs.image.isColor());
509 
510  CPose3D cameraPoseR; // Get Camera Pose on the Robot(B) (CPose3D)
511  CPose3D cameraPoseW; // World Camera Pose
512 
513  obs.getSensorPose(cameraPoseR);
514  cameraPoseW = robotPose + cameraPoseR; // Camera Global Coordinates
515 
516  // Image Information
517  unsigned int imgW = obs.image.getWidth();
518  unsigned int imgH = obs.image.getHeight();
519 
520  // Projection related variables
521  std::vector<TPixelCoordf>
522  projectedPoints; // The set of projected points in the image
524  itProPoints; // Iterator for projectedPoints
525  std::vector<size_t> p_idx;
526  std::vector<float> p_dist;
527  std::vector<unsigned int> p_proj;
528 
529  // Get the N closest points
531  cameraPoseW.x(), cameraPoseW.y(), // query point
532  200000, // number of points to search
533  p_idx, p_dist); // indexes and distances of the returned points
534 
535  // Fill p3D vector
536  for (size_t k = 0; k < p_idx.size(); k++)
537  {
538  float d = sqrt(p_dist[k]);
539  size_t idx = p_idx[k];
540  if (d < colorScheme.d_max) // && d < m_min_dist[idx] )
541  {
542  TPixelCoordf px;
544  TPoint3D(m_x[idx], m_y[idx], m_z[idx]), obs.cameraParams, px, true);
545  projectedPoints.push_back(px);
546  p_proj.push_back(k);
547  } // end if
548  } // end for
549 
550  // Get channel order
551  unsigned int chR, chG, chB;
552  if (obs.image.getChannelsOrder()[0] == 'B')
553  {
554  chR = 2;
555  chG = 1;
556  chB = 0;
557  }
558  else
559  {
560  chR = 0;
561  chG = 1;
562  chB = 2;
563  }
564 
565  unsigned int n_proj = 0;
566  const float factor = 1.0f / 255; // Normalize pixels:
567 
568  // Get the colour of the projected points
569  size_t k;
570  for (itProPoints = projectedPoints.begin(), k = 0;
571  itProPoints != projectedPoints.end(); ++itProPoints, ++k)
572  {
573  // Only get the points projected inside the image
574  if (itProPoints->x >= 0 && itProPoints->x < imgW &&
575  itProPoints->y > 0 && itProPoints->y < imgH)
576  {
577  unsigned int ii = p_idx[p_proj[k]];
578  uint8_t* p = obs.image(
579  (unsigned int)itProPoints->x, (unsigned int)itProPoints->y);
580 
581  m_color_R[ii] = p[chR] * factor; // R
582  m_color_G[ii] = p[chG] * factor; // G
583  m_color_B[ii] = p[chB] * factor; // B
584  // m_min_dist[ii] = p_dist[p_proj[k]];
585 
586  n_proj++;
587  }
588  } // end for
589 
590  return true;
591 } // end colourFromObservation
592 
594 {
595  MRPT_UNUSED_PARAM(defValue);
596  // m_min_dist.assign(x.size(),defValue);
597 }
598 
600  const std::string& file) const
601 {
602  FILE* f = os::fopen(file.c_str(), "wt");
603  if (!f) return false;
604 
605  for (unsigned int i = 0; i < m_x.size(); i++)
606  os::fprintf(
607  f, "%f %f %f %d %d %d\n", m_x[i], m_y[i], m_z[i],
608  (uint8_t)(255 * m_color_R[i]), (uint8_t)(255 * m_color_G[i]),
609  (uint8_t)(255 * m_color_B[i]));
610  // os::fprintf(f,"%f %f %f %f %f %f
611  //%f\n",x[i],y[i],z[i],m_color_R[i],m_color_G[i],m_color_B[i],m_min_dist[i]);
612 
613  os::fclose(f);
614  return true;
615 }
616 
617 // ================================ PLY files import & export virtual methods
618 // ================================
619 
620 /** In a base class, reserve memory to prepare subsequent calls to
621  * PLY_import_set_vertex */
623 {
624  this->setSize(N);
625 }
626 
627 /** In a base class, will be called after PLY_import_set_vertex_count() once for
628  * each loaded point.
629  * \param pt_color Will be nullptr if the loaded file does not provide color
630  * info.
631  */
633  const size_t idx, const mrpt::math::TPoint3Df& pt, const TColorf* pt_color)
634 {
635  if (pt_color)
636  this->setPoint(
637  idx, pt.x, pt.y, pt.z, pt_color->R, pt_color->G, pt_color->B);
638  else
639  this->setPoint(idx, pt.x, pt.y, pt.z);
640 }
641 
642 /** In a base class, will be called after PLY_export_get_vertex_count() once for
643  * each exported point.
644  * \param pt_color Will be nullptr if the loaded file does not provide color
645  * info.
646  */
648  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
649  TColorf& pt_color) const
650 {
651  pt_has_color = true;
652 
653  pt.x = m_x[idx];
654  pt.y = m_y[idx];
655  pt.z = m_z[idx];
656 
657  pt_color.R = m_color_R[idx];
658  pt_color.G = m_color_G[idx];
659  pt_color.B = m_color_B[idx];
660 }
661 
662 /*---------------------------------------------------------------
663 addFrom_classSpecific
664 ---------------------------------------------------------------*/
666  const CPointsMap& anotherMap, const size_t nPreviousPoints)
667 {
668  const size_t nOther = anotherMap.size();
669 
670  // Specific data for this class:
671  const CColouredPointsMap* anotheMap_col =
672  dynamic_cast<const CColouredPointsMap*>(&anotherMap);
673 
674  if (anotheMap_col)
675  {
676  for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
677  {
678  m_color_R[j] = anotheMap_col->m_color_R[i];
679  m_color_G[j] = anotheMap_col->m_color_G[i];
680  m_color_B[j] = anotheMap_col->m_color_B[i];
681  }
682  }
683 }
684 
685 /** Save the point cloud as a PCL PCD file, in either ASCII or binary format
686  * \return false on any error */
688  const std::string& filename, bool save_as_binary) const
689 {
690 #if MRPT_HAS_PCL
691  pcl::PointCloud<pcl::PointXYZRGB> cloud;
692 
693  const size_t nThis = this->size();
694 
695  // Fill in the cloud data
696  cloud.width = nThis;
697  cloud.height = 1;
698  cloud.is_dense = false;
699  cloud.points.resize(cloud.width * cloud.height);
700 
701  const float f = 255.f;
702 
703  union myaux_t {
704  uint8_t rgb[4];
705  float f;
706  } aux_val;
707 
708  for (size_t i = 0; i < nThis; ++i)
709  {
710  cloud.points[i].x = m_x[i];
711  cloud.points[i].y = m_y[i];
712  cloud.points[i].z = m_z[i];
713 
714  aux_val.rgb[0] = static_cast<uint8_t>(this->m_color_B[i] * f);
715  aux_val.rgb[1] = static_cast<uint8_t>(this->m_color_G[i] * f);
716  aux_val.rgb[2] = static_cast<uint8_t>(this->m_color_R[i] * f);
717 
718  cloud.points[i].rgb = aux_val.f;
719  }
720 
721  return 0 == pcl::io::savePCDFile(filename, cloud, save_as_binary);
722 
723 #else
724  MRPT_UNUSED_PARAM(filename);
725  MRPT_UNUSED_PARAM(save_as_binary);
726  THROW_EXCEPTION("Operation not available: MRPT was built without PCL")
727 #endif
728 }
729 
730 namespace mrpt::maps::detail
731 {
733 
734 template <>
736 {
737  /** Helper method fot the generic implementation of
738  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
739  * points - this is the place to reserve memory in lric for extra working
740  * variables. */
742  CColouredPointsMap& me,
744  {
745  // Vars:
746  // [0] -> pR
747  // [1] -> pG
748  // [2] -> pB
749  // [3] -> Az_1_color
750  lric.fVars.resize(4);
751 
753  lric.fVars[3] = 1.0 / (me.colorScheme.z_max - me.colorScheme.z_min);
754  }
755 
756  /** Helper method fot the generic implementation of
757  * CPointsMap::loadFromRangeScan(), to be called once per range data */
759  CColouredPointsMap& me, const float gx, const float gy, const float gz,
761  {
762  MRPT_UNUSED_PARAM(gx);
763  MRPT_UNUSED_PARAM(gy);
764  // Relative height of the point wrt the sensor:
765  const float rel_z = gz - lric.HM.get_unsafe(2, 3); // m23;
766 
767  // Variable renaming:
768  float& pR = lric.fVars[0];
769  float& pG = lric.fVars[1];
770  float& pB = lric.fVars[2];
771  const float& Az_1_color = lric.fVars[3];
772 
773  // Compute color:
774  switch (me.colorScheme.scheme)
775  {
776  // case cmFromHeightRelativeToSensor:
779  {
780  float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
781  if (q < 0)
782  q = 0;
783  else if (q > 1)
784  q = 1;
785 
786  if (me.colorScheme.scheme ==
788  {
789  pR = pG = pB = q;
790  }
791  else
792  {
793  jet2rgb(q, pR, pG, pB);
794  }
795  }
796  break;
798  {
799  // In 2D scans we don't have this channel.
800  pR = pG = pB = 1.0;
801  }
802  break;
803  default:
804  THROW_EXCEPTION("Unknown color scheme");
805  }
806  }
807  /** Helper method fot the generic implementation of
808  * CPointsMap::loadFromRangeScan(), to be called after each
809  * "{x,y,z}.push_back(...);" */
811  CColouredPointsMap& me,
813  {
814  float& pR = lric.fVars[0];
815  float& pG = lric.fVars[1];
816  float& pB = lric.fVars[2];
817  me.m_color_R.push_back(pR);
818  me.m_color_G.push_back(pG);
819  me.m_color_B.push_back(pB);
820  // m_min_dist.push_back(1e4);
821  }
822 
823  /** Helper method fot the generic implementation of
824  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
825  * points - this is the place to reserve memory in lric for extra working
826  * variables. */
828  CColouredPointsMap& me,
830  {
831  // Vars:
832  // [0] -> pR
833  // [1] -> pG
834  // [2] -> pB
835  // [3] -> Az_1_color
836  // [4] -> K_8u
837  // [5] -> cx
838  // [6] -> cy
839  // [7] -> fx
840  // [8] -> fy
841  lric.fVars.resize(9);
842  float& cx = lric.fVars[5];
843  float& cy = lric.fVars[6];
844  float& fx = lric.fVars[7];
845  float& fy = lric.fVars[8];
846 
847  // unsigned int vars:
848  // [0] -> imgW
849  // [1] -> imgH
850  // [2] -> img_idx_x
851  // [3] -> img_idx_y
852  lric.uVars.resize(4);
853  unsigned int& imgW = lric.uVars[0];
854  unsigned int& imgH = lric.uVars[1];
855  unsigned int& img_idx_x = lric.uVars[2];
856  unsigned int& img_idx_y = lric.uVars[3];
857 
858  // bool vars:
859  // [0] -> hasValidIntensityImage
860  // [1] -> hasColorIntensityImg
861  // [2] -> simple_3d_to_color_relation
862  lric.bVars.resize(3);
863  uint8_t& hasValidIntensityImage = lric.bVars[0];
864  uint8_t& hasColorIntensityImg = lric.bVars[1];
865  uint8_t& simple_3d_to_color_relation = lric.bVars[2];
866 
868  lric.fVars[3] = 1.0 / (me.colorScheme.z_max -
869  me.colorScheme.z_min); // Az_1_color = ...
870  lric.fVars[4] = 1.0f / 255; // K_8u
871 
872  hasValidIntensityImage = false;
873  imgW = 0;
874  imgH = 0;
875 
876  if (lric.rangeScan.hasIntensityImage)
877  {
878  // assure the size matches:
879  if (lric.rangeScan.points3D_x.size() ==
882  {
883  hasValidIntensityImage = true;
884  imgW = lric.rangeScan.intensityImage.getWidth();
885  imgH = lric.rangeScan.intensityImage.getHeight();
886  }
887  }
888 
889  hasColorIntensityImg =
890  hasValidIntensityImage && lric.rangeScan.intensityImage.isColor();
891 
892  // running indices for the image pixels for the gray levels:
893  // If both range & intensity images coincide (e.g. SwissRanger), then we
894  // can just
895  // assign 3D points to image pixels one-to-one, but that's not the case
896  // if
897  //
898  simple_3d_to_color_relation =
899  (std::abs(lric.rangeScan.relativePoseIntensityWRTDepth.norm()) <
900  1e-5);
901  img_idx_x = 0;
902  img_idx_y = 0;
903 
904  // Will be used just if simple_3d_to_color_relation=false
905  cx = lric.rangeScan.cameraParamsIntensity.cx();
906  cy = lric.rangeScan.cameraParamsIntensity.cy();
907  fx = lric.rangeScan.cameraParamsIntensity.fx();
908  fy = lric.rangeScan.cameraParamsIntensity.fy();
909  }
910 
911  /** Helper method fot the generic implementation of
912  * CPointsMap::loadFromRangeScan(), to be called once per range data */
914  CColouredPointsMap& me, const float gx, const float gy, const float gz,
916  {
917  MRPT_UNUSED_PARAM(gx);
918  MRPT_UNUSED_PARAM(gy);
919  // Rename variables:
920  float& pR = lric.fVars[0];
921  float& pG = lric.fVars[1];
922  float& pB = lric.fVars[2];
923  float& Az_1_color = lric.fVars[3];
924  float& K_8u = lric.fVars[4];
925  float& cx = lric.fVars[5];
926  float& cy = lric.fVars[6];
927  float& fx = lric.fVars[7];
928  float& fy = lric.fVars[8];
929 
930  unsigned int& imgW = lric.uVars[0];
931  unsigned int& imgH = lric.uVars[1];
932  unsigned int& img_idx_x = lric.uVars[2];
933  unsigned int& img_idx_y = lric.uVars[3];
934 
935  const uint8_t& hasValidIntensityImage = lric.bVars[0];
936  const uint8_t& hasColorIntensityImg = lric.bVars[1];
937  const uint8_t& simple_3d_to_color_relation = lric.bVars[2];
938 
939  // Relative height of the point wrt the sensor:
940  const float rel_z = gz - lric.HM.get_unsafe(2, 3); // m23;
941 
942  // Compute color:
943  switch (me.colorScheme.scheme)
944  {
945  // case cmFromHeightRelativeToSensor:
948  {
949  float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
950  if (q < 0)
951  q = 0;
952  else if (q > 1)
953  q = 1;
954 
955  if (me.colorScheme.scheme ==
957  {
958  pR = pG = pB = q;
959  }
960  else
961  {
962  jet2rgb(q, pR, pG, pB);
963  }
964  }
965  break;
967  {
968  // Do we have to project the 3D point into the image plane??
969  bool hasValidColor = false;
970  if (simple_3d_to_color_relation)
971  {
972  hasValidColor = true;
973  }
974  else
975  {
976  // Do projection:
977  TPoint3D pt; // pt_wrt_colorcam;
980  lric.scan_x, lric.scan_y, lric.scan_z, pt.x, pt.y,
981  pt.z);
982 
983  // Project to image plane:
984  if (pt.z)
985  {
986  img_idx_x = cx + fx * pt.x / pt.z;
987  img_idx_y = cy + fy * pt.y / pt.z;
988 
989  hasValidColor = img_idx_x < imgW && // img_idx_x>=0
990  // isn't needed for
991  // unsigned.
992  img_idx_y < imgH;
993  }
994  }
995 
996  if (hasValidColor && hasColorIntensityImg)
997  {
999  img_idx_x, img_idx_y, 0);
1000  pR = c[2] * K_8u;
1001  pG = c[1] * K_8u;
1002  pB = c[0] * K_8u;
1003  }
1004  else if (hasValidColor && hasValidIntensityImage)
1005  {
1007  img_idx_x, img_idx_y, 0);
1008  pR = pG = pB = c * K_8u;
1009  }
1010  else
1011  {
1012  pR = pG = pB = 1.0;
1013  }
1014  }
1015  break;
1016  default:
1017  THROW_EXCEPTION("Unknown color scheme");
1018  }
1019  }
1020 
1021  /** Helper method fot the generic implementation of
1022  * CPointsMap::loadFromRangeScan(), to be called after each
1023  * "{x,y,z}.push_back(...);" */
1025  CColouredPointsMap& me,
1027  {
1028  float& pR = lric.fVars[0];
1029  float& pG = lric.fVars[1];
1030  float& pB = lric.fVars[2];
1031 
1032  me.m_color_R.push_back(pR);
1033  me.m_color_G.push_back(pG);
1034  me.m_color_B.push_back(pB);
1035 
1036  // m_min_dist.push_back(1e4);
1037  }
1038 
1039  /** Helper method fot the generic implementation of
1040  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
1041  * end */
1043  CColouredPointsMap& me,
1045  {
1046  MRPT_UNUSED_PARAM(me);
1047  unsigned int& imgW = lric.uVars[0];
1048  unsigned int& img_idx_x = lric.uVars[2];
1049  unsigned int& img_idx_y = lric.uVars[3];
1050 
1051  const uint8_t& hasValidIntensityImage = lric.bVars[0];
1052  const uint8_t& simple_3d_to_color_relation = lric.bVars[2];
1053 
1054  // Advance the color pointer (just for simple cases, e.g. SwissRanger,
1055  // not for Kinect)
1056  if (simple_3d_to_color_relation && hasValidIntensityImage)
1057  {
1058  if (++img_idx_x >= imgW)
1059  {
1060  img_idx_y++;
1061  img_idx_x = 0;
1062  }
1063  }
1064  }
1065 };
1066 }
1067 /** See CPointsMap::loadFromRangeScan() */
1069  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
1070 {
1072  templ_loadFromRangeScan(*this, rangeScan, robotPose);
1073 }
1074 
1075 /** See CPointsMap::loadFromRangeScan() */
1077  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
1078 {
1080  templ_loadFromRangeScan(*this, rangeScan, robotPose);
1081 }
1082 
1083 
virtual void getPoint(size_t index, float &x, float &y, float &z, float &R, float &G, float &B) const override
Retrieves a point and its color (colors range is [0,1])
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 void setSize(size_t newLength) override
Resizes all point buffers so they can hold the given number of points, erasing all previous contents ...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
virtual void addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints) override
Auxiliary method called from within addFrom() automatically, to finish the copying of class-specific ...
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Scalar * iterator
Definition: eigen_plugins.h:26
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:3872
bool save3D_and_colour_to_text_file(const std::string &file) const
Save to a text file.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:163
const mrpt::obs::CObservation3DRangeScan & rangeScan
Definition: CPointsMap.h:103
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
const double G
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
GLenum GLsizei n
Definition: glext.h:5074
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
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:127
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:165
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:892
mrpt::aligned_std_vector< float > m_color_R
The color data.
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
STL namespace.
void kdTreeNClosestPoint2DIdx(float x0, float y0, size_t knn, std::vector< size_t > &out_idx, std::vector< float > &out_dist_sqr) const
KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes...
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
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:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
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:113
void aux_projectPoint_with_distortion(const mrpt::math::TPoint3D &P, const TCamera &params, TPixelCoordf &pixel, bool accept_points_behind)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:64
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:864
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:161
Lightweight 3D point (float version).
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:138
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:253
GLuint index
Definition: glext.h:4054
const GLubyte * c
Definition: glext.h:6313
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
Definition: CPointsMap.h:102
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:106
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.
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
virtual void PLY_import_set_vertex_count(const size_t N) override
In a base class, reserve memory to prepare subsequent calls to PLY_import_set_vertex.
const char * getChannelsOrder() const
Returns a string of the form "BGR","RGB" or "GRAY" indicating the channels ordering.
Definition: CImage.cpp:1180
std::vector< float > fVars
Extra variables to be used as desired by the derived class.
Definition: CPointsMap.h:87
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:27
double x
X,Y,Z coordinates.
mrpt::img::TCamera cameraParams
Intrinsic and distortion parameters of the camera.
static void templ_loadFromRangeScan(Derived &obj, const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose)
mrpt::maps::CColouredPointsMap::TColourOptions colourOpts
virtual bool savePCDFile(const std::string &filename, bool save_as_binary) const override
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
#define ASSERT_NOT_EQUAL_(__A, __B)
Definition: exceptions.h:159
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).
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
GLsizei const GLchar ** string
Definition: glext.h:4101
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:108
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
#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:159
mrpt::aligned_std_vector< float > m_z
Definition: CPointsMap.h:1094
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:52
mrpt::aligned_std_vector< float > m_y
Definition: CPointsMap.h:1094
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
Definition: CImage.cpp:906
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:54
const float R
virtual void setPoint(size_t index, float x, float y, float z, float R, float G, float B) override
Changes a given point from map.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
void resetPointsMinDist(float defValue=2000.0f)
Reset the minimum-observed-distance buffer for all the points to a predefined value.
void PLY_export_get_vertex(const size_t idx, mrpt::math::TPoint3Df &pt, bool &pt_has_color, mrpt::img::TColorf &pt_color) const override
In a base class, will be called after PLY_export_get_vertex_count() once for each exported point...
virtual void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose=nullptr) override
See CPointsMap::loadFromRangeScan()
bool hasIntensityImage
true means the field intensityImage contains valid data
A RGB color - floats in the range [0,1].
Definition: TColor.h:77
virtual void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::img::TColorf *pt_color=nullptr) override
In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point...
GLuint in
Definition: glext.h:7274
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:4082
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
Definition: CPointsMap.h:94
GLenum GLint GLint y
Definition: glext.h:3538
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
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:3538
Lightweight 3D point.
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all params from a config file/source.
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
Definition: CPointsMap.h:84
unsigned __int32 uint32_t
Definition: rptypes.h:47
mrpt::aligned_std_vector< float > m_x
The point coordinates.
Definition: CPointsMap.h:1094
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:16
bool colourFromObservation(const mrpt::obs::CObservationImage &obs, const mrpt::poses::CPose3D &robotPose)
Colour a set of points from a CObservationImage and the global pose of the robot. ...
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:408
GLfloat GLfloat p
Definition: glext.h:6305
GLenum const GLfloat * params
Definition: glext.h:3534
void getPointColor(size_t index, float &R, float &G, float &B) const
Retrieves a point color (colors range is [0,1])
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:76
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...
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:649
unsigned char * get_unsafe(unsigned int col, unsigned int row, unsigned int channel=0) const
Access to pixels without checking boundaries - Use normally the () operator better, which checks the coordinates.
Definition: CImage.cpp:491
#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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020