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


Page generated by Doxygen 1.9.1 for MRPT 1.9.9 Git: 63ea9d1f1 Thu Nov 23 00:06:53 2017 +0100 at mar 26 may 2026 12:19:29 CEST