Main MRPT website > C++ reference for MRPT 1.9.9
CObservation3DRangeScan_project3D_impl.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 CObservation3DRangeScan_project3D_impl_H
10 #define CObservation3DRangeScan_project3D_impl_H
11 
12 #include <mrpt/utils/round.h> // round()
13 
14 namespace mrpt
15 {
16 namespace obs
17 {
18 namespace detail
19 {
20 // Auxiliary functions which implement SSE-optimized proyection of 3D point
21 // cloud:
22 template <class POINTMAP>
23 void do_project_3d_pointcloud(
24  const int H, const int W, const float* kys, const float* kzs,
25  const mrpt::math::CMatrix& rangeImage,
26  mrpt::utils::PointCloudAdapter<POINTMAP>& pca,
27  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
28  const mrpt::obs::TRangeImageFilterParams& filterParams, bool MAKE_DENSE);
29 template <class POINTMAP>
30 void do_project_3d_pointcloud_SSE2(
31  const int H, const int W, const float* kys, const float* kzs,
32  const mrpt::math::CMatrix& rangeImage,
33  mrpt::utils::PointCloudAdapter<POINTMAP>& pca,
34  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
35  const mrpt::obs::TRangeImageFilterParams& filterParams, bool MAKE_DENSE);
36 
37 template <class POINTMAP>
38 void project3DPointsFromDepthImageInto(
39  mrpt::obs::CObservation3DRangeScan& src_obs, POINTMAP& dest_pointcloud,
40  const mrpt::obs::T3DPointsProjectionParams& projectParams,
41  const mrpt::obs::TRangeImageFilterParams& filterParams)
42 {
43  using namespace mrpt::math;
44 
45  if (!src_obs.hasRangeImage) return;
46 
47  mrpt::utils::PointCloudAdapter<POINTMAP> pca(dest_pointcloud);
48 
49  // ------------------------------------------------------------
50  // Stage 1/3: Create 3D point cloud local coordinates
51  // ------------------------------------------------------------
52  const int W = src_obs.rangeImage.cols();
53  const int H = src_obs.rangeImage.rows();
54  ASSERT_(W != 0 && H != 0);
55  const size_t WH = W * H;
56 
57  src_obs.resizePoints3DVectors(WH); // This is to make sure
58  // points3D_idxs_{x,y} have the expected
59  // sizes.
60  pca.resize(WH); // Reserve memory for 3D points. It will be later resized
61  // again to the actual number of valid points
62 
63  if (src_obs.range_is_depth)
64  {
65  // range_is_depth = true
66 
67  // Use cached tables?
68  if (projectParams.PROJ3D_USE_LUT)
69  {
70  // Use LUT:
71  if (src_obs.get_3dproj_lut().prev_camParams != src_obs.cameraParams ||
72  WH != size_t(src_obs.get_3dproj_lut().Kys.size()))
73  {
74  src_obs.get_3dproj_lut().prev_camParams = src_obs.cameraParams;
75  src_obs.get_3dproj_lut().Kys.resize(WH);
76  src_obs.get_3dproj_lut().Kzs.resize(WH);
77 
78  const float r_cx = src_obs.cameraParams.cx();
79  const float r_cy = src_obs.cameraParams.cy();
80  const float r_fx_inv = 1.0f / src_obs.cameraParams.fx();
81  const float r_fy_inv = 1.0f / src_obs.cameraParams.fy();
82 
83  float* kys = &src_obs.get_3dproj_lut().Kys[0];
84  float* kzs = &src_obs.get_3dproj_lut().Kzs[0];
85  for (int r = 0; r < H; r++)
86  for (int c = 0; c < W; c++)
87  {
88  *kys++ = (r_cx - c) * r_fx_inv;
89  *kzs++ = (r_cy - r) * r_fy_inv;
90  }
91  } // end update LUT.
92 
93  ASSERT_EQUAL_(WH, size_t(src_obs.get_3dproj_lut().Kys.size()))
94  ASSERT_EQUAL_(WH, size_t(src_obs.get_3dproj_lut().Kzs.size()))
95  float* kys = &src_obs.get_3dproj_lut().Kys[0];
96  float* kzs = &src_obs.get_3dproj_lut().Kzs[0];
97 
98  if (filterParams.rangeMask_min)
99  { // sanity check:
100  ASSERT_EQUAL_(
101  filterParams.rangeMask_min->cols(),
102  src_obs.rangeImage.cols());
103  ASSERT_EQUAL_(
104  filterParams.rangeMask_min->rows(),
105  src_obs.rangeImage.rows());
106  }
107  if (filterParams.rangeMask_max)
108  { // sanity check:
109  ASSERT_EQUAL_(
110  filterParams.rangeMask_max->cols(),
111  src_obs.rangeImage.cols());
112  ASSERT_EQUAL_(
113  filterParams.rangeMask_max->rows(),
114  src_obs.rangeImage.rows());
115  }
116 #if MRPT_HAS_SSE2
117  if ((W & 0x07) == 0 && projectParams.USE_SSE2)
118  do_project_3d_pointcloud_SSE2(
119  H, W, kys, kzs, src_obs.rangeImage, pca,
120  src_obs.points3D_idxs_x, src_obs.points3D_idxs_y,
121  filterParams, projectParams.MAKE_DENSE);
122  else
123  do_project_3d_pointcloud(
124  H, W, kys, kzs, src_obs.rangeImage, pca,
125  src_obs.points3D_idxs_x, src_obs.points3D_idxs_y,
126  filterParams, projectParams.MAKE_DENSE); // if image width
127 // is not 8*N, use
128 // standard method
129 #else
130  do_project_3d_pointcloud(
131  H, W, kys, kzs, src_obs.rangeImage, pca,
132  src_obs.points3D_idxs_x, src_obs.points3D_idxs_y, filterParams,
133  projectParams.MAKE_DENSE);
134 #endif
135  }
136  else
137  {
138  // Without LUT:
139  const float r_cx = src_obs.cameraParams.cx();
140  const float r_cy = src_obs.cameraParams.cy();
141  const float r_fx_inv = 1.0f / src_obs.cameraParams.fx();
142  const float r_fy_inv = 1.0f / src_obs.cameraParams.fy();
143  TRangeImageFilter rif(filterParams);
144  size_t idx = 0;
145  for (int r = 0; r < H; r++)
146  for (int c = 0; c < W; c++)
147  {
148  const float D = src_obs.rangeImage.coeff(r, c);
149  if (rif.do_range_filter(r, c, D))
150  {
151  const float Kz = (r_cy - r) * r_fy_inv;
152  const float Ky = (r_cx - c) * r_fx_inv;
153  pca.setPointXYZ(
154  idx,
155  D, // x
156  Ky * D, // y
157  Kz * D // z
158  );
159  src_obs.points3D_idxs_x[idx] = c;
160  src_obs.points3D_idxs_y[idx] = r;
161  ++idx;
162  }
163  }
164  pca.resize(idx); // Actual number of valid pts
165  }
166  }
167  else
168  {
169  /* range_is_depth = false :
170  * Ky = (r_cx - c)/r_fx
171  * Kz = (r_cy - r)/r_fy
172  *
173  * x(i) = rangeImage(r,c) / sqrt( 1 + Ky^2 + Kz^2 )
174  * y(i) = Ky * x(i)
175  * z(i) = Kz * x(i)
176  */
177  const float r_cx = src_obs.cameraParams.cx();
178  const float r_cy = src_obs.cameraParams.cy();
179  const float r_fx_inv = 1.0f / src_obs.cameraParams.fx();
180  const float r_fy_inv = 1.0f / src_obs.cameraParams.fy();
181  TRangeImageFilter rif(filterParams);
182  size_t idx = 0;
183  for (int r = 0; r < H; r++)
184  for (int c = 0; c < W; c++)
185  {
186  const float D = src_obs.rangeImage.coeff(r, c);
187  if (rif.do_range_filter(r, c, D))
188  {
189  const float Ky = (r_cx - c) * r_fx_inv;
190  const float Kz = (r_cy - r) * r_fy_inv;
191  pca.setPointXYZ(
192  idx,
193  D / std::sqrt(1 + Ky * Ky + Kz * Kz), // x
194  Ky * D, // y
195  Kz * D // z
196  );
197  src_obs.points3D_idxs_x[idx] = c;
198  src_obs.points3D_idxs_y[idx] = r;
199  ++idx;
200  }
201  }
202  pca.resize(idx); // Actual number of valid pts
203  }
204 
205  // -------------------------------------------------------------
206  // Stage 2/3: Project local points into RGB image to get colors
207  // -------------------------------------------------------------
208  if (src_obs.hasIntensityImage)
209  {
210  const int imgW = src_obs.intensityImage.getWidth();
211  const int imgH = src_obs.intensityImage.getHeight();
212  const bool hasColorIntensityImg = src_obs.intensityImage.isColor();
213 
214  const float cx = src_obs.cameraParamsIntensity.cx();
215  const float cy = src_obs.cameraParamsIntensity.cy();
216  const float fx = src_obs.cameraParamsIntensity.fx();
217  const float fy = src_obs.cameraParamsIntensity.fy();
218 
219  // Unless we are in a special case (both depth & RGB images coincide)...
220  const bool isDirectCorresp =
221  src_obs.doDepthAndIntensityCamerasCoincide();
222 
223  // ...precompute the inverse of the pose transformation out of the loop,
224  // store as a 4x4 homogeneous matrix to exploit SSE optimizations
225  // below:
226  mrpt::math::CMatrixFixedNumeric<float, 4, 4> T_inv;
227  if (!isDirectCorresp)
228  {
229  mrpt::math::CMatrixFixedNumeric<double, 3, 3> R_inv;
230  mrpt::math::CMatrixFixedNumeric<double, 3, 1> t_inv;
231  mrpt::math::homogeneousMatrixInverse(
232  src_obs.relativePoseIntensityWRTDepth.getRotationMatrix(),
233  src_obs.relativePoseIntensityWRTDepth.m_coords, R_inv, t_inv);
234 
235  T_inv(3, 3) = 1;
236  T_inv.block<3, 3>(0, 0) = R_inv.cast<float>();
237  T_inv.block<3, 1>(0, 3) = t_inv.cast<float>();
238  }
239 
240  Eigen::Matrix<float, 4, 1> pt_wrt_color, pt_wrt_depth;
241  pt_wrt_depth[3] = 1;
242 
243  mrpt::utils::TColor pCol;
244 
245  // For each local point:
246  const size_t nPts = pca.size();
247  for (size_t i = 0; i < nPts; i++)
248  {
249  int img_idx_x, img_idx_y; // projected pixel coordinates, in the
250  // RGB image plane
251  bool pointWithinImage = false;
252  if (isDirectCorresp)
253  {
254  pointWithinImage = true;
255  img_idx_x = src_obs.points3D_idxs_x[i];
256  img_idx_y = src_obs.points3D_idxs_y[i];
257  }
258  else
259  {
260  // Project point, which is now in "pca" in local coordinates wrt
261  // the depth camera, into the intensity camera:
262  pca.getPointXYZ(
263  i, pt_wrt_depth[0], pt_wrt_depth[1], pt_wrt_depth[2]);
264  pt_wrt_color.noalias() = T_inv * pt_wrt_depth;
265 
266  // Project to image plane:
267  if (pt_wrt_color[2])
268  {
269  img_idx_x = mrpt::utils::round(
270  cx + fx * pt_wrt_color[0] / pt_wrt_color[2]);
271  img_idx_y = mrpt::utils::round(
272  cy + fy * pt_wrt_color[1] / pt_wrt_color[2]);
273  pointWithinImage = img_idx_x >= 0 && img_idx_x < imgW &&
274  img_idx_y >= 0 && img_idx_y < imgH;
275  }
276  }
277 
278  if (pointWithinImage)
279  {
280  if (hasColorIntensityImg)
281  {
282  const uint8_t* c = src_obs.intensityImage.get_unsafe(
283  img_idx_x, img_idx_y, 0);
284  pCol.R = c[2];
285  pCol.G = c[1];
286  pCol.B = c[0];
287  }
288  else
289  {
290  uint8_t c = *src_obs.intensityImage.get_unsafe(
291  img_idx_x, img_idx_y, 0);
292  pCol.R = pCol.G = pCol.B = c;
293  }
294  }
295  else
296  {
297  pCol.R = pCol.G = pCol.B = 255;
298  }
299  // Set color:
300  pca.setPointRGBu8(i, pCol.R, pCol.G, pCol.B);
301  } // end for each point
302  } // end if src_obs has intensity image
303 
304  // ...
305 
306  // ------------------------------------------------------------
307  // Stage 3/3: Apply 6D transformations
308  // ------------------------------------------------------------
309  if (projectParams.takeIntoAccountSensorPoseOnRobot ||
310  projectParams.robotPoseInTheWorld)
311  {
312  mrpt::poses::CPose3D transf_to_apply; // Either ROBOTPOSE or
313  // ROBOTPOSE(+)SENSORPOSE or
314  // SENSORPOSE
315  if (projectParams.takeIntoAccountSensorPoseOnRobot)
316  transf_to_apply = src_obs.sensorPose;
317  if (projectParams.robotPoseInTheWorld)
318  transf_to_apply.composeFrom(
319  *projectParams.robotPoseInTheWorld,
320  mrpt::poses::CPose3D(transf_to_apply));
321 
322  const mrpt::math::CMatrixFixedNumeric<float, 4, 4> HM =
323  transf_to_apply.getHomogeneousMatrixVal().cast<float>();
324  Eigen::Matrix<float, 4, 1> pt, pt_transf;
325  pt[3] = 1;
326 
327  const size_t nPts = pca.size();
328  for (size_t i = 0; i < nPts; i++)
329  {
330  pca.getPointXYZ(i, pt[0], pt[1], pt[2]);
331  pt_transf.noalias() = HM * pt;
332  pca.setPointXYZ(i, pt_transf[0], pt_transf[1], pt_transf[2]);
333  }
334  }
335 } // end of project3DPointsFromDepthImageInto
336 
337 // Auxiliary functions which implement proyection of 3D point clouds:
338 template <class POINTMAP>
339 inline void do_project_3d_pointcloud(
340  const int H, const int W, const float* kys, const float* kzs,
341  const mrpt::math::CMatrix& rangeImage,
342  mrpt::utils::PointCloudAdapter<POINTMAP>& pca,
343  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
344  const mrpt::obs::TRangeImageFilterParams& fp, bool MAKE_DENSE)
345 {
346  TRangeImageFilter rif(fp);
347  // Preconditions: minRangeMask() has the right size
348  size_t idx = 0;
349  for (int r = 0; r < H; r++)
350  for (int c = 0; c < W; c++)
351  {
352  const float D = rangeImage.coeff(r, c);
353  if (!rif.do_range_filter(r, c, D))
354  {
355  if (!MAKE_DENSE)
356  {
357  pca.setInvalidPoint(idx);
358  ++idx;
359  }
360  continue;
361  }
362 
363  pca.setPointXYZ(idx, D /*x*/, *kys++ * D /*y*/, *kzs++ * D /*z*/);
364  idxs_x[idx] = c;
365  idxs_y[idx] = r;
366  ++idx;
367  }
368  pca.resize(idx);
369 }
370 
371 // Auxiliary functions which implement proyection of 3D point clouds:
372 template <class POINTMAP>
373 inline void do_project_3d_pointcloud_SSE2(
374  const int H, const int W, const float* kys, const float* kzs,
375  const mrpt::math::CMatrix& rangeImage,
376  mrpt::utils::PointCloudAdapter<POINTMAP>& pca,
377  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
378  const mrpt::obs::TRangeImageFilterParams& filterParams, bool MAKE_DENSE)
379 {
380 #if MRPT_HAS_SSE2
381  // Preconditions: minRangeMask() has the right size
382  // Use optimized version:
383  const int W_4 = W >> 2; // /=4 , since we process 4 values at a time.
384  size_t idx = 0;
385  alignas(16) float xs[4], ys[4], zs[4];
386  const __m128 D_zeros = _mm_set_ps(.0f, .0f, .0f, .0f);
387  const __m128 xormask =
388  (filterParams.rangeCheckBetween)
389  ? _mm_cmpneq_ps(D_zeros, D_zeros)
390  : // want points BETWEEN min and max to be valid
391  _mm_cmpeq_ps(
392  D_zeros,
393  D_zeros); // want points OUTSIDE of min and max to be valid
394  for (int r = 0; r < H; r++)
395  {
396  const float* D_ptr =
397  &rangeImage.coeffRef(r, 0); // Matrices are 16-aligned
398  const float* Dgt_ptr =
399  !filterParams.rangeMask_min
400  ? nullptr
401  : &filterParams.rangeMask_min->coeffRef(r, 0);
402  const float* Dlt_ptr =
403  !filterParams.rangeMask_max
404  ? nullptr
405  : &filterParams.rangeMask_max->coeffRef(r, 0);
406 
407  for (int c = 0; c < W_4; c++)
408  {
409  const __m128 D = _mm_load_ps(D_ptr);
410  const __m128 nz_mask = _mm_cmpgt_ps(D, D_zeros);
411  __m128 valid_range_mask;
412  if (!filterParams.rangeMask_min && !filterParams.rangeMask_max)
413  { // No filter: just skip D=0 points
414  valid_range_mask = nz_mask;
415  }
416  else
417  {
418  if (!filterParams.rangeMask_min || !filterParams.rangeMask_max)
419  { // Only one filter
420  if (filterParams.rangeMask_min)
421  {
422  const __m128 Dmin = _mm_load_ps(Dgt_ptr);
423  valid_range_mask = _mm_and_ps(
424  _mm_cmpgt_ps(D, Dmin), _mm_cmpgt_ps(Dmin, D_zeros));
425  }
426  else
427  {
428  const __m128 Dmax = _mm_load_ps(Dlt_ptr);
429  valid_range_mask = _mm_and_ps(
430  _mm_cmplt_ps(D, Dmax), _mm_cmpgt_ps(Dmax, D_zeros));
431  }
432  valid_range_mask = _mm_and_ps(
433  valid_range_mask, nz_mask); // Filter out D=0 points
434  }
435  else
436  {
437  // We have both: D>Dmin and D<Dmax conditions, with XOR to
438  // optionally invert the selection:
439  const __m128 Dmin = _mm_load_ps(Dgt_ptr);
440  const __m128 Dmax = _mm_load_ps(Dlt_ptr);
441 
442  const __m128 gt_mask = _mm_cmpgt_ps(D, Dmin);
443  const __m128 lt_mask = _mm_and_ps(
444  _mm_cmplt_ps(D, Dmax), nz_mask); // skip points at zero
445  valid_range_mask =
446  _mm_and_ps(gt_mask, lt_mask); // (D>Dmin && D<Dmax)
447  valid_range_mask = _mm_xor_ps(valid_range_mask, xormask);
448  // Add the case of D_min & D_max = 0 (no filtering)
449  valid_range_mask = _mm_or_ps(
450  valid_range_mask, _mm_and_ps(
451  _mm_cmpeq_ps(Dmin, D_zeros),
452  _mm_cmpeq_ps(Dmax, D_zeros)));
453  // Finally, ensure no invalid ranges get thru:
454  valid_range_mask = _mm_and_ps(valid_range_mask, nz_mask);
455  }
456  }
457  const int valid_range_maski = _mm_movemask_epi8(
458  _mm_castps_si128(valid_range_mask)); // 0x{f|0}{f|0}{f|0}{f|0}
459  if (valid_range_maski != 0) // Any of the 4 values is valid?
460  {
461  const __m128 KY = _mm_load_ps(kys);
462  const __m128 KZ = _mm_load_ps(kzs);
463 
464  _mm_storeu_ps(xs, D);
465  _mm_storeu_ps(ys, _mm_mul_ps(KY, D));
466  _mm_storeu_ps(zs, _mm_mul_ps(KZ, D));
467 
468  for (int q = 0; q < 4; q++)
469  if ((valid_range_maski & (1 << (q * 4))) != 0)
470  {
471  pca.setPointXYZ(idx, xs[q], ys[q], zs[q]);
472  idxs_x[idx] = (c << 2) + q;
473  idxs_y[idx] = r;
474  ++idx;
475  }
476  else if (!MAKE_DENSE)
477  {
478  pca.setInvalidPoint(idx);
479  ++idx;
480  }
481  }
482  else if (!MAKE_DENSE)
483  {
484  for (int q = 0; q < 4; q++)
485  {
486  pca.setInvalidPoint(idx);
487  ++idx;
488  }
489  }
490  D_ptr += 4;
491  if (Dgt_ptr) Dgt_ptr += 4;
492  if (Dlt_ptr) Dlt_ptr += 4;
493  kys += 4;
494  kzs += 4;
495  }
496  }
497  pca.resize(idx);
498 #endif
499 }
500 
501 } // End of namespace
502 } // End of namespace
503 } // End of namespace
504 #endif
ASSERT_EQUAL_
#define ASSERT_EQUAL_(__A, __B)
Definition: CColouredPointsMap.h:319
mrpt::obs::TRangeImageFilter
Mainly for internal use within CObservation3DRangeScan::project3DPointsFromDepthImageInto() ...
Definition: TRangeImageFilter.h:50
mrpt::obs::CObservation3DRangeScan::resizePoints3DVectors
void resizePoints3DVectors(const size_t nPoints)
Use this method instead of resizing all three points3D_x, points3D_y & points3D_z to allow the usage ...
Definition: CObservation3DRangeScan.cpp:859
mrpt::obs::T3DPointsProjectionParams::robotPoseInTheWorld
const mrpt::poses::CPose3D * robotPoseInTheWorld
(Default: nullptr) Read takeIntoAccountSensorPoseOnRobot
Definition: CObservation3DRangeScan.h:39
mrpt::obs::CObservation3DRangeScan::cameraParamsIntensity
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
Definition: CObservation3DRangeScan.h:741
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
mrpt::obs::CObservation3DRangeScan::points3D_idxs_x
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.
Definition: CObservation3DRangeScan.h:403
mrpt::utils::TCamera::cy
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:176
mrpt::obs::CObservation3DRangeScan
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.
Definition: CObservation3DRangeScan.h:225
mrpt::obs::TRangeImageFilterParams::rangeMask_min
const mrpt::math::CMatrix * rangeMask_min
(Default: nullptr) If provided, each data range will be tested to be greater-than (rangeMask_min) or ...
Definition: TRangeImageFilter.h:39
mrpt::obs::TRangeImageFilterParams
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
Definition: TRangeImageFilter.h:20
mrpt::obs::CObservation3DRangeScan::rangeImage
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
Definition: CObservation3DRangeScan.h:445
uint8_t
unsigned char uint8_t
Definition: rptypes.h:41
mrpt::obs::CObservation3DRangeScan::cameraParams
mrpt::utils::TCamera cameraParams
Projection parameters of the depth camera.
Definition: CObservation3DRangeScan.h:739
mrpt::utils::TCamera::fx
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:178
mrpt::utils::TCamera::fy
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:180
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:39
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::utils::CImage::getWidth
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:869
mrpt::obs::CObservation3DRangeScan::relativePoseIntensityWRTDepth
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
Definition: CObservation3DRangeScan.h:751
mrpt::poses::CPose3D::m_coords
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
Definition: CPose3D.h:98
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::getHomogeneousMatrixVal
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:229
c
const GLubyte * c
Definition: glext.h:6313
mrpt::obs::detail::project3DPointsFromDepthImageInto
void project3DPointsFromDepthImageInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
Definition: CObservation3DRangeScan_project3D_impl.h:38
mrpt::utils::CImage::getHeight
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:897
mrpt::utils::TColor
A RGB color - 8bit.
Definition: TColor.h:25
mrpt::obs::CObservation3DRangeScan::hasRangeImage
bool hasRangeImage
true means the field rangeImage contains valid data
Definition: CObservation3DRangeScan.h:442
mrpt::mrpt::utils::PointCloudAdapter
An adapter to different kinds of point cloud object.
Definition: CColouredPointsMap.h:41
mrpt::obs::detail::do_project_3d_pointcloud_SSE2
void do_project_3d_pointcloud_SSE2(const int H, const int W, const float *kys, const float *kzs, const mrpt::math::CMatrix &rangeImage, mrpt::utils::PointCloudAdapter< POINTMAP > &pca, std::vector< uint16_t > &idxs_x, std::vector< uint16_t > &idxs_y, const mrpt::obs::TRangeImageFilterParams &filterParams, bool MAKE_DENSE)
Definition: CObservation3DRangeScan_project3D_impl.h:373
mrpt::utils::TCamera::cx
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:174
mrpt::obs::T3DPointsProjectionParams::USE_SSE2
bool USE_SSE2
(Default:true) If possible, use SSE2 optimized code.
Definition: CObservation3DRangeScan.h:47
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
mrpt::obs::CObservation3DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
Definition: CObservation3DRangeScan.h:763
mrpt::obs::TRangeImageFilterParams::rangeMask_max
const mrpt::math::CMatrix * rangeMask_max
Definition: TRangeImageFilter.h:39
mrpt::math::homogeneousMatrixInverse
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...
Definition: homog_matrices.h:27
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::obs::CObservation3DRangeScan::get_3dproj_lut
static TCached3DProjTables & get_3dproj_lut()
3D point cloud projection look-up-table
Definition: CObservation3DRangeScan.cpp:40
mrpt::obs::T3DPointsProjectionParams
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
Definition: CObservation3DRangeScan.h:31
mrpt::obs::CObservation3DRangeScan::hasIntensityImage
bool hasIntensityImage
true means the field intensityImage contains valid data
Definition: CObservation3DRangeScan.h:500
ASSERT_
#define ASSERT_(f)
Definition: CColouredPointsMap.h:310
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:25
mrpt::obs::T3DPointsProjectionParams::PROJ3D_USE_LUT
bool PROJ3D_USE_LUT
(Default:true) [Only used when range_is_depth=true] Whether to use a Look-up-table (LUT) to speed up ...
Definition: CObservation3DRangeScan.h:45
mrpt::obs::CObservation3DRangeScan::doDepthAndIntensityCamerasCoincide
bool doDepthAndIntensityCamerasCoincide() const
Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
Definition: CObservation3DRangeScan.cpp:951
mrpt::utils::CImage::isColor
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
Definition: CImage.cpp:911
mrpt::utils::CImage::get_unsafe
unsigned char * get_unsafe(unsigned int col, unsigned int row, unsigned int channel=0) const
Access to pixels without checking boundaries - Use normally the () operator better, which checks the coordinates.
Definition: CImage.cpp:496
mrpt::utils::PointCloudAdapter
An adapter to different kinds of point cloud object.
Definition: adapters.h:41
mrpt::poses::CPose3D::getRotationMatrix
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:237
mrpt::obs::T3DPointsProjectionParams::MAKE_DENSE
bool MAKE_DENSE
(Default:true) set to false if you want to preserve the organization of the point cloud ...
Definition: CObservation3DRangeScan.h:50
mrpt::obs::CObservation3DRangeScan::intensityImage
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
Definition: CObservation3DRangeScan.h:503
mrpt::math::CMatrix
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:25
mrpt::obs::CObservation3DRangeScan::range_is_depth
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
Definition: CObservation3DRangeScan.h:449
mrpt::obs::detail::do_project_3d_pointcloud
void do_project_3d_pointcloud(const int H, const int W, const float *kys, const float *kzs, const mrpt::math::CMatrix &rangeImage, mrpt::utils::PointCloudAdapter< POINTMAP > &pca, std::vector< uint16_t > &idxs_x, std::vector< uint16_t > &idxs_y, const mrpt::obs::TRangeImageFilterParams &filterParams, bool MAKE_DENSE)
Definition: CObservation3DRangeScan_project3D_impl.h:339
mrpt::obs::T3DPointsProjectionParams::takeIntoAccountSensorPoseOnRobot
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated.
Definition: CObservation3DRangeScan.h:37
mrpt::obs::TRangeImageFilter::do_range_filter
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.
Definition: TRangeImageFilter.h:63
mrpt::obs::TRangeImageFilterParams::rangeCheckBetween
bool rangeCheckBetween
Only used if both rangeMask_min and rangeMask_max are present.
Definition: TRangeImageFilter.h:31


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019