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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST