MRPT  2.0.4
CObservation3DRangeScan_project3D_impl.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-2020, 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/cpu.h>
12 #include <mrpt/core/round.h> // round()
13 #include <mrpt/math/CMatrixF.h>
14 #include <mrpt/math/CVectorFixed.h>
18 #include <Eigen/Dense> // block<>()
19 
20 namespace mrpt::obs::detail
21 {
22 // Auxiliary functions which implement SSE-optimized proyection of 3D point
23 // cloud:
24 template <class POINTMAP>
26  const int H, const int W, const float* kxs, const float* kys,
27  const float* kzs, mrpt::math::CMatrix_u16& rangeImage,
28  const float rangeUnits, mrpt::opengl::PointCloudAdapter<POINTMAP>& pca,
29  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
30  const mrpt::obs::TRangeImageFilterParams& fp, bool MAKE_ORGANIZED,
31  const int DECIM);
32 template <class POINTMAP>
34  const int H, const int W, const float* kxs, const float* kys,
35  const float* kzs, mrpt::math::CMatrix_u16& rangeImage,
36  const float rangeUnits, mrpt::opengl::PointCloudAdapter<POINTMAP>& pca,
37  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
38  const mrpt::obs::TRangeImageFilterParams& fp, bool MAKE_ORGANIZED);
39 
40 template <typename POINTMAP>
41 inline void range2XYZ_LUT(
45  const mrpt::obs::TRangeImageFilterParams& fp, const int H, const int W,
46  const int DECIM, const bool use_rotated_LUT)
47 {
48  const size_t WH = W * H;
49  const auto& lut = src_obs.get_unproj_lut();
50 
51  // Select between coordinates wrt the robot/vehicle, or local wrt sensor:
52  const auto& Kxs = use_rotated_LUT ? lut.Kxs_rot : lut.Kxs;
53  const auto& Kys = use_rotated_LUT ? lut.Kys_rot : lut.Kys;
54  const auto& Kzs = use_rotated_LUT ? lut.Kzs_rot : lut.Kzs;
55 
56  ASSERT_EQUAL_(WH, size_t(Kxs.size()));
57  ASSERT_EQUAL_(WH, size_t(Kys.size()));
58  ASSERT_EQUAL_(WH, size_t(Kzs.size()));
59  const float* kxs = &Kxs[0];
60  const float* kys = &Kys[0];
61  const float* kzs = &Kzs[0];
62 
63  if (fp.rangeMask_min)
64  { // sanity check:
67  }
68  if (fp.rangeMask_max)
69  { // sanity check:
72  }
73 
75  pp.layer.empty() ? &src_obs.rangeImage
76  : &src_obs.rangeImageOtherLayers.at(pp.layer);
77 
78 #if MRPT_HAS_SSE2
79  // if image width is not 8*N, use standard method
80  if ((W & 0x07) == 0 && pp.USE_SSE2 && DECIM == 1 &&
83  H, W, kxs, kys, kzs, *ri, src_obs.rangeUnits, pca,
84  src_obs.points3D_idxs_x, src_obs.points3D_idxs_y, fp,
85  pp.MAKE_ORGANIZED);
86  else
87 #endif
89  H, W, kxs, kys, kzs, *ri, src_obs.rangeUnits, pca,
90  src_obs.points3D_idxs_x, src_obs.points3D_idxs_y, fp,
91  pp.MAKE_ORGANIZED, DECIM);
92 
93  // Do final traslation, if we were generating points in the vehicle frame:
94  if (use_rotated_LUT)
95  {
96  const size_t nPts = pca.size();
97  const auto trans = mrpt::math::TPoint3Df(
98  src_obs.sensorPose.x(), src_obs.sensorPose.y(),
99  src_obs.sensorPose.z());
100  for (size_t i = 0; i < nPts; i++)
101  {
103  pca.getPointXYZ(i, pt.x, pt.y, pt.z);
104  pt += trans;
105  pca.setPointXYZ(i, pt.x, pt.y, pt.z);
106  }
107  }
108 }
109 
110 template <class POINTMAP>
112  mrpt::obs::CObservation3DRangeScan& src_obs, POINTMAP& dest_pointcloud,
115 {
116  using namespace mrpt::math;
117 
118  mrpt::opengl::PointCloudAdapter<POINTMAP> pca(dest_pointcloud);
119 
120  if (!src_obs.hasRangeImage)
121  {
122  pca.resize(0);
123  return;
124  }
125  if (!pp.layer.empty())
126  {
127  ASSERT_EQUAL_(src_obs.rangeImageOtherLayers.count(pp.layer), 1);
128  const auto& ri = src_obs.rangeImageOtherLayers.at(pp.layer);
129 
130  ASSERT_EQUAL_(ri.cols(), src_obs.rangeImage.cols());
131  ASSERT_EQUAL_(ri.rows(), src_obs.rangeImage.rows());
132  }
133 
134  // Decide whether we could directly use the precomputed LUT of pixel
135  // directions including the sensor pose:
136  const bool use_rotated_LUT =
137  // The user DO want the transformed points
139  // We don't have colors (local coordinates needed then)
140  (!pca.HAS_RGB || !src_obs.hasIntensityImage) &&
141  // and we are not to use a global pose in the world/map
143 
144  // ------------------------------------------------------------
145  // Stage 1/3: Create 3D point cloud local coordinates
146  // ------------------------------------------------------------
147  const int W = src_obs.rangeImage.cols();
148  const int H = src_obs.rangeImage.rows();
149  ASSERT_(W != 0 && H != 0);
150  const size_t WH = W * H;
151 
152  const auto DECIM = pp.decimation;
153  if (pp.decimation == 1)
154  {
155  // No decimation: one point per range image pixel
156 
157  // This is to make sure points3D_idxs_{x,y} have the expected sizes
158  src_obs.resizePoints3DVectors(WH);
159  // Reserve memory for 3D points. It will be later resized again to the
160  // actual number of valid points
161  pca.resize(WH);
162  if (pp.MAKE_ORGANIZED) pca.setDimensions(H, W);
163  }
164  else
165  {
166  // Decimate range image:
167  ASSERTMSG_(
168  (W % DECIM) == 0 && (H % DECIM == 0),
169  "Width/Height are not an exact multiple of decimation");
170  const int Wd = W / DECIM;
171  const int Hd = H / DECIM;
172  ASSERT_(Wd != 0 && Hd != 0);
173  const size_t WHd = Wd * Hd;
174 
175  src_obs.resizePoints3DVectors(WHd);
176  pca.resize(WHd);
177  if (pp.MAKE_ORGANIZED) pca.setDimensions(Hd, Wd);
178  }
179  range2XYZ_LUT<POINTMAP>(pca, src_obs, pp, fp, H, W, DECIM, use_rotated_LUT);
180 
181  // -------------------------------------------------------------
182  // Stage 2/3: Project local points into RGB image to get colors
183  // -------------------------------------------------------------
184  if constexpr (pca.HAS_RGB)
185  {
186  if (src_obs.hasIntensityImage)
187  {
188  const int imgW = src_obs.intensityImage.getWidth();
189  const int imgH = src_obs.intensityImage.getHeight();
190  const bool hasColorIntensityImg = src_obs.intensityImage.isColor();
191 
192  const float cx = src_obs.cameraParamsIntensity.cx();
193  const float cy = src_obs.cameraParamsIntensity.cy();
194  const float fx = src_obs.cameraParamsIntensity.fx();
195  const float fy = src_obs.cameraParamsIntensity.fy();
196 
197  // Unless we are in a special case (both depth & RGB images
198  // coincide)...
199  const bool isDirectCorresp =
200  src_obs.doDepthAndIntensityCamerasCoincide();
201 
202  // ...precompute the inverse of the pose transformation out of
203  // the
204  // loop,
205  // store as a 4x4 homogeneous matrix to exploit SSE
206  // optimizations
207  // below:
209  if (!isDirectCorresp)
210  {
214  src_obs.relativePoseIntensityWRTDepth.getRotationMatrix(),
215  src_obs.relativePoseIntensityWRTDepth.m_coords, R_inv,
216  t_inv);
217 
218  T_inv(3, 3) = 1;
219  T_inv.insertMatrix(0, 0, R_inv.cast_float());
220  T_inv.insertMatrix(0, 3, t_inv.cast_float());
221  }
222 
223  CVectorFixedFloat<4> pt_wrt_color, pt_wrt_depth;
224  pt_wrt_depth[3] = 1;
225  mrpt::img::TColor pCol;
226 
227  // For each local point:
228  const size_t nPts = pca.size();
229  const auto& iimg = src_obs.intensityImage;
230  const uint8_t* img_data = iimg.ptrLine<uint8_t>(0);
231  const auto img_stride = iimg.getRowStride();
232  for (size_t i = 0; i < nPts; i++)
233  {
234  int img_idx_x,
235  img_idx_y; // projected pixel coordinates, in the
236  // RGB image plane
237  bool pointWithinImage = false;
238  if (isDirectCorresp)
239  {
240  pointWithinImage = true;
241  img_idx_x = src_obs.points3D_idxs_x[i];
242  img_idx_y = src_obs.points3D_idxs_y[i];
243  }
244  else
245  {
246  // Project point, which is now in "pca" in local
247  // coordinates
248  // wrt the depth camera, into the intensity camera:
249  pca.getPointXYZ(
250  i, pt_wrt_depth[0], pt_wrt_depth[1], pt_wrt_depth[2]);
251  pt_wrt_color = T_inv * pt_wrt_depth;
252 
253  // Project to image plane:
254  if (pt_wrt_color[2])
255  {
256  img_idx_x = mrpt::round(
257  cx + fx * pt_wrt_color[0] / pt_wrt_color[2]);
258  img_idx_y = mrpt::round(
259  cy + fy * pt_wrt_color[1] / pt_wrt_color[2]);
260  pointWithinImage = img_idx_x >= 0 && img_idx_x < imgW &&
261  img_idx_y >= 0 && img_idx_y < imgH;
262  }
263  }
264 
265  if (pointWithinImage)
266  {
267  if (hasColorIntensityImg)
268  {
269  const auto px_idx =
270  img_stride * img_idx_y + 3 * img_idx_x;
271  pCol.R = img_data[px_idx + 2];
272  pCol.G = img_data[px_idx + 1];
273  pCol.B = img_data[px_idx + 0];
274  }
275  else
276  {
277  const auto px_idx = img_stride * img_idx_y + img_idx_x;
278  pCol.R = pCol.G = pCol.B = img_data[px_idx];
279  }
280  }
281  else
282  {
283  pCol.R = pCol.G = pCol.B = 255;
284  }
285  // Set color:
286  pca.setPointRGBu8(i, pCol.R, pCol.G, pCol.B);
287  } // end for each point
288  } // end if src_obs has intensity image
289  }
290  // ...
291 
292  // ------------------------------------------------------------
293  // Stage 3/3: Apply 6D transformations
294  // ------------------------------------------------------------
295  if (!use_rotated_LUT &&
296  (pp.takeIntoAccountSensorPoseOnRobot || pp.robotPoseInTheWorld))
297  {
298  mrpt::poses::CPose3D transf_to_apply; // Either ROBOTPOSE or
299  // ROBOTPOSE(+)SENSORPOSE or
300  // SENSORPOSE
301  if (pp.takeIntoAccountSensorPoseOnRobot)
302  transf_to_apply = src_obs.sensorPose;
303  if (pp.robotPoseInTheWorld)
304  transf_to_apply.composeFrom(
305  *pp.robotPoseInTheWorld, mrpt::poses::CPose3D(transf_to_apply));
306 
307  const auto HM =
308  transf_to_apply
310  .cast_float();
311  mrpt::math::CVectorFixedFloat<4> pt, pt_transf;
312  pt[3] = 1;
313 
314  const size_t nPts = pca.size();
315  for (size_t i = 0; i < nPts; i++)
316  {
317  pca.getPointXYZ(i, pt[0], pt[1], pt[2]);
318  pt_transf = HM * pt;
319  pca.setPointXYZ(i, pt_transf[0], pt_transf[1], pt_transf[2]);
320  }
321  }
322 } // end of unprojectInto
323 
324 // Auxiliary functions which implement (un)projection of 3D point clouds:
325 template <class POINTMAP>
327  const int H, const int W, const float* kxs, const float* kys,
328  const float* kzs, mrpt::math::CMatrix_u16& rangeImage,
329  const float rangeUnits, mrpt::opengl::PointCloudAdapter<POINTMAP>& pca,
330  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
331  const mrpt::obs::TRangeImageFilterParams& fp, bool MAKE_ORGANIZED,
332  const int DECIM)
333 {
334  TRangeImageFilter rif(fp);
335  // Preconditions: minRangeMask() has the right size
336  size_t idx = 0;
337  if (DECIM == 1)
338  {
339  for (int r = 0; r < H; r++)
340  for (int c = 0; c < W; c++)
341  {
342  const float D = rangeImage.coeff(r, c) * rangeUnits;
343  // LUT projection coefs:
344  const auto kx = *kxs++, ky = *kys++, kz = *kzs++;
345  if (!rif.do_range_filter(r, c, D))
346  {
347  if (MAKE_ORGANIZED) pca.setInvalidPoint(idx++);
348  if (fp.mark_invalid_ranges) rangeImage.coeffRef(r, c) = 0;
349  continue;
350  }
351  pca.setPointXYZ(idx, kx * D, ky * D /*y*/, kz * D /*z*/);
352  idxs_x[idx] = c;
353  idxs_y[idx] = r;
354  ++idx;
355  }
356  }
357  else
358  {
359  const int Hd = H / DECIM, Wd = W / DECIM;
360 
361  for (int rd = 0; rd < Hd; rd++)
362  for (int cd = 0; cd < Wd; cd++)
363  {
364  bool valid_pt = false;
365  float min_d = std::numeric_limits<float>::max();
366  for (int rb = 0; rb < DECIM; rb++)
367  for (int cb = 0; cb < DECIM; cb++)
368  {
369  const auto r = rd * DECIM + rb, c = cd * DECIM + cb;
370  const float D = rangeImage.coeff(r, c) * rangeUnits;
371  if (rif.do_range_filter(r, c, D))
372  {
373  valid_pt = true;
374  if (D < min_d) min_d = D;
375  }
376  else
377  {
378  if (fp.mark_invalid_ranges)
379  rangeImage.coeffRef(r, c) = 0;
380  }
381  }
382  if (!valid_pt)
383  {
384  if (MAKE_ORGANIZED) pca.setInvalidPoint(idx++);
385  continue;
386  }
387  const auto eq_r = rd * DECIM + DECIM / 2,
388  eq_c = cd * DECIM + DECIM / 2;
389  const auto eq_idx = eq_c + eq_r * W;
390  const auto kx = kxs[eq_idx], ky = kys[eq_idx], kz = kzs[eq_idx];
391  pca.setPointXYZ(
392  idx, kx * min_d /*x*/, ky * min_d /*y*/, kz * min_d /*z*/);
393  idxs_x[idx] = eq_c;
394  idxs_y[idx] = eq_r;
395  ++idx;
396  }
397  }
398  pca.resize(idx);
399  // Make sure indices are also resized down to the actual number of points,
400  // even if they are not part of the object PCA refers to:
401  idxs_x.resize(idx);
402  idxs_y.resize(idx);
403 }
404 
405 // Auxiliary functions which implement (un)projection of 3D point clouds:
406 template <class POINTMAP>
408  const int H, const int W, const float* kxs, const float* kys,
409  const float* kzs, mrpt::math::CMatrix_u16& rangeImage,
410  const float rangeUnits, mrpt::opengl::PointCloudAdapter<POINTMAP>& pca,
411  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
412  const mrpt::obs::TRangeImageFilterParams& fp, bool MAKE_ORGANIZED)
413 {
414 #if MRPT_HAS_SSE2
415  // Preconditions: minRangeMask() has the right size
416  // Use optimized version:
417  const int W_4 = W >> 2; // /=4 , since we process 4 values at a time.
418  size_t idx = 0;
419  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float xs[4], ys[4], zs[4];
420  const __m128 D_zeros = _mm_set_ps(.0f, .0f, .0f, .0f);
421  const __m128 xormask =
422  (fp.rangeCheckBetween) ? _mm_cmpneq_ps(D_zeros, D_zeros)
423  : // want points BETWEEN min and max to be valid
424  _mm_cmpeq_ps(
425  D_zeros,
426  D_zeros); // want points OUTSIDE of min and max to be valid
427  for (int r = 0; r < H; r++)
428  {
429  const uint16_t* Du16_ptr = &rangeImage(r, 0);
430  const float* Dgt_ptr =
431  !fp.rangeMask_min ? nullptr : &(*fp.rangeMask_min)(r, 0);
432  const float* Dlt_ptr =
433  !fp.rangeMask_max ? nullptr : &(*fp.rangeMask_max)(r, 0);
434 
435  for (int c = 0; c < W_4; c++)
436  {
437  // Let the compiler optimize this as MMX instructions (tested in
438  // godbolt):
439  alignas(16) float tmp[4];
440  for (int b = 0; b < 4; b++) tmp[b] = Du16_ptr[b] * rangeUnits;
441 
442  const __m128 D = _mm_load_ps(&tmp[0]);
443  const __m128 nz_mask = _mm_cmpgt_ps(D, D_zeros);
444  __m128 valid_range_mask;
445  if (!fp.rangeMask_min && !fp.rangeMask_max)
446  { // No filter: just skip D=0 points
447  valid_range_mask = nz_mask;
448  }
449  else
450  {
451  if (!fp.rangeMask_min || !fp.rangeMask_max)
452  { // Only one filter
453  if (fp.rangeMask_min)
454  {
455  const __m128 Dmin = _mm_load_ps(Dgt_ptr);
456  valid_range_mask = _mm_or_ps(
457  _mm_cmpgt_ps(D, Dmin), _mm_cmpeq_ps(Dmin, D_zeros));
458  }
459  else
460  {
461  const __m128 Dmax = _mm_load_ps(Dlt_ptr);
462  valid_range_mask = _mm_or_ps(
463  _mm_cmplt_ps(D, Dmax), _mm_cmpeq_ps(Dmax, D_zeros));
464  }
465  valid_range_mask = _mm_and_ps(
466  valid_range_mask, nz_mask); // Filter out D=0 points
467  }
468  else
469  {
470  // We have both: D>Dmin and D<Dmax conditions, with XOR to
471  // optionally invert the selection:
472  const __m128 Dmin = _mm_load_ps(Dgt_ptr);
473  const __m128 Dmax = _mm_load_ps(Dlt_ptr);
474 
475  const __m128 gt_mask = _mm_or_ps(
476  _mm_cmpgt_ps(D, Dmin), _mm_cmpeq_ps(Dmin, D_zeros));
477  const __m128 lt_mask = _mm_or_ps(
478  _mm_cmplt_ps(D, Dmax), _mm_cmpeq_ps(Dmax, D_zeros));
479  // (D>Dmin && D<Dmax) & skip points at zero
480  valid_range_mask =
481  _mm_and_ps(nz_mask, _mm_and_ps(gt_mask, lt_mask));
482  valid_range_mask = _mm_xor_ps(valid_range_mask, xormask);
483  // Add the case of D_min & D_max = 0 (no filtering)
484  valid_range_mask = _mm_or_ps(
485  valid_range_mask, _mm_and_ps(
486  _mm_cmpeq_ps(Dmin, D_zeros),
487  _mm_cmpeq_ps(Dmax, D_zeros)));
488  // Finally, ensure no invalid ranges get thru:
489  valid_range_mask = _mm_and_ps(valid_range_mask, nz_mask);
490  }
491  }
492  const int valid_range_maski = _mm_movemask_epi8(
493  _mm_castps_si128(valid_range_mask)); // 0x{f|0}{f|0}{f|0}{f|0}
494  if (valid_range_maski != 0) // Any of the 4 values is valid?
495  {
496  const __m128 KX = _mm_load_ps(kxs);
497  const __m128 KY = _mm_load_ps(kys);
498  const __m128 KZ = _mm_load_ps(kzs);
499 
500  _mm_storeu_ps(xs, _mm_mul_ps(KX, D));
501  _mm_storeu_ps(ys, _mm_mul_ps(KY, D));
502  _mm_storeu_ps(zs, _mm_mul_ps(KZ, D));
503 
504  for (int q = 0; q < 4; q++)
505  {
506  const int actual_c = (c << 2) + q;
507  if ((valid_range_maski & (1 << (q * 4))) != 0)
508  {
509  pca.setPointXYZ(idx, xs[q], ys[q], zs[q]);
510  idxs_x[idx] = actual_c;
511  idxs_y[idx] = r;
512  ++idx;
513  }
514  else
515  {
516  if (MAKE_ORGANIZED)
517  {
518  pca.setInvalidPoint(idx);
519  ++idx;
520  }
521  if (fp.mark_invalid_ranges)
522  rangeImage.coeffRef(r, actual_c) = 0;
523  }
524  }
525  }
526  else if (MAKE_ORGANIZED)
527  {
528  for (int q = 0; q < 4; q++)
529  {
530  pca.setInvalidPoint(idx);
531  ++idx;
532  const int actual_c = (c << 2) + q;
533  if (fp.mark_invalid_ranges)
534  rangeImage.coeffRef(r, actual_c) = 0;
535  }
536  }
537  Du16_ptr += 4;
538  if (Dgt_ptr) Dgt_ptr += 4;
539  if (Dlt_ptr) Dlt_ptr += 4;
540  kxs += 4;
541  kys += 4;
542  kzs += 4;
543  }
544  }
545  pca.resize(idx);
546  // Make sure indices are also resized down to the actual number of points,
547  // even if they are not part of the object PCA refers to:
548  idxs_x.resize(idx);
549  idxs_y.resize(idx);
550 #endif
551 }
552 } // namespace mrpt::obs::detail
std::map< std::string, mrpt::math::CMatrix_u16 > rangeImageOtherLayers
Additional layer range/depth images.
Mainly for internal use within CObservation3DRangeScan::unprojectInto()
constexpr matrix_size_t size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Definition: CMatrixFixed.h:233
uint8_t decimation
(Default:1) If !=1, split the range image in blocks of DxD (D=decimation), and only generates one poi...
void resizePoints3DVectors(const size_t nPoints)
Use this method instead of resizing all three points3D_x, points3D_y & points3D_z to allow the usage ...
const mrpt::poses::CPose3D * robotPoseInTheWorld
(Default: nullptr) Read takeIntoAccountSensorPoseOnRobot
An adapter to different kinds of point cloud object.
std::vector< uint16_t > points3D_idxs_x
If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x, points3D_y, points3D_z.
void insertMatrix(const int row_start, const int col_start, const OTHERMATVEC &submat)
Copies the given input submatrix/vector into this matrix/vector, starting at the given top-left coord...
Definition: MatrixBase.h:210
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
uint8_t B
Definition: TColor.h:51
uint8_t G
Definition: TColor.h:51
Used in CObservation3DRangeScan::unprojectInto()
bool supports(feature f) noexcept
Returns true if the current CPU (and OS) supports the given CPU feature.
Definition: cpu.h:75
const mrpt::math::CMatrixF * rangeMask_max
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
An adapter to different kinds of point cloud object.
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:556
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
mrpt::aligned_std_vector< float > Kxs_rot
x,y,z: in the rotated frame of coordinates of the sensorPose.
void unprojectInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
bool hasRangeImage
true means the field rangeImage contains valid data
mrpt::math::CMatrix_u16 rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters)...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
const mrpt::math::CMatrixF * rangeMask_min
(Default: nullptr) If provided, each data range will be tested to be greater-than (rangeMask_min) or ...
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
bool mark_invalid_ranges
If enabled, the range pixels of points that do NOT pass the mask filter will be marked as invalid ran...
uint8_t R
Definition: TColor.h:51
const Scalar & coeff(int r, int c) const
bool USE_SSE2
(Default:true) If possible, use SSE2 optimized code.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
bool MAKE_ORGANIZED
(Default:false) set to true if you want an organized point cloud
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
void range2XYZ_LUT(mrpt::opengl::PointCloudAdapter< POINTMAP > &pca, mrpt::obs::CObservation3DRangeScan &src_obs, const mrpt::obs::T3DPointsProjectionParams &pp, const mrpt::obs::TRangeImageFilterParams &fp, const int H, const int W, const int DECIM, const bool use_rotated_LUT)
void homogeneousMatrixInverse(const MATRIXLIKE1 &M, MATRIXLIKE2 &out_inverse_M)
Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part...
CMatrixFixed< float, ROWS, COLS > cast_float() const
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
Used in CObservation3DRangeScan::unprojectInto()
bool hasIntensityImage
true means the field intensityImage contains valid data
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:278
TPoint3D_< float > TPoint3Df
Definition: TPoint3D.h:269
A RGB color - 8bit.
Definition: TColor.h:25
void do_project_3d_pointcloud(const int H, const int W, const float *kxs, const float *kys, const float *kzs, mrpt::math::CMatrix_u16 &rangeImage, const float rangeUnits, mrpt::opengl::PointCloudAdapter< POINTMAP > &pca, std::vector< uint16_t > &idxs_x, std::vector< uint16_t > &idxs_y, const mrpt::obs::TRangeImageFilterParams &fp, bool MAKE_ORGANIZED, const int DECIM)
std::string layer
If empty, the main rangeImage layer will be unprojected.
void do_project_3d_pointcloud_SSE2(const int H, const int W, const float *kxs, const float *kys, const float *kzs, mrpt::math::CMatrix_u16 &rangeImage, const float rangeUnits, mrpt::opengl::PointCloudAdapter< POINTMAP > &pca, std::vector< uint16_t > &idxs_x, std::vector< uint16_t > &idxs_y, const mrpt::obs::TRangeImageFilterParams &fp, bool MAKE_ORGANIZED)
float rangeUnits
The conversion factor from integer units in rangeImage and actual distances in meters.
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated.
const unproject_LUT_t & get_unproj_lut() const
Gets (or generates upon first request) the 3D point cloud projection look-up-table for the current de...
Scalar & coeffRef(int r, int c)
bool do_range_filter(size_t r, size_t c, const float D) const
Returns true if the point (r,c) with depth D passes all filters.
bool rangeCheckBetween
Only used if both rangeMask_min and rangeMask_max are present.
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 2.0.4 Git: 5711e29ae Wed May 27 14:29:47 2020 +0200 at miƩ may 27 14:30:10 CEST 2020