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



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020