MRPT  2.0.4
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-2020, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
12 #include <mrpt/core/bits_mem.h>
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);
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  [[maybe_unused]] bool accept_points_behind)
451 {
452  // Pinhole model:
453  const double x = P.x / P.z;
454  const double y = P.y / P.z;
455 
456  // Radial distortion:
457  const double r2 = square(x) + square(y);
458  const double r4 = square(r2);
459  const double r6 = r2 * r4;
460 
461  pixel.x = params.cx() +
462  params.fx() * (x * (1 + params.dist[0] * r2 +
463  params.dist[1] * r4 + params.dist[4] * r6) +
464  2 * params.dist[2] * x * y +
465  params.dist[3] * (r2 + 2 * square(x)));
466  pixel.y = params.cy() +
467  params.fy() * (y * (1 + params.dist[0] * r2 +
468  params.dist[1] * r4 + params.dist[4] * r6) +
469  2 * params.dist[3] * x * y +
470  params.dist[2] * (r2 + 2 * square(y)));
471 }
472 
473 /*---------------------------------------------------------------
474 getPoint
475 ---------------------------------------------------------------*/
476 bool CColouredPointsMap::colourFromObservation(
477  const CObservationImage& obs, const CPose3D& robotPose)
478 {
479  // Check if image is not grayscale
480  ASSERT_(obs.image.isColor());
481 
482  CPose3D cameraPoseR; // Get Camera Pose on the Robot(B) (CPose3D)
483  CPose3D cameraPoseW; // World Camera Pose
484 
485  obs.getSensorPose(cameraPoseR);
486  cameraPoseW = robotPose + cameraPoseR; // Camera Global Coordinates
487 
488  // Image Information
489  unsigned int imgW = obs.image.getWidth();
490  unsigned int imgH = obs.image.getHeight();
491 
492  // Projection related variables
493  std::vector<TPixelCoordf>
494  projectedPoints; // The set of projected points in the image
495  std::vector<TPixelCoordf>::iterator
496  itProPoints; // Iterator for projectedPoints
497  std::vector<size_t> p_idx;
498  std::vector<float> p_dist;
499  std::vector<unsigned int> p_proj;
500 
501  // Get the N closest points
502  kdTreeNClosestPoint2DIdx(
503  cameraPoseW.x(), cameraPoseW.y(), // query point
504  200000, // number of points to search
505  p_idx, p_dist); // indexes and distances of the returned points
506 
507  // Fill p3D vector
508  for (size_t k = 0; k < p_idx.size(); k++)
509  {
510  float d = sqrt(p_dist[k]);
511  size_t idx = p_idx[k];
512  if (d < colorScheme.d_max) // && d < m_min_dist[idx] )
513  {
514  TPixelCoordf px;
516  TPoint3D(m_x[idx], m_y[idx], m_z[idx]), obs.cameraParams, px,
517  true);
518  projectedPoints.push_back(px);
519  p_proj.push_back(k);
520  } // end if
521  } // end for
522 
523  // Get channel order
524  unsigned int chR, chG, chB;
525  if (obs.image.getChannelsOrder()[0] == 'B')
526  {
527  chR = 2;
528  chG = 1;
529  chB = 0;
530  }
531  else
532  {
533  chR = 0;
534  chG = 1;
535  chB = 2;
536  }
537 
538  unsigned int n_proj = 0;
539  const float factor = 1.0f / 255; // Normalize pixels:
540 
541  // Get the colour of the projected points
542  size_t k;
543  for (itProPoints = projectedPoints.begin(), k = 0;
544  itProPoints != projectedPoints.end(); ++itProPoints, ++k)
545  {
546  // Only get the points projected inside the image
547  if (itProPoints->x >= 0 && itProPoints->x < imgW &&
548  itProPoints->y > 0 && itProPoints->y < imgH)
549  {
550  unsigned int ii = p_idx[p_proj[k]];
551  uint8_t* p = obs.image(
552  (unsigned int)itProPoints->x, (unsigned int)itProPoints->y);
553 
554  m_color_R[ii] = p[chR] * factor; // R
555  m_color_G[ii] = p[chG] * factor; // G
556  m_color_B[ii] = p[chB] * factor; // B
557  // m_min_dist[ii] = p_dist[p_proj[k]];
558 
559  n_proj++;
560  }
561  } // end for
562 
563  return true;
564 } // end colourFromObservation
565 
566 void CColouredPointsMap::resetPointsMinDist([[maybe_unused]] float defValue)
567 {
568  // m_min_dist.assign(x.size(),defValue);
569 }
570 
571 bool CColouredPointsMap::save3D_and_colour_to_text_file(
572  const std::string& file) const
573 {
574  FILE* f = os::fopen(file.c_str(), "wt");
575  if (!f) return false;
576 
577  for (size_t i = 0; i < m_x.size(); i++)
578  os::fprintf(
579  f, "%f %f %f %d %d %d\n", m_x[i], m_y[i], m_z[i],
580  (uint8_t)(255 * m_color_R[i]), (uint8_t)(255 * m_color_G[i]),
581  (uint8_t)(255 * m_color_B[i]));
582 
583  os::fclose(f);
584  return true;
585 }
586 
587 // ================================ PLY files import & export virtual methods
588 // ================================
589 
590 /** In a base class, reserve memory to prepare subsequent calls to
591  * PLY_import_set_vertex */
592 void CColouredPointsMap::PLY_import_set_vertex_count(const size_t N)
593 {
594  this->setSize(N);
595 }
596 
597 /** In a base class, will be called after PLY_import_set_vertex_count() once for
598  * each loaded point.
599  * \param pt_color Will be nullptr if the loaded file does not provide color
600  * info.
601  */
602 void CColouredPointsMap::PLY_import_set_vertex(
603  const size_t idx, const mrpt::math::TPoint3Df& pt, const TColorf* pt_color)
604 {
605  if (pt_color)
606  this->setPointRGB(
607  idx, pt.x, pt.y, pt.z, pt_color->R, pt_color->G, pt_color->B);
608  else
609  this->setPoint(idx, pt.x, pt.y, pt.z);
610 }
611 
612 /** In a base class, will be called after PLY_export_get_vertex_count() once for
613  * each exported point.
614  * \param pt_color Will be nullptr if the loaded file does not provide color
615  * info.
616  */
617 void CColouredPointsMap::PLY_export_get_vertex(
618  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
619  TColorf& pt_color) const
620 {
621  pt_has_color = true;
622 
623  pt.x = m_x[idx];
624  pt.y = m_y[idx];
625  pt.z = m_z[idx];
626 
627  pt_color.R = m_color_R[idx];
628  pt_color.G = m_color_G[idx];
629  pt_color.B = m_color_B[idx];
630 }
631 
632 /*---------------------------------------------------------------
633 addFrom_classSpecific
634 ---------------------------------------------------------------*/
635 void CColouredPointsMap::addFrom_classSpecific(
636  const CPointsMap& anotherMap, const size_t nPreviousPoints)
637 {
638  const size_t nOther = anotherMap.size();
639 
640  // Specific data for this class:
641  const auto* anotheMap_col =
642  dynamic_cast<const CColouredPointsMap*>(&anotherMap);
643 
644  if (anotheMap_col)
645  {
646  for (size_t i = 0, j = nPreviousPoints; i < nOther; i++, j++)
647  {
648  m_color_R[j] = anotheMap_col->m_color_R[i];
649  m_color_G[j] = anotheMap_col->m_color_G[i];
650  m_color_B[j] = anotheMap_col->m_color_B[i];
651  }
652  }
653 }
654 
655 namespace mrpt::maps::detail
656 {
658 
659 template <>
661 {
662  /** Helper method fot the generic implementation of
663  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
664  * points - this is the place to reserve memory in lric for extra working
665  * variables. */
667  CColouredPointsMap& me,
669  {
670  // Vars:
671  // [0] -> pR
672  // [1] -> pG
673  // [2] -> pB
674  // [3] -> Az_1_color
675  lric.fVars.resize(4);
676 
678  lric.fVars[3] = 1.0 / (me.colorScheme.z_max - me.colorScheme.z_min);
679  }
680 
681  /** Helper method fot the generic implementation of
682  * CPointsMap::loadFromRangeScan(), to be called once per range data */
684  CColouredPointsMap& me, [[maybe_unused]] const float gx,
685  [[maybe_unused]] const float gy, const float gz,
687  {
688  // Relative height of the point wrt the sensor:
689  const float rel_z = gz - lric.HM(2, 3); // m23;
690 
691  // Variable renaming:
692  float& pR = lric.fVars[0];
693  float& pG = lric.fVars[1];
694  float& pB = lric.fVars[2];
695  float Az_1_color = lric.fVars[3];
696 
697  // Compute color:
698  switch (me.colorScheme.scheme)
699  {
700  // case cmFromHeightRelativeToSensor:
701  case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
702  case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
703  {
704  float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
705  if (q < 0)
706  q = 0;
707  else if (q > 1)
708  q = 1;
709 
710  if (me.colorScheme.scheme ==
711  CColouredPointsMap::cmFromHeightRelativeToSensorGray)
712  {
713  pR = pG = pB = q;
714  }
715  else
716  {
717  jet2rgb(q, pR, pG, pB);
718  }
719  }
720  break;
721  case CColouredPointsMap::cmFromIntensityImage:
722  {
723  // In 2D scans we don't have this channel.
724  pR = pG = pB = 1.0;
725  }
726  break;
727  default:
728  THROW_EXCEPTION("Unknown color scheme");
729  }
730  }
731  /** Helper method fot the generic implementation of
732  * CPointsMap::loadFromRangeScan(), to be called after each
733  * "{x,y,z}.push_back(...);" */
735  CColouredPointsMap& me,
737  {
738  float& pR = lric.fVars[0];
739  float& pG = lric.fVars[1];
740  float& pB = lric.fVars[2];
741  me.m_color_R.push_back(pR);
742  me.m_color_G.push_back(pG);
743  me.m_color_B.push_back(pB);
744  // m_min_dist.push_back(1e4);
745  }
746 
747  /** Helper method fot the generic implementation of
748  * CPointsMap::loadFromRangeScan(), to be called only once before inserting
749  * points - this is the place to reserve memory in lric for extra working
750  * variables. */
752  CColouredPointsMap& me,
754  {
755  // Vars:
756  // [0] -> pR
757  // [1] -> pG
758  // [2] -> pB
759  // [3] -> Az_1_color
760  // [4] -> K_8u
761  // [5] -> cx
762  // [6] -> cy
763  // [7] -> fx
764  // [8] -> fy
765  lric.fVars.resize(9);
766  float& cx = lric.fVars[5];
767  float& cy = lric.fVars[6];
768  float& fx = lric.fVars[7];
769  float& fy = lric.fVars[8];
770 
771  // unsigned int vars:
772  // [0] -> imgW
773  // [1] -> imgH
774  // [2] -> img_idx_x
775  // [3] -> img_idx_y
776  lric.uVars.resize(4);
777  unsigned int& imgW = lric.uVars[0];
778  unsigned int& imgH = lric.uVars[1];
779  unsigned int& img_idx_x = lric.uVars[2];
780  unsigned int& img_idx_y = lric.uVars[3];
781 
782  // bool vars:
783  // [0] -> hasValidIntensityImage
784  // [1] -> hasColorIntensityImg
785  // [2] -> simple_3d_to_color_relation
786  lric.bVars.resize(3);
787  uint8_t& hasValidIntensityImage = lric.bVars[0];
788  uint8_t& hasColorIntensityImg = lric.bVars[1];
789  uint8_t& simple_3d_to_color_relation = lric.bVars[2];
790 
792  lric.fVars[3] = 1.0 / (me.colorScheme.z_max -
793  me.colorScheme.z_min); // Az_1_color = ...
794  lric.fVars[4] = 1.0f / 255; // K_8u
795 
796  hasValidIntensityImage = false;
797  imgW = 0;
798  imgH = 0;
799 
800  if (lric.rangeScan.hasIntensityImage)
801  {
802  // assure the size matches:
803  if (lric.rangeScan.points3D_x.size() ==
806  {
807  hasValidIntensityImage = true;
808  imgW = lric.rangeScan.intensityImage.getWidth();
809  imgH = lric.rangeScan.intensityImage.getHeight();
810  }
811  }
812 
813  hasColorIntensityImg =
814  hasValidIntensityImage && lric.rangeScan.intensityImage.isColor();
815 
816  // running indices for the image pixels for the gray levels:
817  // If both range & intensity images coincide (e.g. SwissRanger), then we
818  // can just
819  // assign 3D points to image pixels one-to-one, but that's not the case
820  // if
821  //
822  simple_3d_to_color_relation =
823  (std::abs(lric.rangeScan.relativePoseIntensityWRTDepth.norm()) <
824  1e-5);
825  img_idx_x = 0;
826  img_idx_y = 0;
827 
828  // Will be used just if simple_3d_to_color_relation=false
829  cx = lric.rangeScan.cameraParamsIntensity.cx();
830  cy = lric.rangeScan.cameraParamsIntensity.cy();
831  fx = lric.rangeScan.cameraParamsIntensity.fx();
832  fy = lric.rangeScan.cameraParamsIntensity.fy();
833  }
834 
835  /** Helper method fot the generic implementation of
836  * CPointsMap::loadFromRangeScan(), to be called once per range data */
838  CColouredPointsMap& me, [[maybe_unused]] const float gx,
839  [[maybe_unused]] const float gy, const float gz,
841  {
842  // Rename variables:
843  float& pR = lric.fVars[0];
844  float& pG = lric.fVars[1];
845  float& pB = lric.fVars[2];
846  float& Az_1_color = lric.fVars[3];
847  float& K_8u = lric.fVars[4];
848  float& cx = lric.fVars[5];
849  float& cy = lric.fVars[6];
850  float& fx = lric.fVars[7];
851  float& fy = lric.fVars[8];
852 
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  const uint8_t& hasValidIntensityImage = lric.bVars[0];
859  const uint8_t& hasColorIntensityImg = lric.bVars[1];
860  const uint8_t& simple_3d_to_color_relation = lric.bVars[2];
861 
862  // Relative height of the point wrt the sensor:
863  const float rel_z = gz - lric.HM(2, 3); // m23;
864 
865  // Compute color:
866  switch (me.colorScheme.scheme)
867  {
868  // case cmFromHeightRelativeToSensor:
869  case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
870  case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
871  {
872  float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
873  if (q < 0)
874  q = 0;
875  else if (q > 1)
876  q = 1;
877 
878  if (me.colorScheme.scheme ==
879  CColouredPointsMap::cmFromHeightRelativeToSensorGray)
880  {
881  pR = pG = pB = q;
882  }
883  else
884  {
885  jet2rgb(q, pR, pG, pB);
886  }
887  }
888  break;
889  case CColouredPointsMap::cmFromIntensityImage:
890  {
891  // Do we have to project the 3D point into the image plane??
892  bool hasValidColor = false;
893  if (simple_3d_to_color_relation)
894  {
895  hasValidColor = true;
896  }
897  else
898  {
899  // Do projection:
900  TPoint3D pt; // pt_wrt_colorcam;
903  lric.scan_x, lric.scan_y, lric.scan_z, pt.x, pt.y,
904  pt.z);
905 
906  // Project to image plane:
907  if (pt.z)
908  {
909  img_idx_x = cx + fx * pt.x / pt.z;
910  img_idx_y = cy + fy * pt.y / pt.z;
911 
912  hasValidColor = img_idx_x < imgW && // img_idx_x>=0
913  // isn't needed for
914  // unsigned.
915  img_idx_y < imgH;
916  }
917  }
918 
919  const auto& ii = lric.rangeScan.intensityImage;
920  if (hasValidColor && hasColorIntensityImg)
921  {
922  const auto* c = ii.ptr<uint8_t>(img_idx_x, img_idx_y);
923  pR = c[2] * K_8u;
924  pG = c[1] * K_8u;
925  pB = c[0] * K_8u;
926  }
927  else if (hasValidColor && hasValidIntensityImage)
928  {
929  const auto c = ii.at<uint8_t>(img_idx_x, img_idx_y);
930  pR = pG = pB = c * K_8u;
931  }
932  else
933  {
934  pR = pG = pB = 1.0;
935  }
936  }
937  break;
938  default:
939  THROW_EXCEPTION("Unknown color scheme");
940  }
941  }
942 
943  /** Helper method fot the generic implementation of
944  * CPointsMap::loadFromRangeScan(), to be called after each
945  * "{x,y,z}.push_back(...);" */
947  CColouredPointsMap& me,
949  {
950  float& pR = lric.fVars[0];
951  float& pG = lric.fVars[1];
952  float& pB = lric.fVars[2];
953 
954  me.m_color_R.push_back(pR);
955  me.m_color_G.push_back(pG);
956  me.m_color_B.push_back(pB);
957 
958  // m_min_dist.push_back(1e4);
959  }
960 
961  /** Helper method fot the generic implementation of
962  * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
963  * end */
965  [[maybe_unused]] CColouredPointsMap& me,
967  {
968  unsigned int& imgW = lric.uVars[0];
969  unsigned int& img_idx_x = lric.uVars[2];
970  unsigned int& img_idx_y = lric.uVars[3];
971 
972  const uint8_t& hasValidIntensityImage = lric.bVars[0];
973  const uint8_t& simple_3d_to_color_relation = lric.bVars[2];
974 
975  // Advance the color pointer (just for simple cases, e.g. SwissRanger,
976  // not for Kinect)
977  if (simple_3d_to_color_relation && hasValidIntensityImage)
978  {
979  if (++img_idx_x >= imgW)
980  {
981  img_idx_y++;
982  img_idx_x = 0;
983  }
984  }
985  }
986 };
987 } // namespace mrpt::maps::detail
988 /** See CPointsMap::loadFromRangeScan() */
989 void CColouredPointsMap::loadFromRangeScan(
990  const CObservation2DRangeScan& rangeScan, const CPose3D* robotPose)
991 {
993  templ_loadFromRangeScan(*this, rangeScan, robotPose);
994 }
995 
996 /** See CPointsMap::loadFromRangeScan() */
997 void CColouredPointsMap::loadFromRangeScan(
998  const CObservation3DRangeScan& rangeScan, const CPose3D* robotPose)
999 {
1001  templ_loadFromRangeScan(*this, rangeScan, robotPose);
1002 }
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.
#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:175
const mrpt::obs::CObservation3DRangeScan & rangeScan
Definition: CPointsMap.h:106
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:286
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
const double G
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 .
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:177
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:855
mrpt::vision::TStereoCalibParams params
static void internal_loadFromRangeScan3D_prepareOneRange(CColouredPointsMap &me, [[maybe_unused]] const float gx, [[maybe_unused]] 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...
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:828
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
STL namespace.
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.
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
#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:67
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:818
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:173
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
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
Definition: CPointsMap.h:105
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:109
static void internal_loadFromRangeScan2D_prepareOneRange(CColouredPointsMap &me, [[maybe_unused]] const float gx, [[maybe_unused]] 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...
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:90
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
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
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:111
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:419
return_t square(const num_t x)
Inline function for the square of a number.
#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:171
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
static void aux_projectPoint_with_distortion(const mrpt::math::TPoint3D &P, const TCamera &params, TPixelCoordf &pixel, [[maybe_unused]] bool accept_points_behind)
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:865
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
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
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:583
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
Definition: CPointsMap.h:97
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:268
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...
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:87
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
Definition: CArchive.h:94
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:459
images resize(NUM_IMGS)
static void internal_loadFromRangeScan3D_postOneRange([[maybe_unused]] 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:79
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...



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020