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


Page generated by Doxygen 1.9.1 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at mar 26 may 2026 13:12:03 CEST