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  "mrpt::maps::CColouredPointsMap,colourPointsMap",
37 
38 CColouredPointsMap::TMapDefinition::TMapDefinition() = default;
39 void CColouredPointsMap::TMapDefinition::loadFromConfigFile_map_specific(
40  const CConfigFileBase& source, const std::string& sectionNamePrefix)
41 {
42  insertionOpts.loadFromConfigFile(
43  source, sectionNamePrefix + string("_insertOpts"));
44  likelihoodOpts.loadFromConfigFile(
45  source, sectionNamePrefix + string("_likelihoodOpts"));
46  colourOpts.loadFromConfigFile(
47  source, sectionNamePrefix + string("_colorOpts"));
48 }
49 
50 void CColouredPointsMap::TMapDefinition::dumpToTextStream_map_specific(
51  std::ostream& out) const
52 {
53  this->insertionOpts.dumpToTextStream(out);
54  this->likelihoodOpts.dumpToTextStream(out);
55  this->colourOpts.dumpToTextStream(out);
56 }
57 
58 mrpt::maps::CMetricMap* CColouredPointsMap::internal_CreateFromMapDefinition(
60 {
62  *dynamic_cast<const CColouredPointsMap::TMapDefinition*>(&_def);
63  auto* obj = new CColouredPointsMap();
64  obj->insertionOptions = def.insertionOpts;
65  obj->likelihoodOptions = def.likelihoodOpts;
66  obj->colorScheme = def.colourOpts;
67  return obj;
68 }
69 // =========== End of Map definition Block =========
70 
72 
73 void CColouredPointsMap::reserve(size_t newLength)
74 {
75  m_x.reserve(newLength);
76  m_y.reserve(newLength);
77  m_z.reserve(newLength);
78  m_color_R.reserve(newLength);
79  m_color_G.reserve(newLength);
80  m_color_B.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 CColouredPointsMap::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_color_R.resize(newLength, 1);
92  m_color_G.resize(newLength, 1);
93  m_color_B.resize(newLength, 1);
94  mark_as_modified();
95 }
96 
97 // Resizes all point buffers so they can hold the given number of points,
98 // *erasing* all previous contents
99 // and leaving all points to default values.
100 void CColouredPointsMap::setSize(size_t newLength)
101 {
102  m_x.assign(newLength, 0);
103  m_y.assign(newLength, 0);
104  m_z.assign(newLength, 0);
105  m_color_R.assign(newLength, 1);
106  m_color_G.assign(newLength, 1);
107  m_color_B.assign(newLength, 1);
108  mark_as_modified();
109 }
110 
111 void CColouredPointsMap::impl_copyFrom(const CPointsMap& obj)
112 {
113  // This also does a ::resize(N) of all data fields.
114  CPointsMap::base_copyFrom(obj);
115 
116  const auto* pCol = dynamic_cast<const CColouredPointsMap*>(&obj);
117  if (pCol)
118  {
119  m_color_R = pCol->m_color_R;
120  m_color_G = pCol->m_color_G;
121  m_color_B = pCol->m_color_B;
122  }
123 }
124 
125 uint8_t CColouredPointsMap::serializeGetVersion() const { return 9; }
126 void CColouredPointsMap::serializeTo(mrpt::serialization::CArchive& out) const
127 {
128  uint32_t n = m_x.size();
129 
130  // First, write the number of points:
131  out << n;
132 
133  if (n > 0)
134  {
135  out.WriteBufferFixEndianness(&m_x[0], n);
136  out.WriteBufferFixEndianness(&m_y[0], n);
137  out.WriteBufferFixEndianness(&m_z[0], n);
138  }
139  out << m_color_R << m_color_G << m_color_B; // added in v4
140 
141  out << genericMapParams; // v9
142  insertionOptions.writeToStream(
143  out); // version 9?: insert options are saved with its own method
144  likelihoodOptions.writeToStream(out); // Added in version 5
145 }
146 
147 void CColouredPointsMap::serializeFrom(
148  mrpt::serialization::CArchive& in, uint8_t version)
149 {
150  switch (version)
151  {
152  case 8:
153  case 9:
154  {
155  mark_as_modified();
156 
157  // Read the number of points:
158  uint32_t n;
159  in >> n;
160 
161  this->resize(n);
162 
163  if (n > 0)
164  {
165  in.ReadBufferFixEndianness(&m_x[0], n);
166  in.ReadBufferFixEndianness(&m_y[0], n);
167  in.ReadBufferFixEndianness(&m_z[0], n);
168  }
169  in >> m_color_R >> m_color_G >> m_color_B;
170 
171  if (version >= 9)
172  in >> genericMapParams;
173  else
174  {
175  bool disableSaveAs3DObject;
176  in >> disableSaveAs3DObject;
177  genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
178  }
179  insertionOptions.readFromStream(in);
180  likelihoodOptions.readFromStream(in);
181  }
182  break;
183 
184  case 0:
185  case 1:
186  case 2:
187  case 3:
188  case 4:
189  case 5:
190  case 6:
191  case 7:
192  {
193  mark_as_modified();
194 
195  // Read the number of points:
196  uint32_t n;
197  in >> n;
198 
199  this->resize(n);
200 
201  if (n > 0)
202  {
203  in.ReadBufferFixEndianness(&m_x[0], n);
204  in.ReadBufferFixEndianness(&m_y[0], n);
205  in.ReadBufferFixEndianness(&m_z[0], n);
206 
207  // Version 1: weights are also stored:
208  // Version 4: Type becomes long int -> uint32_t for
209  // portability!!
210  if (version >= 1)
211  {
212  if (version >= 4)
213  {
214  if (version >= 7)
215  {
216  // Weights were removed from this class in v7 (MRPT
217  // 0.9.5),
218  // so nothing else to do.
219  }
220  else
221  {
222  // Go on with old serialization format, but discard
223  // weights:
224  std::vector<uint32_t> dummy_pointWeight(n);
225  in.ReadBufferFixEndianness(
226  &dummy_pointWeight[0], n);
227  }
228  }
229  else
230  {
231  std::vector<uint32_t> dummy_pointWeight(n);
232  in.ReadBufferFixEndianness(&dummy_pointWeight[0], n);
233  }
234  }
235  }
236 
237  if (version >= 2)
238  {
239  // version 2: options saved too
240  in >> insertionOptions.minDistBetweenLaserPoints >>
241  insertionOptions.addToExistingPointsMap >>
242  insertionOptions.also_interpolate >>
243  insertionOptions.disableDeletion >>
244  insertionOptions.fuseWithExisting >>
245  insertionOptions.isPlanarMap;
246 
247  if (version < 6)
248  {
249  bool old_matchStaticPointsOnly;
250  in >> old_matchStaticPointsOnly;
251  }
252 
253  in >> insertionOptions.maxDistForInterpolatePoints;
254  {
255  bool disableSaveAs3DObject;
256  in >> disableSaveAs3DObject;
257  genericMapParams.enableSaveAs3DObject =
258  !disableSaveAs3DObject;
259  }
260  }
261 
262  if (version >= 3)
263  {
264  in >> insertionOptions.horizontalTolerance;
265  }
266 
267  if (version >= 4) // Color data
268  {
269  in >> m_color_R >> m_color_G >> m_color_B;
270  if (version >= 7)
271  {
272  // Removed: in >> m_min_dist;
273  }
274  else
275  {
276  std::vector<float> dummy_dist;
277  in >> dummy_dist;
278  }
279  }
280  else
281  {
282  m_color_R.assign(m_x.size(), 1.0f);
283  m_color_G.assign(m_x.size(), 1.0f);
284  m_color_B.assign(m_x.size(), 1.0f);
285  // m_min_dist.assign(x.size(),2000.0f);
286  }
287 
288  if (version >= 5) // version 5: added likelihoodOptions
289  likelihoodOptions.readFromStream(in);
290 
291  if (version >= 8) // version 8: added insertInvalidPoints
292  in >> insertionOptions.insertInvalidPoints;
293  }
294  break;
295  default:
297  };
298 }
299 
300 /*---------------------------------------------------------------
301 Clear
302 ---------------------------------------------------------------*/
303 void CColouredPointsMap::internal_clear()
304 {
305  // This swap() thing is the only way to really deallocate the memory.
306  vector_strong_clear(m_x);
307  vector_strong_clear(m_y);
308  vector_strong_clear(m_z);
309 
310  vector_strong_clear(m_color_R);
311  vector_strong_clear(m_color_G);
312  vector_strong_clear(m_color_B);
313 
314  mark_as_modified();
315 }
316 
317 /** Changes a given point from map. First index is 0.
318  * \exception Throws std::exception on index out of bound.
319  */
320 void CColouredPointsMap::setPointRGB(
321  size_t index, float x, float y, float z, float R, float G, float B)
322 {
323  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
324  m_x[index] = x;
325  m_y[index] = y;
326  m_z[index] = z;
327  this->m_color_R[index] = R;
328  this->m_color_G[index] = G;
329  this->m_color_B[index] = B;
330  mark_as_modified();
331 }
332 
333 /** Changes just the color of a given point from the map. First index is 0.
334  * \exception Throws std::exception on index out of bound.
335  */
336 void CColouredPointsMap::setPointColor(size_t index, float R, float G, float B)
337 {
338  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
339  this->m_color_R[index] = R;
340  this->m_color_G[index] = G;
341  this->m_color_B[index] = B;
342  // mark_as_modified(); // No need to rebuild KD-trees, etc...
343 }
344 
345 void CColouredPointsMap::insertPointFast(float x, float y, float z)
346 {
347  m_x.push_back(x);
348  m_y.push_back(y);
349  m_z.push_back(z);
350  m_color_R.push_back(1);
351  m_color_G.push_back(1);
352  m_color_B.push_back(1);
353 
354  // mark_as_modified(); -> Fast
355 }
356 
357 void CColouredPointsMap::insertPointRGB(
358  float x, float y, float z, float R, float G, float B)
359 {
360  m_x.push_back(x);
361  m_y.push_back(y);
362  m_z.push_back(z);
363  m_color_R.push_back(R);
364  m_color_G.push_back(G);
365  m_color_B.push_back(B);
366 
367  mark_as_modified();
368 }
369 
370 /*---------------------------------------------------------------
371 getAs3DObject
372 ---------------------------------------------------------------*/
373 void CColouredPointsMap::getAs3DObject(
374  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
375 {
376  ASSERT_(outObj);
377 
378  if (!genericMapParams.enableSaveAs3DObject) return;
379 
381  std::make_shared<opengl::CPointCloudColoured>();
382 
383  obj->loadFromPointsMap(this);
384  obj->setColor(1, 1, 1, 1.0);
385 
386  obj->setPointSize(this->renderOptions.point_size);
387 
388  outObj->insert(obj);
389 }
390 
391 /*---------------------------------------------------------------
392 TColourOptions
393 ---------------------------------------------------------------*/
394 CColouredPointsMap::TColourOptions::TColourOptions()
395 
396  = default;
397 
398 void CColouredPointsMap::TColourOptions::loadFromConfigFile(
399  const CConfigFileBase& source, const std::string& section)
400 {
401  scheme = source.read_enum(section, "scheme", scheme);
402  MRPT_LOAD_CONFIG_VAR(z_min, float, source, section)
403  MRPT_LOAD_CONFIG_VAR(z_max, float, source, section)
404  MRPT_LOAD_CONFIG_VAR(d_max, float, source, section)
405 }
406 
407 void CColouredPointsMap::TColourOptions::dumpToTextStream(
408  std::ostream& out) const
409 {
410  out << "\n----------- [CColouredPointsMap::TColourOptions] ------------ "
411  "\n\n";
412 
413  out << "scheme = " << scheme << endl;
414  out << "z_min = " << z_min << endl;
415  out << "z_max = " << z_max << endl;
416  out << "d_max = " << d_max << endl;
417 }
418 
419 void CColouredPointsMap::getPointRGB(
420  size_t index, float& x, float& y, float& z, float& R, float& G,
421  float& B) const
422 {
423  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
424 
425  x = m_x[index];
426  y = m_y[index];
427  z = m_z[index];
428  R = m_color_R[index];
429  G = m_color_G[index];
430  B = m_color_B[index];
431 }
432 
433 /** Retrieves a point color (colors range is [0,1])
434  */
435 void CColouredPointsMap::getPointColor(
436  size_t index, float& R, float& G, float& B) const
437 {
438  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
439 
440  R = m_color_R[index];
441  G = m_color_G[index];
442  B = m_color_B[index];
443 }
444 
445 // This function is duplicated from
446 // "mrpt::vision::pinhole::projectPoint_with_distortion", to avoid
447 // a dependency on mrpt-vision.
449  const mrpt::math::TPoint3D& P, const TCamera& params, TPixelCoordf& pixel,
450  bool accept_points_behind)
451 {
452  MRPT_UNUSED_PARAM(accept_points_behind);
453  // Pinhole model:
454  const double x = P.x / P.z;
455  const double y = P.y / P.z;
456 
457  // Radial distortion:
458  const double r2 = square(x) + square(y);
459  const double r4 = square(r2);
460  const double r6 = r2 * r4;
461 
462  pixel.x = params.cx() +
463  params.fx() * (x * (1 + params.dist[0] * r2 +
464  params.dist[1] * r4 + params.dist[4] * r6) +
465  2 * params.dist[2] * x * y +
466  params.dist[3] * (r2 + 2 * square(x)));
467  pixel.y = params.cy() +
468  params.fy() * (y * (1 + params.dist[0] * r2 +
469  params.dist[1] * r4 + params.dist[4] * r6) +
470  2 * params.dist[3] * x * y +
471  params.dist[2] * (r2 + 2 * square(y)));
472 }
473 
474 /*---------------------------------------------------------------
475 getPoint
476 ---------------------------------------------------------------*/
477 bool CColouredPointsMap::colourFromObservation(
478  const CObservationImage& obs, const CPose3D& robotPose)
479 {
480  // Check if image is not grayscale
481  ASSERT_(obs.image.isColor());
482 
483  CPose3D cameraPoseR; // Get Camera Pose on the Robot(B) (CPose3D)
484  CPose3D cameraPoseW; // World Camera Pose
485 
486  obs.getSensorPose(cameraPoseR);
487  cameraPoseW = robotPose + cameraPoseR; // Camera Global Coordinates
488 
489  // Image Information
490  unsigned int imgW = obs.image.getWidth();
491  unsigned int imgH = obs.image.getHeight();
492 
493  // Projection related variables
494  std::vector<TPixelCoordf>
495  projectedPoints; // The set of projected points in the image
496  std::vector<TPixelCoordf>::iterator
497  itProPoints; // Iterator for projectedPoints
498  std::vector<size_t> p_idx;
499  std::vector<float> p_dist;
500  std::vector<unsigned int> p_proj;
501 
502  // Get the N closest points
503  kdTreeNClosestPoint2DIdx(
504  cameraPoseW.x(), cameraPoseW.y(), // query point
505  200000, // number of points to search
506  p_idx, p_dist); // indexes and distances of the returned points
507 
508  // Fill p3D vector
509  for (size_t k = 0; k < p_idx.size(); k++)
510  {
511  float d = sqrt(p_dist[k]);
512  size_t idx = p_idx[k];
513  if (d < colorScheme.d_max) // && d < m_min_dist[idx] )
514  {
515  TPixelCoordf px;
517  TPoint3D(m_x[idx], m_y[idx], m_z[idx]), obs.cameraParams, px,
518  true);
519  projectedPoints.push_back(px);
520  p_proj.push_back(k);
521  } // end if
522  } // end for
523 
524  // Get channel order
525  unsigned int chR, chG, chB;
526  if (obs.image.getChannelsOrder()[0] == 'B')
527  {
528  chR = 2;
529  chG = 1;
530  chB = 0;
531  }
532  else
533  {
534  chR = 0;
535  chG = 1;
536  chB = 2;
537  }
538 
539  unsigned int n_proj = 0;
540  const float factor = 1.0f / 255; // Normalize pixels:
541 
542  // Get the colour of the projected points
543  size_t k;
544  for (itProPoints = projectedPoints.begin(), k = 0;
545  itProPoints != projectedPoints.end(); ++itProPoints, ++k)
546  {
547  // Only get the points projected inside the image
548  if (itProPoints->x >= 0 && itProPoints->x < imgW &&
549  itProPoints->y > 0 && itProPoints->y < imgH)
550  {
551  unsigned int ii = p_idx[p_proj[k]];
552  uint8_t* p = obs.image(
553  (unsigned int)itProPoints->x, (unsigned int)itProPoints->y);
554 
555  m_color_R[ii] = p[chR] * factor; // R
556  m_color_G[ii] = p[chG] * factor; // G
557  m_color_B[ii] = p[chB] * factor; // B
558  // m_min_dist[ii] = p_dist[p_proj[k]];
559 
560  n_proj++;
561  }
562  } // end for
563 
564  return true;
565 } // end colourFromObservation
566 
567 void CColouredPointsMap::resetPointsMinDist(float defValue)
568 {
569  MRPT_UNUSED_PARAM(defValue);
570  // m_min_dist.assign(x.size(),defValue);
571 }
572 
573 bool CColouredPointsMap::save3D_and_colour_to_text_file(
574  const std::string& file) const
575 {
576  FILE* f = os::fopen(file.c_str(), "wt");
577  if (!f) return false;
578 
579  for (unsigned int i = 0; i < m_x.size(); i++)
580  os::fprintf(
581  f, "%f %f %f %d %d %d\n", m_x[i], m_y[i], m_z[i],
582  (uint8_t)(255 * m_color_R[i]), (uint8_t)(255 * m_color_G[i]),
583  (uint8_t)(255 * m_color_B[i]));
584  // os::fprintf(f,"%f %f %f %f %f %f
585  //%f\n",x[i],y[i],z[i],m_color_R[i],m_color_G[i],m_color_B[i],m_min_dist[i]);
586 
587  os::fclose(f);
588  return true;
589 }
590 
591 // ================================ PLY files import & export virtual methods
592 // ================================
593 
594 /** In a base class, reserve memory to prepare subsequent calls to
595  * PLY_import_set_vertex */
596 void CColouredPointsMap::PLY_import_set_vertex_count(const size_t N)
597 {
598  this->setSize(N);
599 }
600 
601 /** In a base class, will be called after PLY_import_set_vertex_count() once for
602  * each loaded point.
603  * \param pt_color Will be nullptr if the loaded file does not provide color
604  * info.
605  */
606 void CColouredPointsMap::PLY_import_set_vertex(
607  const size_t idx, const mrpt::math::TPoint3Df& pt, const TColorf* pt_color)
608 {
609  if (pt_color)
610  this->setPointRGB(
611  idx, pt.x, pt.y, pt.z, pt_color->R, pt_color->G, pt_color->B);
612  else
613  this->setPoint(idx, pt.x, pt.y, pt.z);
614 }
615 
616 /** In a base class, will be called after PLY_export_get_vertex_count() once for
617  * each exported point.
618  * \param pt_color Will be nullptr if the loaded file does not provide color
619  * info.
620  */
621 void CColouredPointsMap::PLY_export_get_vertex(
622  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
623  TColorf& pt_color) const
624 {
625  pt_has_color = true;
626 
627  pt.x = m_x[idx];
628  pt.y = m_y[idx];
629  pt.z = m_z[idx];
630 
631  pt_color.R = m_color_R[idx];
632  pt_color.G = m_color_G[idx];
633  pt_color.B = m_color_B[idx];
634 }
635 
636 /*---------------------------------------------------------------
637 addFrom_classSpecific
638 ---------------------------------------------------------------*/
639 void CColouredPointsMap::addFrom_classSpecific(
640  const CPointsMap& anotherMap, const size_t nPreviousPoints)
641 {
642  const size_t nOther = anotherMap.size();
643 
644  // Specific data for this class:
645  const auto* anotheMap_col =
646  dynamic_cast<const CColouredPointsMap*>(&anotherMap);
647 
648  if (anotheMap_col)
649  {
650  for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
651  {
652  m_color_R[j] = anotheMap_col->m_color_R[i];
653  m_color_G[j] = anotheMap_col->m_color_G[i];
654  m_color_B[j] = anotheMap_col->m_color_B[i];
655  }
656  }
657 }
658 
659 namespace mrpt::maps::detail
660 {
662 
663 template <>
665 {
666  /** Helper method fot the generic implementation of
667  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
668  * points - this is the place to reserve memory in lric for extra working
669  * variables. */
671  CColouredPointsMap& me,
673  {
674  // Vars:
675  // [0] -> pR
676  // [1] -> pG
677  // [2] -> pB
678  // [3] -> Az_1_color
679  lric.fVars.resize(4);
680 
682  lric.fVars[3] = 1.0 / (me.colorScheme.z_max - me.colorScheme.z_min);
683  }
684 
685  /** Helper method fot the generic implementation of
686  * CPointsMap::loadFromRangeScan(), to be called once per range data */
688  CColouredPointsMap& me, const float gx, const float gy, const float gz,
690  {
691  MRPT_UNUSED_PARAM(gx);
692  MRPT_UNUSED_PARAM(gy);
693  // Relative height of the point wrt the sensor:
694  const float rel_z = gz - lric.HM(2, 3); // m23;
695 
696  // Variable renaming:
697  float& pR = lric.fVars[0];
698  float& pG = lric.fVars[1];
699  float& pB = lric.fVars[2];
700  float Az_1_color = lric.fVars[3];
701 
702  // Compute color:
703  switch (me.colorScheme.scheme)
704  {
705  // case cmFromHeightRelativeToSensor:
706  case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
707  case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
708  {
709  float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
710  if (q < 0)
711  q = 0;
712  else if (q > 1)
713  q = 1;
714 
715  if (me.colorScheme.scheme ==
716  CColouredPointsMap::cmFromHeightRelativeToSensorGray)
717  {
718  pR = pG = pB = q;
719  }
720  else
721  {
722  jet2rgb(q, pR, pG, pB);
723  }
724  }
725  break;
726  case CColouredPointsMap::cmFromIntensityImage:
727  {
728  // In 2D scans we don't have this channel.
729  pR = pG = pB = 1.0;
730  }
731  break;
732  default:
733  THROW_EXCEPTION("Unknown color scheme");
734  }
735  }
736  /** Helper method fot the generic implementation of
737  * CPointsMap::loadFromRangeScan(), to be called after each
738  * "{x,y,z}.push_back(...);" */
740  CColouredPointsMap& me,
742  {
743  float& pR = lric.fVars[0];
744  float& pG = lric.fVars[1];
745  float& pB = lric.fVars[2];
746  me.m_color_R.push_back(pR);
747  me.m_color_G.push_back(pG);
748  me.m_color_B.push_back(pB);
749  // m_min_dist.push_back(1e4);
750  }
751 
752  /** Helper method fot the generic implementation of
753  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
754  * points - this is the place to reserve memory in lric for extra working
755  * variables. */
757  CColouredPointsMap& me,
759  {
760  // Vars:
761  // [0] -> pR
762  // [1] -> pG
763  // [2] -> pB
764  // [3] -> Az_1_color
765  // [4] -> K_8u
766  // [5] -> cx
767  // [6] -> cy
768  // [7] -> fx
769  // [8] -> fy
770  lric.fVars.resize(9);
771  float& cx = lric.fVars[5];
772  float& cy = lric.fVars[6];
773  float& fx = lric.fVars[7];
774  float& fy = lric.fVars[8];
775 
776  // unsigned int vars:
777  // [0] -> imgW
778  // [1] -> imgH
779  // [2] -> img_idx_x
780  // [3] -> img_idx_y
781  lric.uVars.resize(4);
782  unsigned int& imgW = lric.uVars[0];
783  unsigned int& imgH = lric.uVars[1];
784  unsigned int& img_idx_x = lric.uVars[2];
785  unsigned int& img_idx_y = lric.uVars[3];
786 
787  // bool vars:
788  // [0] -> hasValidIntensityImage
789  // [1] -> hasColorIntensityImg
790  // [2] -> simple_3d_to_color_relation
791  lric.bVars.resize(3);
792  uint8_t& hasValidIntensityImage = lric.bVars[0];
793  uint8_t& hasColorIntensityImg = lric.bVars[1];
794  uint8_t& simple_3d_to_color_relation = lric.bVars[2];
795 
797  lric.fVars[3] = 1.0 / (me.colorScheme.z_max -
798  me.colorScheme.z_min); // Az_1_color = ...
799  lric.fVars[4] = 1.0f / 255; // K_8u
800 
801  hasValidIntensityImage = false;
802  imgW = 0;
803  imgH = 0;
804 
805  if (lric.rangeScan.hasIntensityImage)
806  {
807  // assure the size matches:
808  if (lric.rangeScan.points3D_x.size() ==
811  {
812  hasValidIntensityImage = true;
813  imgW = lric.rangeScan.intensityImage.getWidth();
814  imgH = lric.rangeScan.intensityImage.getHeight();
815  }
816  }
817 
818  hasColorIntensityImg =
819  hasValidIntensityImage && lric.rangeScan.intensityImage.isColor();
820 
821  // running indices for the image pixels for the gray levels:
822  // If both range & intensity images coincide (e.g. SwissRanger), then we
823  // can just
824  // assign 3D points to image pixels one-to-one, but that's not the case
825  // if
826  //
827  simple_3d_to_color_relation =
828  (std::abs(lric.rangeScan.relativePoseIntensityWRTDepth.norm()) <
829  1e-5);
830  img_idx_x = 0;
831  img_idx_y = 0;
832 
833  // Will be used just if simple_3d_to_color_relation=false
834  cx = lric.rangeScan.cameraParamsIntensity.cx();
835  cy = lric.rangeScan.cameraParamsIntensity.cy();
836  fx = lric.rangeScan.cameraParamsIntensity.fx();
837  fy = lric.rangeScan.cameraParamsIntensity.fy();
838  }
839 
840  /** Helper method fot the generic implementation of
841  * CPointsMap::loadFromRangeScan(), to be called once per range data */
843  CColouredPointsMap& me, const float gx, const float gy, const float gz,
845  {
846  MRPT_UNUSED_PARAM(gx);
847  MRPT_UNUSED_PARAM(gy);
848  // Rename variables:
849  float& pR = lric.fVars[0];
850  float& pG = lric.fVars[1];
851  float& pB = lric.fVars[2];
852  float& Az_1_color = lric.fVars[3];
853  float& K_8u = lric.fVars[4];
854  float& cx = lric.fVars[5];
855  float& cy = lric.fVars[6];
856  float& fx = lric.fVars[7];
857  float& fy = lric.fVars[8];
858 
859  unsigned int& imgW = lric.uVars[0];
860  unsigned int& imgH = lric.uVars[1];
861  unsigned int& img_idx_x = lric.uVars[2];
862  unsigned int& img_idx_y = lric.uVars[3];
863 
864  const uint8_t& hasValidIntensityImage = lric.bVars[0];
865  const uint8_t& hasColorIntensityImg = lric.bVars[1];
866  const uint8_t& simple_3d_to_color_relation = lric.bVars[2];
867 
868  // Relative height of the point wrt the sensor:
869  const float rel_z = gz - lric.HM(2, 3); // m23;
870 
871  // Compute color:
872  switch (me.colorScheme.scheme)
873  {
874  // case cmFromHeightRelativeToSensor:
875  case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
876  case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
877  {
878  float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
879  if (q < 0)
880  q = 0;
881  else if (q > 1)
882  q = 1;
883 
884  if (me.colorScheme.scheme ==
885  CColouredPointsMap::cmFromHeightRelativeToSensorGray)
886  {
887  pR = pG = pB = q;
888  }
889  else
890  {
891  jet2rgb(q, pR, pG, pB);
892  }
893  }
894  break;
895  case CColouredPointsMap::cmFromIntensityImage:
896  {
897  // Do we have to project the 3D point into the image plane??
898  bool hasValidColor = false;
899  if (simple_3d_to_color_relation)
900  {
901  hasValidColor = true;
902  }
903  else
904  {
905  // Do projection:
906  TPoint3D pt; // pt_wrt_colorcam;
909  lric.scan_x, lric.scan_y, lric.scan_z, pt.x, pt.y,
910  pt.z);
911 
912  // Project to image plane:
913  if (pt.z)
914  {
915  img_idx_x = cx + fx * pt.x / pt.z;
916  img_idx_y = cy + fy * pt.y / pt.z;
917 
918  hasValidColor = img_idx_x < imgW && // img_idx_x>=0
919  // isn't needed for
920  // unsigned.
921  img_idx_y < imgH;
922  }
923  }
924 
925  const auto& ii = lric.rangeScan.intensityImage;
926  if (hasValidColor && hasColorIntensityImg)
927  {
928  const auto* c = ii.ptr<uint8_t>(img_idx_x, img_idx_y);
929  pR = c[2] * K_8u;
930  pG = c[1] * K_8u;
931  pB = c[0] * K_8u;
932  }
933  else if (hasValidColor && hasValidIntensityImage)
934  {
935  const auto c = ii.at<uint8_t>(img_idx_x, img_idx_y);
936  pR = pG = pB = c * K_8u;
937  }
938  else
939  {
940  pR = pG = pB = 1.0;
941  }
942  }
943  break;
944  default:
945  THROW_EXCEPTION("Unknown color scheme");
946  }
947  }
948 
949  /** Helper method fot the generic implementation of
950  * CPointsMap::loadFromRangeScan(), to be called after each
951  * "{x,y,z}.push_back(...);" */
953  CColouredPointsMap& me,
955  {
956  float& pR = lric.fVars[0];
957  float& pG = lric.fVars[1];
958  float& pB = lric.fVars[2];
959 
960  me.m_color_R.push_back(pR);
961  me.m_color_G.push_back(pG);
962  me.m_color_B.push_back(pB);
963 
964  // m_min_dist.push_back(1e4);
965  }
966 
967  /** Helper method fot the generic implementation of
968  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
969  * end */
971  CColouredPointsMap& me,
973  {
974  MRPT_UNUSED_PARAM(me);
975  unsigned int& imgW = lric.uVars[0];
976  unsigned int& img_idx_x = lric.uVars[2];
977  unsigned int& img_idx_y = lric.uVars[3];
978 
979  const uint8_t& hasValidIntensityImage = lric.bVars[0];
980  const uint8_t& simple_3d_to_color_relation = lric.bVars[2];
981 
982  // Advance the color pointer (just for simple cases, e.g. SwissRanger,
983  // not for Kinect)
984  if (simple_3d_to_color_relation && hasValidIntensityImage)
985  {
986  if (++img_idx_x >= imgW)
987  {
988  img_idx_y++;
989  img_idx_x = 0;
990  }
991  }
992  }
993 };
994 } // namespace mrpt::maps::detail
995 /** See CPointsMap::loadFromRangeScan() */
996 void CColouredPointsMap::loadFromRangeScan(
997  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
998 {
1000  templ_loadFromRangeScan(*this, rangeScan, robotPose);
1001 }
1002 
1003 /** See CPointsMap::loadFromRangeScan() */
1004 void CColouredPointsMap::loadFromRangeScan(
1005  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
1006 {
1008  templ_loadFromRangeScan(*this, rangeScan, robotPose);
1009 }
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:174
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
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt) const
Computes the 3D point L such as .
GLenum GLsizei n
Definition: glext.h:5136
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:176
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:878
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:857
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.
void reserve(size_t newLength) override
Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
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:847
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:172
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
T square(const T x)
Inline function for the square of a number.
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
Parameters for the Brown-Conrady camera lens distortion 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:170
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
Definition: CImage.cpp:888
void vector_strong_clear(VECTOR_T &v)
Like calling a std::vector<>&#39;s clear() method, but really forcing deallocating the memory...
Definition: bits_mem.h:18
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
bool hasIntensityImage
true means the field intensityImage contains valid data
A RGB color - floats in the range [0,1].
Definition: TColor.h:78
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:582
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
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
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: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
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:441
images resize(NUM_IMGS)
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: d655b8ca0 Wed Dec 4 23:23:44 2019 +0100 at miƩ dic 4 23:30:15 CET 2019