MRPT  1.9.9
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-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/cpu.h>
12 #include <mrpt/core/round.h> // round()
13 #include <mrpt/math/CVectorFixed.h>
14 #include <Eigen/Dense> // block<>()
15 
16 namespace mrpt::obs::detail
17 {
18 // Auxiliary functions which implement SSE-optimized proyection of 3D point
19 // cloud:
20 template <class POINTMAP>
22  const int H, const int W, const float* kys, const float* kzs,
23  mrpt::math::CMatrix_u16& rangeImage, const float rangeUnits,
25  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
26  const mrpt::obs::TRangeImageFilterParams& fp, bool MAKE_ORGANIZED,
27  const int DECIM);
28 template <class POINTMAP>
30  const int H, const int W, const float* kys, const float* kzs,
31  mrpt::math::CMatrix_u16& rangeImage, const float rangeUnits,
33  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
34  const mrpt::obs::TRangeImageFilterParams& fp, bool MAKE_ORGANIZED);
35 
36 template <typename POINTMAP, bool isDepth>
37 inline void range2XYZ(
40  const mrpt::obs::TRangeImageFilterParams& fp, const int H, const int W)
41 {
42  /* range_is_depth = false :
43  * Ky = (r_cx - c)/r_fx
44  * Kz = (r_cy - r)/r_fy
45  *
46  * x(i) = rangeImage(r,c) * rangeUnits / sqrt( 1 + Ky^2 + Kz^2 )
47  * y(i) = Ky * x(i)
48  * z(i) = Kz * x(i)
49  */
50  const float r_cx = src_obs.cameraParams.cx();
51  const float r_cy = src_obs.cameraParams.cy();
52  const float r_fx_inv = 1.0f / src_obs.cameraParams.fx();
53  const float r_fy_inv = 1.0f / src_obs.cameraParams.fy();
54  TRangeImageFilter rif(fp);
55  size_t idx = 0;
56  for (int r = 0; r < H; r++)
57  for (int c = 0; c < W; c++)
58  {
59  const float D = src_obs.rangeImage.coeff(r, c) * src_obs.rangeUnits;
60  if (rif.do_range_filter(r, c, D))
61  {
62  const float Ky = (r_cx - c) * r_fx_inv;
63  const float Kz = (r_cy - r) * r_fy_inv;
64  pca.setPointXYZ(
65  idx,
66  isDepth ? D : D / std::sqrt(1 + Ky * Ky + Kz * Kz), // x
67  Ky * D, // y
68  Kz * D // z
69  );
70  src_obs.points3D_idxs_x[idx] = c;
71  src_obs.points3D_idxs_y[idx] = r;
72  ++idx;
73  }
74  else
75  {
76  if (fp.mark_invalid_ranges)
77  src_obs.rangeImage.coeffRef(r, c) = 0;
78  }
79  }
80  pca.resize(idx); // Actual number of valid pts
81 }
82 
83 template <typename POINTMAP, bool isDepth>
84 inline void range2XYZ_LUT(
88  const mrpt::obs::TRangeImageFilterParams& fp, const int H, const int W,
89  const int DECIM = 1)
90 {
91  const size_t WH = W * H;
92  if (src_obs.get_3dproj_lut().prev_camParams != src_obs.cameraParams ||
93  WH != size_t(src_obs.get_3dproj_lut().Kys.size()))
94  {
95  src_obs.get_3dproj_lut().prev_camParams = src_obs.cameraParams;
96  src_obs.get_3dproj_lut().Kys.resize(WH);
97  src_obs.get_3dproj_lut().Kzs.resize(WH);
98 
99  const float r_cx = src_obs.cameraParams.cx();
100  const float r_cy = src_obs.cameraParams.cy();
101  const float r_fx_inv = 1.0f / src_obs.cameraParams.fx();
102  const float r_fy_inv = 1.0f / src_obs.cameraParams.fy();
103 
104  float* kys = &src_obs.get_3dproj_lut().Kys[0];
105  float* kzs = &src_obs.get_3dproj_lut().Kzs[0];
106  for (int r = 0; r < H; r++)
107  for (int c = 0; c < W; c++)
108  {
109  *kys++ = (r_cx - c) * r_fx_inv;
110  *kzs++ = (r_cy - r) * r_fy_inv;
111  }
112  } // end update LUT.
113 
114  ASSERT_EQUAL_(WH, size_t(src_obs.get_3dproj_lut().Kys.size()));
115  ASSERT_EQUAL_(WH, size_t(src_obs.get_3dproj_lut().Kzs.size()));
116  float* kys = &src_obs.get_3dproj_lut().Kys[0];
117  float* kzs = &src_obs.get_3dproj_lut().Kzs[0];
118 
119  if (fp.rangeMask_min)
120  { // sanity check:
121  ASSERT_EQUAL_(fp.rangeMask_min->cols(), src_obs.rangeImage.cols());
122  ASSERT_EQUAL_(fp.rangeMask_min->rows(), src_obs.rangeImage.rows());
123  }
124  if (fp.rangeMask_max)
125  { // sanity check:
126  ASSERT_EQUAL_(fp.rangeMask_max->cols(), src_obs.rangeImage.cols());
127  ASSERT_EQUAL_(fp.rangeMask_max->rows(), src_obs.rangeImage.rows());
128  }
129 #if MRPT_HAS_SSE2
130  // if image width is not 8*N, use standard method
131  if ((W & 0x07) == 0 && pp.USE_SSE2 && DECIM == 1 &&
134  H, W, kys, kzs, src_obs.rangeImage, src_obs.rangeUnits, pca,
135  src_obs.points3D_idxs_x, src_obs.points3D_idxs_y, fp,
136  pp.MAKE_ORGANIZED);
137  else
138 #endif
140  H, W, kys, kzs, src_obs.rangeImage, src_obs.rangeUnits, pca,
141  src_obs.points3D_idxs_x, src_obs.points3D_idxs_y, fp,
142  pp.MAKE_ORGANIZED, DECIM);
143 }
144 
145 template <class POINTMAP>
147  mrpt::obs::CObservation3DRangeScan& src_obs, POINTMAP& dest_pointcloud,
150 {
151  using namespace mrpt::math;
152 
153  if (!src_obs.hasRangeImage) return;
154 
155  mrpt::opengl::PointCloudAdapter<POINTMAP> pca(dest_pointcloud);
156 
157  // ------------------------------------------------------------
158  // Stage 1/3: Create 3D point cloud local coordinates
159  // ------------------------------------------------------------
160  const int W = src_obs.rangeImage.cols();
161  const int H = src_obs.rangeImage.rows();
162  ASSERT_(W != 0 && H != 0);
163  const size_t WH = W * H;
164 
165  if (pp.decimation == 1)
166  {
167  // No decimation: one point per range image pixel
168 
169  // This is to make sure points3D_idxs_{x,y} have the expected sizes
170  src_obs.resizePoints3DVectors(WH);
171  // Reserve memory for 3D points. It will be later resized again to the
172  // actual number of valid points
173  pca.resize(WH);
174  if (pp.MAKE_ORGANIZED) pca.setDimensions(H, W);
175  if (src_obs.range_is_depth)
176  {
177  // range_is_depth = true
178  // Use cached tables?
179  if (pp.PROJ3D_USE_LUT)
180  range2XYZ_LUT<POINTMAP, true>(pca, src_obs, pp, fp, H, W);
181  else
182  range2XYZ<POINTMAP, true>(pca, src_obs, fp, H, W);
183  }
184  else
185  range2XYZ<POINTMAP, false>(pca, src_obs, fp, H, W);
186  }
187  else
188  {
189  // Decimate range image:
190  const auto DECIM = pp.decimation;
191  ASSERTMSG_(
192  (W % DECIM) == 0 && (H % DECIM == 0),
193  "Width/Height are not an exact multiple of decimation");
194  const int Wd = W / DECIM;
195  const int Hd = H / DECIM;
196  ASSERT_(Wd != 0 && Hd != 0);
197  const size_t WHd = Wd * Hd;
198 
199  src_obs.resizePoints3DVectors(WHd);
200  pca.resize(WHd);
201  if (pp.MAKE_ORGANIZED) pca.setDimensions(Hd, Wd);
202  ASSERTMSG_(
203  src_obs.range_is_depth && pp.PROJ3D_USE_LUT,
204  "Decimation only available if range_is_depth && PROJ3D_USE_LUT");
205  range2XYZ_LUT<POINTMAP, true>(pca, src_obs, pp, fp, H, W, DECIM);
206  }
207 
208  // -------------------------------------------------------------
209  // Stage 2/3: Project local points into RGB image to get colors
210  // -------------------------------------------------------------
211  if constexpr (pca.HAS_RGB)
212  {
213  if (src_obs.hasIntensityImage)
214  {
215  const int imgW = src_obs.intensityImage.getWidth();
216  const int imgH = src_obs.intensityImage.getHeight();
217  const bool hasColorIntensityImg = src_obs.intensityImage.isColor();
218 
219  const float cx = src_obs.cameraParamsIntensity.cx();
220  const float cy = src_obs.cameraParamsIntensity.cy();
221  const float fx = src_obs.cameraParamsIntensity.fx();
222  const float fy = src_obs.cameraParamsIntensity.fy();
223 
224  // Unless we are in a special case (both depth & RGB images
225  // coincide)...
226  const bool isDirectCorresp =
228 
229  // ...precompute the inverse of the pose transformation out of
230  // the
231  // loop,
232  // store as a 4x4 homogeneous matrix to exploit SSE
233  // optimizations
234  // below:
236  if (!isDirectCorresp)
237  {
243  t_inv);
244 
245  T_inv(3, 3) = 1;
246  T_inv.insertMatrix(0, 0, R_inv.cast_float());
247  T_inv.insertMatrix(0, 3, t_inv.cast_float());
248  }
249 
250  CVectorFixedFloat<4> pt_wrt_color, pt_wrt_depth;
251  pt_wrt_depth[3] = 1;
252  mrpt::img::TColor pCol;
253 
254  // For each local point:
255  const size_t nPts = pca.size();
256  const auto& iimg = src_obs.intensityImage;
257  const uint8_t* img_data = iimg.ptrLine<uint8_t>(0);
258  const auto img_stride = iimg.getRowStride();
259  for (size_t i = 0; i < nPts; i++)
260  {
261  int img_idx_x,
262  img_idx_y; // projected pixel coordinates, in the
263  // RGB image plane
264  bool pointWithinImage = false;
265  if (isDirectCorresp)
266  {
267  pointWithinImage = true;
268  img_idx_x = src_obs.points3D_idxs_x[i];
269  img_idx_y = src_obs.points3D_idxs_y[i];
270  }
271  else
272  {
273  // Project point, which is now in "pca" in local
274  // coordinates
275  // wrt the depth camera, into the intensity camera:
276  pca.getPointXYZ(
277  i, pt_wrt_depth[0], pt_wrt_depth[1], pt_wrt_depth[2]);
278  pt_wrt_color = T_inv * pt_wrt_depth;
279 
280  // Project to image plane:
281  if (pt_wrt_color[2])
282  {
283  img_idx_x = mrpt::round(
284  cx + fx * pt_wrt_color[0] / pt_wrt_color[2]);
285  img_idx_y = mrpt::round(
286  cy + fy * pt_wrt_color[1] / pt_wrt_color[2]);
287  pointWithinImage = img_idx_x >= 0 && img_idx_x < imgW &&
288  img_idx_y >= 0 && img_idx_y < imgH;
289  }
290  }
291 
292  if (pointWithinImage)
293  {
294  if (hasColorIntensityImg)
295  {
296  const auto px_idx =
297  img_stride * img_idx_y + 3 * img_idx_x;
298  pCol.R = img_data[px_idx + 2];
299  pCol.G = img_data[px_idx + 1];
300  pCol.B = img_data[px_idx + 0];
301  }
302  else
303  {
304  const auto px_idx = img_stride * img_idx_y + img_idx_x;
305  pCol.R = pCol.G = pCol.B = img_data[px_idx];
306  }
307  }
308  else
309  {
310  pCol.R = pCol.G = pCol.B = 255;
311  }
312  // Set color:
313  pca.setPointRGBu8(i, pCol.R, pCol.G, pCol.B);
314  } // end for each point
315  } // end if src_obs has intensity image
316  }
317  // ...
318 
319  // ------------------------------------------------------------
320  // Stage 3/3: Apply 6D transformations
321  // ------------------------------------------------------------
323  {
324  mrpt::poses::CPose3D transf_to_apply; // Either ROBOTPOSE or
325  // ROBOTPOSE(+)SENSORPOSE or
326  // SENSORPOSE
328  transf_to_apply = src_obs.sensorPose;
329  if (pp.robotPoseInTheWorld)
330  transf_to_apply.composeFrom(
331  *pp.robotPoseInTheWorld, mrpt::poses::CPose3D(transf_to_apply));
332 
333  const auto HM =
334  transf_to_apply
336  .cast_float();
337  mrpt::math::CVectorFixedFloat<4> pt, pt_transf;
338  pt[3] = 1;
339 
340  const size_t nPts = pca.size();
341  for (size_t i = 0; i < nPts; i++)
342  {
343  pca.getPointXYZ(i, pt[0], pt[1], pt[2]);
344  pt_transf = HM * pt;
345  pca.setPointXYZ(i, pt_transf[0], pt_transf[1], pt_transf[2]);
346  }
347  }
348 } // end of project3DPointsFromDepthImageInto
349 
350 // Auxiliary functions which implement (un)projection of 3D point clouds:
351 template <class POINTMAP>
353  const int H, const int W, const float* kys, const float* kzs,
354  mrpt::math::CMatrix_u16& rangeImage, const float rangeUnits,
356  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
357  const mrpt::obs::TRangeImageFilterParams& fp, bool MAKE_ORGANIZED,
358  const int DECIM)
359 {
360  TRangeImageFilter rif(fp);
361  // Preconditions: minRangeMask() has the right size
362  size_t idx = 0;
363  if (DECIM == 1)
364  {
365  for (int r = 0; r < H; r++)
366  for (int c = 0; c < W; c++)
367  {
368  const float D = rangeImage.coeff(r, c) * rangeUnits;
369  // LUT projection coefs:
370  const auto ky = *kys++, kz = *kzs++;
371  if (!rif.do_range_filter(r, c, D))
372  {
373  if (MAKE_ORGANIZED) pca.setInvalidPoint(idx++);
374  if (fp.mark_invalid_ranges) rangeImage.coeffRef(r, c) = 0;
375  continue;
376  }
377  pca.setPointXYZ(idx, D /*x*/, ky * D /*y*/, kz * D /*z*/);
378  idxs_x[idx] = c;
379  idxs_y[idx] = r;
380  ++idx;
381  }
382  }
383  else
384  {
385  const int Hd = H / DECIM, Wd = W / DECIM;
386 
387  for (int rd = 0; rd < Hd; rd++)
388  for (int cd = 0; cd < Wd; cd++)
389  {
390  bool valid_pt = false;
391  float min_d = std::numeric_limits<float>::max();
392  for (int rb = 0; rb < DECIM; rb++)
393  for (int cb = 0; cb < DECIM; cb++)
394  {
395  const auto r = rd * DECIM + rb, c = cd * DECIM + cb;
396  const float D = rangeImage.coeff(r, c) * rangeUnits;
397  if (rif.do_range_filter(r, c, D))
398  {
399  valid_pt = true;
400  if (D < min_d) min_d = D;
401  }
402  else
403  {
404  if (fp.mark_invalid_ranges)
405  rangeImage.coeffRef(r, c) = 0;
406  }
407  }
408  if (!valid_pt)
409  {
410  if (MAKE_ORGANIZED) pca.setInvalidPoint(idx++);
411  continue;
412  }
413  const auto eq_r = rd * DECIM + DECIM / 2,
414  eq_c = cd * DECIM + DECIM / 2;
415  const auto ky = kys[eq_c + eq_r * W], kz = kzs[eq_c + eq_r * W];
416  pca.setPointXYZ(
417  idx, min_d /*x*/, ky * min_d /*y*/, kz * min_d /*z*/);
418  idxs_x[idx] = eq_c;
419  idxs_y[idx] = eq_r;
420  ++idx;
421  }
422  }
423  pca.resize(idx);
424  // Make sure indices are also resized down to the actual number of points,
425  // even if they are not part of the object PCA refers to:
426  idxs_x.resize(idx);
427  idxs_y.resize(idx);
428 }
429 
430 // Auxiliary functions which implement (un)projection of 3D point clouds:
431 template <class POINTMAP>
433  const int H, const int W, const float* kys, const float* kzs,
434  mrpt::math::CMatrix_u16& rangeImage, const float rangeUnits,
436  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
437  const mrpt::obs::TRangeImageFilterParams& fp, bool MAKE_ORGANIZED)
438 {
439 #if MRPT_HAS_SSE2
440  // Preconditions: minRangeMask() has the right size
441  // Use optimized version:
442  const int W_4 = W >> 2; // /=4 , since we process 4 values at a time.
443  size_t idx = 0;
444  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float xs[4], ys[4], zs[4];
445  const __m128 D_zeros = _mm_set_ps(.0f, .0f, .0f, .0f);
446  const __m128 xormask =
447  (fp.rangeCheckBetween) ? _mm_cmpneq_ps(D_zeros, D_zeros)
448  : // want points BETWEEN min and max to be valid
449  _mm_cmpeq_ps(
450  D_zeros,
451  D_zeros); // want points OUTSIDE of min and max to be valid
452  for (int r = 0; r < H; r++)
453  {
454  const uint16_t* Du16_ptr = &rangeImage(r, 0);
455  const float* Dgt_ptr =
456  !fp.rangeMask_min ? nullptr : &(*fp.rangeMask_min)(r, 0);
457  const float* Dlt_ptr =
458  !fp.rangeMask_max ? nullptr : &(*fp.rangeMask_max)(r, 0);
459 
460  for (int c = 0; c < W_4; c++)
461  {
462  // Let the compiler optimize this as MMX instructions (tested in
463  // godbolt):
464  alignas(16) float tmp[4];
465  for (int b = 0; b < 4; b++) tmp[b] = Du16_ptr[b] * rangeUnits;
466 
467  const __m128 D = _mm_load_ps(&tmp[0]);
468  const __m128 nz_mask = _mm_cmpgt_ps(D, D_zeros);
469  __m128 valid_range_mask;
470  if (!fp.rangeMask_min && !fp.rangeMask_max)
471  { // No filter: just skip D=0 points
472  valid_range_mask = nz_mask;
473  }
474  else
475  {
476  if (!fp.rangeMask_min || !fp.rangeMask_max)
477  { // Only one filter
478  if (fp.rangeMask_min)
479  {
480  const __m128 Dmin = _mm_load_ps(Dgt_ptr);
481  valid_range_mask = _mm_or_ps(
482  _mm_cmpgt_ps(D, Dmin), _mm_cmpeq_ps(Dmin, D_zeros));
483  }
484  else
485  {
486  const __m128 Dmax = _mm_load_ps(Dlt_ptr);
487  valid_range_mask = _mm_or_ps(
488  _mm_cmplt_ps(D, Dmax), _mm_cmpeq_ps(Dmax, D_zeros));
489  }
490  valid_range_mask = _mm_and_ps(
491  valid_range_mask, nz_mask); // Filter out D=0 points
492  }
493  else
494  {
495  // We have both: D>Dmin and D<Dmax conditions, with XOR to
496  // optionally invert the selection:
497  const __m128 Dmin = _mm_load_ps(Dgt_ptr);
498  const __m128 Dmax = _mm_load_ps(Dlt_ptr);
499 
500  const __m128 gt_mask = _mm_or_ps(
501  _mm_cmpgt_ps(D, Dmin), _mm_cmpeq_ps(Dmin, D_zeros));
502  const __m128 lt_mask = _mm_or_ps(
503  _mm_cmplt_ps(D, Dmax), _mm_cmpeq_ps(Dmax, D_zeros));
504  // (D>Dmin && D<Dmax) & skip points at zero
505  valid_range_mask =
506  _mm_and_ps(nz_mask, _mm_and_ps(gt_mask, lt_mask));
507  valid_range_mask = _mm_xor_ps(valid_range_mask, xormask);
508  // Add the case of D_min & D_max = 0 (no filtering)
509  valid_range_mask = _mm_or_ps(
510  valid_range_mask, _mm_and_ps(
511  _mm_cmpeq_ps(Dmin, D_zeros),
512  _mm_cmpeq_ps(Dmax, D_zeros)));
513  // Finally, ensure no invalid ranges get thru:
514  valid_range_mask = _mm_and_ps(valid_range_mask, nz_mask);
515  }
516  }
517  const int valid_range_maski = _mm_movemask_epi8(
518  _mm_castps_si128(valid_range_mask)); // 0x{f|0}{f|0}{f|0}{f|0}
519  if (valid_range_maski != 0) // Any of the 4 values is valid?
520  {
521  const __m128 KY = _mm_load_ps(kys);
522  const __m128 KZ = _mm_load_ps(kzs);
523 
524  _mm_storeu_ps(xs, D);
525  _mm_storeu_ps(ys, _mm_mul_ps(KY, D));
526  _mm_storeu_ps(zs, _mm_mul_ps(KZ, D));
527 
528  for (int q = 0; q < 4; q++)
529  {
530  const int actual_c = (c << 2) + q;
531  if ((valid_range_maski & (1 << (q * 4))) != 0)
532  {
533  pca.setPointXYZ(idx, xs[q], ys[q], zs[q]);
534  idxs_x[idx] = actual_c;
535  idxs_y[idx] = r;
536  ++idx;
537  }
538  else
539  {
540  if (MAKE_ORGANIZED)
541  {
542  pca.setInvalidPoint(idx);
543  ++idx;
544  }
545  if (fp.mark_invalid_ranges)
546  rangeImage.coeffRef(r, actual_c) = 0;
547  }
548  }
549  }
550  else if (MAKE_ORGANIZED)
551  {
552  for (int q = 0; q < 4; q++)
553  {
554  pca.setInvalidPoint(idx);
555  ++idx;
556  const int actual_c = (c << 2) + q;
557  if (fp.mark_invalid_ranges)
558  rangeImage.coeffRef(r, actual_c) = 0;
559  }
560  }
561  Du16_ptr += 4;
562  if (Dgt_ptr) Dgt_ptr += 4;
563  if (Dlt_ptr) Dlt_ptr += 4;
564  kys += 4;
565  kzs += 4;
566  }
567  }
568  pca.resize(idx);
569  // Make sure indices are also resized down to the actual number of points,
570  // even if they are not part of the object PCA refers to:
571  idxs_x.resize(idx);
572  idxs_y.resize(idx);
573 #endif
574 }
575 } // namespace mrpt::obs::detail
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
Mainly for internal use within CObservation3DRangeScan::project3DPointsFromDepthImageInto() ...
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
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:174
mrpt::math::CVectorFixedDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
Definition: CPose3D.h:97
const T * ptrLine(unsigned int row) const
Returns a pointer to the first pixel of the given line.
Definition: img/CImage.h:596
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
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:176
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.
void range2XYZ(mrpt::opengl::PointCloudAdapter< POINTMAP > &pca, mrpt::obs::CObservation3DRangeScan &src_obs, const mrpt::obs::TRangeImageFilterParams &fp, const int H, const int W)
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:845
uint8_t B
Definition: TColor.h:46
uint8_t G
Definition: TColor.h:46
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
bool supports(feature f) noexcept
Returns true if CPU (and OS) supports the given CPU feature, and that instruction set or feature was ...
Definition: cpu.h:77
void do_project_3d_pointcloud_SSE2(const int H, const int W, 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 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.
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:814
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:172
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:572
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
void project3DPointsFromDepthImageInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
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
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:46
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=1)
const Scalar & coeff(int r, int c) const
bool USE_SSE2
(Default:true) If possible, use SSE2 optimized code.
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:170
bool MAKE_ORGANIZED
(Default:false) set to true if you want an organized point cloud
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
Definition: CImage.cpp:855
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
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
static TCached3DProjTables & get_3dproj_lut()
3D point cloud projection look-up-table
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
bool hasIntensityImage
true means the field intensityImage contains valid data
bool PROJ3D_USE_LUT
(Default:true) [Only used when range_is_depth=true] Whether to use a Look-up-table (LUT) to speed up ...
bool doDepthAndIntensityCamerasCoincide() const
Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:278
void do_project_3d_pointcloud(const int H, const int W, 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)
A RGB color - 8bit.
Definition: TColor.h:20
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:225
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
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.
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 1.9.9 Git: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020