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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019