Main MRPT website > C++ reference for MRPT 1.5.5
CPointsMap_crtp_common.h
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 #ifndef cpointsmap_crtp_common_H
10 #define cpointsmap_crtp_common_H
11 
12 #include <mrpt/utils/round.h>
13 #include <mrpt/obs/CObservation3DRangeScan.h>
14 #include <mrpt/obs/CObservation2DRangeScan.h>
15 
16 namespace mrpt
17 {
18 namespace maps
19 {
20 namespace detail
21 {
22 
23  template <class Derived>
24  struct loadFromRangeImpl
25  {
26  static inline void templ_loadFromRangeScan(
27  Derived &obj,
28  const mrpt::obs::CObservation2DRangeScan &rangeScan,
29  const mrpt::poses::CPose3D *robotPose )
30  {
31  using namespace mrpt::poses;
32  using mrpt::math::square;
33  using mrpt::utils::DEG2RAD;
34  obj.mark_as_modified();
35 
36  // If robot pose is supplied, compute sensor pose relative to it.
37  CPose3D sensorPose3D(UNINITIALIZED_POSE);
38  if (!robotPose)
39  sensorPose3D = rangeScan.sensorPose;
40  else sensorPose3D.composeFrom(*robotPose, rangeScan.sensorPose);
41 
42  // Insert vs. load and replace:
43  if (!obj.insertionOptions.addToExistingPointsMap)
44  obj.resize(0); // Resize to 0 instead of clear() so the std::vector<> memory is not actually deadllocated and can be reused.
45 
46  const int sizeRangeScan = rangeScan.scan.size();
47 
48  if (!sizeRangeScan)
49  return; // Nothing to do.
50 
51  // For a great gain in efficiency:
52  if ( obj.x.size()+sizeRangeScan > obj.x.capacity() )
53  {
54  obj.reserve( (size_t) (obj.x.size() * 1.2f) + 3*sizeRangeScan );
55  }
56 
57  // GENERAL CASE OF SCAN WITH ARBITRARY 3D ORIENTATION:
58  // Specialize a bit the equations since we know that z=0 always for the scan in local coordinates:
59  mrpt::maps::CPointsMap::TLaserRange2DInsertContext lric(rangeScan);
60  sensorPose3D.getHomogeneousMatrix(lric.HM);
61 
62  // For quicker access as "float" numbers:
63  float m00 = lric.HM.get_unsafe(0,0);
64  float m01 = lric.HM.get_unsafe(0,1);
65  float m03 = lric.HM.get_unsafe(0,3);
66  float m10 = lric.HM.get_unsafe(1,0);
67  float m11 = lric.HM.get_unsafe(1,1);
68  float m13 = lric.HM.get_unsafe(1,3);
69  float m20 = lric.HM.get_unsafe(2,0);
70  float m21 = lric.HM.get_unsafe(2,1);
71  float m23 = lric.HM.get_unsafe(2,3);
72 
73  float lx_1,ly_1,lz_1,lx=0,ly=0,lz=0; // Punto anterior y actual:
74  float lx_2,ly_2; // Punto antes del anterior
75 
76  // Initial last point:
77  lx_1 = -100; ly_1 = -100; lz_1 = -100;
78  lx_2 = -100; ly_2 = -100;
79 
80  // Minimum distance between points to reduce high density scans:
81  const bool useMinDist = obj.insertionOptions.minDistBetweenLaserPoints>0;
82  const float minDistSqrBetweenLaserPoints = square( obj.insertionOptions.minDistBetweenLaserPoints );
83 
84  // ----------------------------------------------------------------
85  // Transform these points into 3D using the pose transformation:
86  // ----------------------------------------------------------------
87  bool lastPointWasValid = true;
88  bool thisIsTheFirst = true;
89  bool lastPointWasInserted = false;
90 
91  // Initialize extra stuff in derived class:
92  pointmap_traits<Derived>::internal_loadFromRangeScan2D_init(obj, lric);
93 
94  // Resize now for efficiency, if there're invalid or filtered points, buffers
95  // will be reduced at the end:
96  const size_t nPointsAtStart = obj.size();
97  size_t nextPtIdx = nPointsAtStart;
98 
99  {
100  const size_t expectedMaxSize = nPointsAtStart+(sizeRangeScan* (obj.insertionOptions.also_interpolate ? 3:1) );
101  obj.x.resize( expectedMaxSize );
102  obj.y.resize( expectedMaxSize );
103  obj.z.resize( expectedMaxSize );
104  }
105 
106  // ------------------------------------------------------
107  // Pass range scan to a set of 2D points:
108  // ------------------------------------------------------
109  // Use a LUT to convert ranges -> (x,y) ; Automatically computed upon first usage.
110  const mrpt::obs::CSinCosLookUpTableFor2DScans::TSinCosValues & sincos_vals = obj.m_scans_sincos_cache.getSinCosForScan(rangeScan);
111 
112  // Build list of points in global coordinates:
113  Eigen::Array<float,Eigen::Dynamic,1> scan_gx(sizeRangeScan+3), scan_gy(sizeRangeScan+3),scan_gz(sizeRangeScan+3); // The +3 is to assure there's room for "nPackets*4"
114  {
115  #if MRPT_HAS_SSE2
116  // Number of 4-floats:
117  size_t nPackets = sizeRangeScan/4;
118  if ( (sizeRangeScan & 0x03)!=0) nPackets++;
119 
120  // We want to implement:
121  // scan_gx = m00*scan_x+m01*scan_y+m03;
122  // scan_gy = m10*scan_x+m11*scan_y+m13;
123  // scan_gz = m20*scan_x+m21*scan_y+m23;
124  //
125  // With: scan_x = ccos*range
126  // scan_y = csin*range
127  //
128  const __m128 m00_4val = _mm_set1_ps(m00); // load 4 copies of the same value
129  const __m128 m01_4val = _mm_set1_ps(m01);
130  const __m128 m03_4val = _mm_set1_ps(m03);
131 
132  const __m128 m10_4val = _mm_set1_ps(m10);
133  const __m128 m11_4val = _mm_set1_ps(m11);
134  const __m128 m13_4val = _mm_set1_ps(m13);
135 
136  const __m128 m20_4val = _mm_set1_ps(m20);
137  const __m128 m21_4val = _mm_set1_ps(m21);
138  const __m128 m23_4val = _mm_set1_ps(m23);
139 
140  // Make sure the input std::vector<> has room enough for reads of 4-float at a time:
141  // Invalid reads should not be a problem, but just for safety...
142  // JLBC: OCT/2016: rangeScan.scan() is now, by design, ensured to hold vectors of 4*N capacity, so there is no need to call reserve() here.
143 
144  const float *ptr_in_scan = &rangeScan.scan[0];
145  const float *ptr_in_cos = &sincos_vals.ccos[0];
146  const float *ptr_in_sin = &sincos_vals.csin[0];
147 
148  float *ptr_out_x = &scan_gx[0];
149  float *ptr_out_y = &scan_gy[0];
150  float *ptr_out_z = &scan_gz[0];
151 
152  for( ; nPackets; nPackets--, ptr_in_scan+=4, ptr_in_cos+=4, ptr_in_sin+=4, ptr_out_x+=4, ptr_out_y+=4, ptr_out_z+=4 )
153  {
154  const __m128 scan_4vals = _mm_loadu_ps(ptr_in_scan); // *Unaligned* load
155 
156  const __m128 xs = _mm_mul_ps(scan_4vals, _mm_load_ps(ptr_in_cos) );
157  const __m128 ys = _mm_mul_ps(scan_4vals, _mm_load_ps(ptr_in_sin) );
158 
159  _mm_store_ps(ptr_out_x, _mm_add_ps(m03_4val, _mm_add_ps( _mm_mul_ps(xs,m00_4val), _mm_mul_ps(ys,m01_4val) ) ) );
160  _mm_store_ps(ptr_out_y, _mm_add_ps(m13_4val, _mm_add_ps( _mm_mul_ps(xs,m10_4val), _mm_mul_ps(ys,m11_4val) ) ) );
161  _mm_store_ps(ptr_out_z, _mm_add_ps(m23_4val, _mm_add_ps( _mm_mul_ps(xs,m20_4val), _mm_mul_ps(ys,m21_4val) ) ) );
162  }
163  #else // MRPT_HAS_SSE2
164  // The "+3" is to assure the buffer has room for the SSE2 method which works with 4-tuples of floats.
165  Eigen::Array<float,Eigen::Dynamic,1> scan_x(sizeRangeScan+3), scan_y(sizeRangeScan+3);
166 
167  // Convert from the std::vector format:
168  const Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,1> > scan_vals( const_cast<float*>(&rangeScan.scan[0]),rangeScan.scan.size(),1 );
169  // SinCos table allocates N+4 floats for the convenience of SSE2: Map to make it appears it has the correct size:
170  const Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,1> > ccos( const_cast<float*>(&sincos_vals.ccos[0]),rangeScan.scan.size(),1 );
171  const Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,1> > csin( const_cast<float*>(&sincos_vals.csin[0]),rangeScan.scan.size(),1 );
172 
173  // Vectorized (optimized) scalar multiplications:
174  scan_x = scan_vals.array() * ccos.array();
175  scan_y = scan_vals.array() * csin.array();
176 
177  // To global:
178  // Non (manually) vectorized version:
179  scan_gx = m00*scan_x+m01*scan_y+m03;
180  scan_gy = m10*scan_x+m11*scan_y+m13;
181  scan_gz = m20*scan_x+m21*scan_y+m23;
182  #endif // MRPT_HAS_SSE2
183  }
184 
185 
186  for (int i=0;i<sizeRangeScan;i++)
187  {
188  if ( rangeScan.validRange[i] )
189  {
190  lx = scan_gx[i];
191  ly = scan_gy[i];
192  lz = scan_gz[i];
193 
194  // Specialized work in derived classes:
195  pointmap_traits<Derived>::internal_loadFromRangeScan2D_prepareOneRange(obj,lx,ly,lz,lric);
196 
197  lastPointWasInserted = false;
198 
199  // Add if distance > minimum only:
200  bool pt_pass_min_dist = true;
201  float d2 = 0;
202  if (useMinDist || obj.insertionOptions.also_interpolate)
203  {
204  if (!lastPointWasValid)
205  pt_pass_min_dist = false;
206  else
207  {
208  d2 = (square(lx-lx_1) + square(ly-ly_1) + square(lz-lz_1) );
209  pt_pass_min_dist = (d2 > minDistSqrBetweenLaserPoints);
210  }
211  }
212 
213  if ( thisIsTheFirst || pt_pass_min_dist )
214  {
215  thisIsTheFirst = false;
216  // Si quieren que interpolemos tb. los puntos lejanos, hacerlo:
217 
218  if (obj.insertionOptions.also_interpolate && i>1)
219  {
220  float changeInDirection;
221  const float d = std::sqrt( d2 );
222 
223  if ((lx!=lx_1 || ly!=ly_1) && (lx_1!=lx_2 || ly_1!=ly_2) )
224  changeInDirection = atan2(ly-ly_1,lx-lx_1)-atan2(ly_1-ly_2,lx_1-lx_2);
225  else changeInDirection = 0;
226 
227  // Conditions to really interpolate the points:
228  if (d>=2*obj.insertionOptions.minDistBetweenLaserPoints &&
229  d<obj.insertionOptions.maxDistForInterpolatePoints &&
230  fabs(changeInDirection)<DEG2RAD(5) )
231  {
232  int nInterpol = mrpt::utils::round(d / (2*sqrt(minDistSqrBetweenLaserPoints)));
233 
234  for (int q=1;q<nInterpol;q++)
235  {
236  float i_x = lx_1 + q*(lx-lx_1)/nInterpol;
237  float i_y = ly_1 + q*(ly-ly_1)/nInterpol;
238  float i_z = lz_1 + q*(lz-lz_1)/nInterpol;
239  if( !obj.m_heightfilter_enabled || ( i_z >= obj.m_heightfilter_z_min && i_z <= obj.m_heightfilter_z_max ) )
240  {
241  obj.x.push_back( i_x );
242  obj.y.push_back( i_y );
243  obj.z.push_back( i_z );
244  // Allow derived classes to add any other information to that point:
245  pointmap_traits<Derived>::internal_loadFromRangeScan2D_postPushBack(obj,lric);
246  } // end if
247  } // end for
248  } // End of interpolate:
249  }
250 
251  if( !obj.m_heightfilter_enabled || (lz >= obj.m_heightfilter_z_min && lz <= obj.m_heightfilter_z_max ) )
252  {
253  obj.x[nextPtIdx] = lx;
254  obj.y[nextPtIdx] = ly;
255  obj.z[nextPtIdx] = lz;
256  nextPtIdx++;
257 
258  // Allow derived classes to add any other information to that point:
259  pointmap_traits<Derived>::internal_loadFromRangeScan2D_postPushBack(obj,lric);
260 
261  lastPointWasInserted = true;
262  if (useMinDist)
263  {
264  lx_2 = lx_1;
265  ly_2 = ly_1;
266 
267  lx_1 = lx;
268  ly_1 = ly;
269  lz_1 = lz;
270  }
271 
272  }
273  }
274  }
275 
276  // Save for next iteration:
277  lastPointWasValid = rangeScan.validRange[i] != 0;
278  }
279 
280  // The last point
281  if (lastPointWasValid && !lastPointWasInserted)
282  {
283  if( !obj.m_heightfilter_enabled || (lz >= obj.m_heightfilter_z_min && lz <= obj.m_heightfilter_z_max ) )
284  {
285  obj.x[nextPtIdx] = lx;
286  obj.y[nextPtIdx] = ly;
287  obj.z[nextPtIdx] = lz;
288  nextPtIdx++;
289  // Allow derived classes to add any other information to that point:
290  pointmap_traits<Derived>::internal_loadFromRangeScan2D_postPushBack(obj,lric);
291  }
292  }
293 
294  // Adjust size:
295  obj.x.resize( nextPtIdx );
296  obj.y.resize( nextPtIdx );
297  obj.z.resize( nextPtIdx );
298  }
299 
300  static inline void templ_loadFromRangeScan(
301  Derived &obj,
302  const mrpt::obs::CObservation3DRangeScan &rangeScan,
303  const mrpt::poses::CPose3D *robotPose )
304  {
305  using namespace mrpt::poses;
306  using mrpt::math::square;
307  obj.mark_as_modified();
308 
309  // If robot pose is supplied, compute sensor pose relative to it.
310  CPose3D sensorPose3D(UNINITIALIZED_POSE);
311  if (!robotPose)
312  sensorPose3D = rangeScan.sensorPose;
313  else sensorPose3D.composeFrom(*robotPose, rangeScan.sensorPose);
314 
315  // Insert vs. load and replace:
316  if (!obj.insertionOptions.addToExistingPointsMap)
317  obj.resize(0); // Resize to 0 instead of clear() so the std::vector<> memory is not actually deadllocated and can be reused.
318 
319  if (!rangeScan.hasPoints3D)
320  return; // Nothing to do!
321 
322  const size_t sizeRangeScan = rangeScan.points3D_x.size();
323 
324  // For a great gain in efficiency:
325  if ( obj.x.size()+sizeRangeScan> obj.x.capacity() )
326  obj.reserve( size_t(obj.x.size() + 1.1*sizeRangeScan) );
327 
328 
329  // GENERAL CASE OF SCAN WITH ARBITRARY 3D ORIENTATION:
330  // --------------------------------------------------------------------------
331  mrpt::maps::CPointsMap::TLaserRange3DInsertContext lric(rangeScan);
332  sensorPose3D.getHomogeneousMatrix(lric.HM);
333  // For quicker access to values as "float" instead of "doubles":
334  float m00 = lric.HM.get_unsafe(0,0);
335  float m01 = lric.HM.get_unsafe(0,1);
336  float m02 = lric.HM.get_unsafe(0,2);
337  float m03 = lric.HM.get_unsafe(0,3);
338  float m10 = lric.HM.get_unsafe(1,0);
339  float m11 = lric.HM.get_unsafe(1,1);
340  float m12 = lric.HM.get_unsafe(1,2);
341  float m13 = lric.HM.get_unsafe(1,3);
342  float m20 = lric.HM.get_unsafe(2,0);
343  float m21 = lric.HM.get_unsafe(2,1);
344  float m22 = lric.HM.get_unsafe(2,2);
345  float m23 = lric.HM.get_unsafe(2,3);
346 
347  float lx_1,ly_1,lz_1,lx=0,ly=0,lz=0; // Punto anterior y actual:
348 
349  // Initial last point:
350  lx_1 = -100; ly_1 = -100; lz_1 = -100;
351 
352  float minDistSqrBetweenLaserPoints = square( obj.insertionOptions.minDistBetweenLaserPoints );
353 
354  // If the user doesn't want a minimum distance:
355  if (obj.insertionOptions.minDistBetweenLaserPoints<=0)
356  minDistSqrBetweenLaserPoints = -1;
357 
358  // ----------------------------------------------------------------
359  // Transform these points into 3D using the pose transformation:
360  // ----------------------------------------------------------------
361  bool lastPointWasValid = true;
362  bool thisIsTheFirst = true;
363  bool lastPointWasInserted = false;
364 
365  // Initialize extra stuff in derived class:
366  pointmap_traits<Derived>::internal_loadFromRangeScan3D_init(obj,lric);
367 
368  for (size_t i=0;i<sizeRangeScan;i++)
369  {
370  // Valid point?
371  if ( rangeScan.points3D_x[i]!=0 || rangeScan.points3D_y[i]!=0 || rangeScan.points3D_z[i]!=0 || obj.insertionOptions.insertInvalidPoints)
372  {
373  lric.scan_x = rangeScan.points3D_x[i];
374  lric.scan_y = rangeScan.points3D_y[i];
375  lric.scan_z = rangeScan.points3D_z[i];
376 
377  lx = m00*lric.scan_x + m01*lric.scan_y + m02*lric.scan_z + m03;
378  ly = m10*lric.scan_x + m11*lric.scan_y + m12*lric.scan_z + m13;
379  lz = m20*lric.scan_x + m21*lric.scan_y + m22*lric.scan_z + m23;
380 
381  // Specialized work in derived classes:
382  pointmap_traits<Derived>::internal_loadFromRangeScan3D_prepareOneRange(obj,lx,ly,lz,lric);
383 
384  lastPointWasInserted = false;
385 
386  // Add if distance > minimum only:
387  float d2 = (square(lx-lx_1) + square(ly-ly_1) + square(lz-lz_1) );
388  if ( thisIsTheFirst || (lastPointWasValid && (d2 > minDistSqrBetweenLaserPoints)) )
389  {
390  thisIsTheFirst = false;
391 
392  obj.x.push_back( lx );
393  obj.y.push_back( ly );
394  obj.z.push_back( lz );
395  // Allow derived classes to add any other information to that point:
396  pointmap_traits<Derived>::internal_loadFromRangeScan3D_postPushBack(obj,lric);
397 
398  lastPointWasInserted = true;
399 
400  lx_1 = lx;
401  ly_1 = ly;
402  lz_1 = lz;
403  }
404 
405  lastPointWasValid = true;
406  }
407  else
408  {
409  lastPointWasValid = false;
410  }
411 
412  pointmap_traits<Derived>::internal_loadFromRangeScan3D_postOneRange(obj,lric);
413  }
414 
415  // The last point
416  if (lastPointWasValid && !lastPointWasInserted)
417  {
418  if (lx!=0 || ly!=0 || lz!=0)
419  {
420  obj.x.push_back( lx );
421  obj.y.push_back( ly );
422  obj.z.push_back( lz );
423  // Allow derived classes to add any other information to that point:
424  pointmap_traits<Derived>::internal_loadFromRangeScan3D_postPushBack(obj,lric);
425  }
426  }
427  }
428 
429  };
430 
431 } // end NS
432 } // end NS
433 } // end NS
434 
435 #endif
436 
437 
mrpt::mrpt::utils::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: maps/CColouredPointsMap.h:82
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
mrpt::obs::CObservation3DRangeScan
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.
Definition: obs/CObservation3DRangeScan.h:141
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
mrpt::maps::CPointsMap::TLaserRange3DInsertContext::scan_z
float scan_z
In internal_loadFromRangeScan3D_prepareOneRange, these are the local coordinates of the scan points b...
Definition: maps/CPointsMap.h:87
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
mrpt::obs::CObservation3DRangeScan::points3D_z
std::vector< float > points3D_z
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
Definition: obs/CObservation3DRangeScan.h:289
mrpt::maps::detail::loadFromRangeImpl::templ_loadFromRangeScan
static void templ_loadFromRangeScan(Derived &obj, const mrpt::obs::CObservation3DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose)
Definition: CPointsMap_crtp_common.h:300
mrpt::poses::CPose3D::composeFrom
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:595
mrpt::maps::CPointsMap::TLaserRange3DInsertContext::HM
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
Definition: maps/CPointsMap.h:85
mrpt::obs::CObservation2DRangeScan::validRange
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< char > > validRange
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.
Definition: obs/CObservation2DRangeScan.h:77
mrpt::maps::detail::loadFromRangeImpl::templ_loadFromRangeScan
static void templ_loadFromRangeScan(Derived &obj, const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose)
Definition: CPointsMap_crtp_common.h:26
DEG2RAD
#define DEG2RAD
Definition: maps/CColouredPointsMap.h:99
mrpt::obs::CObservation3DRangeScan::hasPoints3D
bool hasPoints3D
true means the field points3D contains valid data.
Definition: obs/CObservation3DRangeScan.h:288
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: obs/CObservation2DRangeScan.h:43
mrpt::obs::CObservation3DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
Definition: obs/CObservation3DRangeScan.h:542
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: maps/CColouredPointsMap.h:52
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
mrpt::maps::CPointsMap::TLaserRange3DInsertContext
Helper struct used for internal_loadFromRangeScan3D_prepareOneRange()
Definition: maps/CPointsMap.h:82
mrpt::obs::CObservation2DRangeScan::scan
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters. Must have same length than validRange.
Definition: obs/CObservation2DRangeScan.h:69
mrpt::maps::CPointsMap::TLaserRange2DInsertContext::HM
mrpt::math::CMatrixDouble44 HM
Homog matrix of the local sensor pose within the robot.
Definition: maps/CPointsMap.h:74
mrpt::obs::CObservation2DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
Definition: obs/CObservation2DRangeScan.h:84
mrpt::maps::detail::pointmap_traits
Definition: maps/CPointsMap.h:38
mrpt::poses::CPose3D::getHomogeneousMatrix
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3D.h:166
mrpt::maps::CPointsMap::TLaserRange2DInsertContext
Helper struct used for internal_loadFromRangeScan2D_prepareOneRange()
Definition: maps/CPointsMap.h:71


Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019