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-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #ifndef CObservation3DRangeScan_project3D_impl_H
10 #define CObservation3DRangeScan_project3D_impl_H
11 
12 #include <mrpt/core/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>
24  const int H, const int W, const float* kys, const float* kzs,
25  const mrpt::math::CMatrix& rangeImage,
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>
31  const int H, const int W, const float* kys, const float* kzs,
32  const mrpt::math::CMatrix& rangeImage,
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>
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::opengl::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 !=
72  src_obs.cameraParams ||
73  WH != size_t(src_obs.get_3dproj_lut().Kys.size()))
74  {
75  src_obs.get_3dproj_lut().prev_camParams = src_obs.cameraParams;
76  src_obs.get_3dproj_lut().Kys.resize(WH);
77  src_obs.get_3dproj_lut().Kzs.resize(WH);
78 
79  const float r_cx = src_obs.cameraParams.cx();
80  const float r_cy = src_obs.cameraParams.cy();
81  const float r_fx_inv = 1.0f / src_obs.cameraParams.fx();
82  const float r_fy_inv = 1.0f / src_obs.cameraParams.fy();
83 
84  float* kys = &src_obs.get_3dproj_lut().Kys[0];
85  float* kzs = &src_obs.get_3dproj_lut().Kzs[0];
86  for (int r = 0; r < H; r++)
87  for (int c = 0; c < W; c++)
88  {
89  *kys++ = (r_cx - c) * r_fx_inv;
90  *kzs++ = (r_cy - r) * r_fy_inv;
91  }
92  } // end update LUT.
93 
94  ASSERT_EQUAL_(WH, size_t(src_obs.get_3dproj_lut().Kys.size()));
95  ASSERT_EQUAL_(WH, size_t(src_obs.get_3dproj_lut().Kzs.size()));
96  float* kys = &src_obs.get_3dproj_lut().Kys[0];
97  float* kzs = &src_obs.get_3dproj_lut().Kzs[0];
98 
99  if (filterParams.rangeMask_min)
100  { // sanity check:
102  filterParams.rangeMask_min->cols(),
103  src_obs.rangeImage.cols());
105  filterParams.rangeMask_min->rows(),
106  src_obs.rangeImage.rows());
107  }
108  if (filterParams.rangeMask_max)
109  { // sanity check:
111  filterParams.rangeMask_max->cols(),
112  src_obs.rangeImage.cols());
114  filterParams.rangeMask_max->rows(),
115  src_obs.rangeImage.rows());
116  }
117 #if MRPT_HAS_SSE2
118  if ((W & 0x07) == 0 && projectParams.USE_SSE2)
120  H, W, kys, kzs, src_obs.rangeImage, pca,
121  src_obs.points3D_idxs_x, src_obs.points3D_idxs_y,
122  filterParams, projectParams.MAKE_DENSE);
123  else
125  H, W, kys, kzs, src_obs.rangeImage, pca,
126  src_obs.points3D_idxs_x, src_obs.points3D_idxs_y,
127  filterParams, projectParams.MAKE_DENSE); // if image width
128 // is not 8*N, use
129 // standard method
130 #else
132  H, W, kys, kzs, src_obs.rangeImage, pca,
133  src_obs.points3D_idxs_x, src_obs.points3D_idxs_y, filterParams,
134  projectParams.MAKE_DENSE);
135 #endif
136  }
137  else
138  {
139  // Without LUT:
140  const float r_cx = src_obs.cameraParams.cx();
141  const float r_cy = src_obs.cameraParams.cy();
142  const float r_fx_inv = 1.0f / src_obs.cameraParams.fx();
143  const float r_fy_inv = 1.0f / src_obs.cameraParams.fy();
144  TRangeImageFilter rif(filterParams);
145  size_t idx = 0;
146  for (int r = 0; r < H; r++)
147  for (int c = 0; c < W; c++)
148  {
149  const float D = src_obs.rangeImage.coeff(r, c);
150  if (rif.do_range_filter(r, c, D))
151  {
152  const float Kz = (r_cy - r) * r_fy_inv;
153  const float Ky = (r_cx - c) * r_fx_inv;
154  pca.setPointXYZ(
155  idx,
156  D, // x
157  Ky * D, // y
158  Kz * D // z
159  );
160  src_obs.points3D_idxs_x[idx] = c;
161  src_obs.points3D_idxs_y[idx] = r;
162  ++idx;
163  }
164  }
165  pca.resize(idx); // Actual number of valid pts
166  }
167  }
168  else
169  {
170  /* range_is_depth = false :
171  * Ky = (r_cx - c)/r_fx
172  * Kz = (r_cy - r)/r_fy
173  *
174  * x(i) = rangeImage(r,c) / sqrt( 1 + Ky^2 + Kz^2 )
175  * y(i) = Ky * x(i)
176  * z(i) = Kz * x(i)
177  */
178  const float r_cx = src_obs.cameraParams.cx();
179  const float r_cy = src_obs.cameraParams.cy();
180  const float r_fx_inv = 1.0f / src_obs.cameraParams.fx();
181  const float r_fy_inv = 1.0f / src_obs.cameraParams.fy();
182  TRangeImageFilter rif(filterParams);
183  size_t idx = 0;
184  for (int r = 0; r < H; r++)
185  for (int c = 0; c < W; c++)
186  {
187  const float D = src_obs.rangeImage.coeff(r, c);
188  if (rif.do_range_filter(r, c, D))
189  {
190  const float Ky = (r_cx - c) * r_fx_inv;
191  const float Kz = (r_cy - r) * r_fy_inv;
192  pca.setPointXYZ(
193  idx,
194  D / std::sqrt(1 + Ky * Ky + Kz * Kz), // x
195  Ky * D, // y
196  Kz * D // z
197  );
198  src_obs.points3D_idxs_x[idx] = c;
199  src_obs.points3D_idxs_y[idx] = r;
200  ++idx;
201  }
202  }
203  pca.resize(idx); // Actual number of valid pts
204  }
205 
206  // -------------------------------------------------------------
207  // Stage 2/3: Project local points into RGB image to get colors
208  // -------------------------------------------------------------
209  if (src_obs.hasIntensityImage)
210  {
211  const int imgW = src_obs.intensityImage.getWidth();
212  const int imgH = src_obs.intensityImage.getHeight();
213  const bool hasColorIntensityImg = src_obs.intensityImage.isColor();
214 
215  const float cx = src_obs.cameraParamsIntensity.cx();
216  const float cy = src_obs.cameraParamsIntensity.cy();
217  const float fx = src_obs.cameraParamsIntensity.fx();
218  const float fy = src_obs.cameraParamsIntensity.fy();
219 
220  // Unless we are in a special case (both depth & RGB images coincide)...
221  const bool isDirectCorresp =
223 
224  // ...precompute the inverse of the pose transformation out of the loop,
225  // store as a 4x4 homogeneous matrix to exploit SSE optimizations
226  // below:
228  if (!isDirectCorresp)
229  {
234  src_obs.relativePoseIntensityWRTDepth.m_coords, R_inv, t_inv);
235 
236  T_inv(3, 3) = 1;
237  T_inv.block<3, 3>(0, 0) = R_inv.cast<float>();
238  T_inv.block<3, 1>(0, 3) = t_inv.cast<float>();
239  }
240 
241  Eigen::Matrix<float, 4, 1> pt_wrt_color, pt_wrt_depth;
242  pt_wrt_depth[3] = 1;
243 
244  mrpt::img::TColor pCol;
245 
246  // For each local point:
247  const size_t nPts = pca.size();
248  for (size_t i = 0; i < nPts; i++)
249  {
250  int img_idx_x, img_idx_y; // projected pixel coordinates, in the
251  // RGB image plane
252  bool pointWithinImage = false;
253  if (isDirectCorresp)
254  {
255  pointWithinImage = true;
256  img_idx_x = src_obs.points3D_idxs_x[i];
257  img_idx_y = src_obs.points3D_idxs_y[i];
258  }
259  else
260  {
261  // Project point, which is now in "pca" in local coordinates wrt
262  // the depth camera, into the intensity camera:
263  pca.getPointXYZ(
264  i, pt_wrt_depth[0], pt_wrt_depth[1], pt_wrt_depth[2]);
265  pt_wrt_color = T_inv * pt_wrt_depth;
266 
267  // Project to image plane:
268  if (pt_wrt_color[2])
269  {
270  img_idx_x = mrpt::round(
271  cx + fx * pt_wrt_color[0] / pt_wrt_color[2]);
272  img_idx_y = mrpt::round(
273  cy + fy * pt_wrt_color[1] / pt_wrt_color[2]);
274  pointWithinImage = img_idx_x >= 0 && img_idx_x < imgW &&
275  img_idx_y >= 0 && img_idx_y < imgH;
276  }
277  }
278 
279  if (pointWithinImage)
280  {
281  if (hasColorIntensityImg)
282  {
283  const uint8_t* c = src_obs.intensityImage.get_unsafe(
284  img_idx_x, img_idx_y, 0);
285  pCol.R = c[2];
286  pCol.G = c[1];
287  pCol.B = c[0];
288  }
289  else
290  {
291  uint8_t c = *src_obs.intensityImage.get_unsafe(
292  img_idx_x, img_idx_y, 0);
293  pCol.R = pCol.G = pCol.B = c;
294  }
295  }
296  else
297  {
298  pCol.R = pCol.G = pCol.B = 255;
299  }
300  // Set color:
301  pca.setPointRGBu8(i, pCol.R, pCol.G, pCol.B);
302  } // end for each point
303  } // end if src_obs has intensity image
304 
305  // ...
306 
307  // ------------------------------------------------------------
308  // Stage 3/3: Apply 6D transformations
309  // ------------------------------------------------------------
310  if (projectParams.takeIntoAccountSensorPoseOnRobot ||
311  projectParams.robotPoseInTheWorld)
312  {
313  mrpt::poses::CPose3D transf_to_apply; // Either ROBOTPOSE or
314  // ROBOTPOSE(+)SENSORPOSE or
315  // SENSORPOSE
316  if (projectParams.takeIntoAccountSensorPoseOnRobot)
317  transf_to_apply = src_obs.sensorPose;
318  if (projectParams.robotPoseInTheWorld)
319  transf_to_apply.composeFrom(
320  *projectParams.robotPoseInTheWorld,
321  mrpt::poses::CPose3D(transf_to_apply));
322 
323  const Eigen::Matrix<float, 4, 4> HM =
324  transf_to_apply
326  .cast<float>();
327  Eigen::Matrix<float, 4, 1> pt, pt_transf;
328  pt[3] = 1;
329 
330  const size_t nPts = pca.size();
331  for (size_t i = 0; i < nPts; i++)
332  {
333  pca.getPointXYZ(i, pt[0], pt[1], pt[2]);
334  pt_transf = HM * pt;
335  pca.setPointXYZ(i, pt_transf[0], pt_transf[1], pt_transf[2]);
336  }
337  }
338 } // end of project3DPointsFromDepthImageInto
339 
340 // Auxiliary functions which implement proyection of 3D point clouds:
341 template <class POINTMAP>
343  const int H, const int W, const float* kys, const float* kzs,
344  const mrpt::math::CMatrix& rangeImage,
346  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
347  const mrpt::obs::TRangeImageFilterParams& fp, bool MAKE_DENSE)
348 {
349  TRangeImageFilter rif(fp);
350  // Preconditions: minRangeMask() has the right size
351  size_t idx = 0;
352  for (int r = 0; r < H; r++)
353  for (int c = 0; c < W; c++)
354  {
355  const float D = rangeImage.coeff(r, c);
356  if (!rif.do_range_filter(r, c, D))
357  {
358  if (!MAKE_DENSE)
359  {
360  pca.setInvalidPoint(idx);
361  ++idx;
362  }
363  continue;
364  }
365 
366  pca.setPointXYZ(idx, D /*x*/, *kys++ * D /*y*/, *kzs++ * D /*z*/);
367  idxs_x[idx] = c;
368  idxs_y[idx] = r;
369  ++idx;
370  }
371  pca.resize(idx);
372 }
373 
374 // Auxiliary functions which implement proyection of 3D point clouds:
375 template <class POINTMAP>
377  const int H, const int W, const float* kys, const float* kzs,
378  const mrpt::math::CMatrix& rangeImage,
380  std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
381  const mrpt::obs::TRangeImageFilterParams& filterParams, bool MAKE_DENSE)
382 {
383 #if MRPT_HAS_SSE2
384  // Preconditions: minRangeMask() has the right size
385  // Use optimized version:
386  const int W_4 = W >> 2; // /=4 , since we process 4 values at a time.
387  size_t idx = 0;
388  alignas(MRPT_MAX_ALIGN_BYTES) float xs[4], ys[4], zs[4];
389  const __m128 D_zeros = _mm_set_ps(.0f, .0f, .0f, .0f);
390  const __m128 xormask =
391  (filterParams.rangeCheckBetween)
392  ? _mm_cmpneq_ps(D_zeros, D_zeros)
393  : // want points BETWEEN min and max to be valid
394  _mm_cmpeq_ps(
395  D_zeros,
396  D_zeros); // want points OUTSIDE of min and max to be valid
397  for (int r = 0; r < H; r++)
398  {
399  const float* D_ptr =
400  &rangeImage.coeffRef(r, 0); // Matrices are 16-aligned
401  const float* Dgt_ptr =
402  !filterParams.rangeMask_min
403  ? nullptr
404  : &filterParams.rangeMask_min->coeffRef(r, 0);
405  const float* Dlt_ptr =
406  !filterParams.rangeMask_max
407  ? nullptr
408  : &filterParams.rangeMask_max->coeffRef(r, 0);
409 
410  for (int c = 0; c < W_4; c++)
411  {
412  const __m128 D = _mm_load_ps(D_ptr);
413  const __m128 nz_mask = _mm_cmpgt_ps(D, D_zeros);
414  __m128 valid_range_mask;
415  if (!filterParams.rangeMask_min && !filterParams.rangeMask_max)
416  { // No filter: just skip D=0 points
417  valid_range_mask = nz_mask;
418  }
419  else
420  {
421  if (!filterParams.rangeMask_min || !filterParams.rangeMask_max)
422  { // Only one filter
423  if (filterParams.rangeMask_min)
424  {
425  const __m128 Dmin = _mm_load_ps(Dgt_ptr);
426  valid_range_mask = _mm_and_ps(
427  _mm_cmpgt_ps(D, Dmin), _mm_cmpgt_ps(Dmin, D_zeros));
428  }
429  else
430  {
431  const __m128 Dmax = _mm_load_ps(Dlt_ptr);
432  valid_range_mask = _mm_and_ps(
433  _mm_cmplt_ps(D, Dmax), _mm_cmpgt_ps(Dmax, D_zeros));
434  }
435  valid_range_mask = _mm_and_ps(
436  valid_range_mask, nz_mask); // Filter out D=0 points
437  }
438  else
439  {
440  // We have both: D>Dmin and D<Dmax conditions, with XOR to
441  // optionally invert the selection:
442  const __m128 Dmin = _mm_load_ps(Dgt_ptr);
443  const __m128 Dmax = _mm_load_ps(Dlt_ptr);
444 
445  const __m128 gt_mask = _mm_cmpgt_ps(D, Dmin);
446  const __m128 lt_mask = _mm_and_ps(
447  _mm_cmplt_ps(D, Dmax), nz_mask); // skip points at zero
448  valid_range_mask =
449  _mm_and_ps(gt_mask, lt_mask); // (D>Dmin && D<Dmax)
450  valid_range_mask = _mm_xor_ps(valid_range_mask, xormask);
451  // Add the case of D_min & D_max = 0 (no filtering)
452  valid_range_mask = _mm_or_ps(
453  valid_range_mask, _mm_and_ps(
454  _mm_cmpeq_ps(Dmin, D_zeros),
455  _mm_cmpeq_ps(Dmax, D_zeros)));
456  // Finally, ensure no invalid ranges get thru:
457  valid_range_mask = _mm_and_ps(valid_range_mask, nz_mask);
458  }
459  }
460  const int valid_range_maski = _mm_movemask_epi8(
461  _mm_castps_si128(valid_range_mask)); // 0x{f|0}{f|0}{f|0}{f|0}
462  if (valid_range_maski != 0) // Any of the 4 values is valid?
463  {
464  const __m128 KY = _mm_load_ps(kys);
465  const __m128 KZ = _mm_load_ps(kzs);
466 
467  _mm_storeu_ps(xs, D);
468  _mm_storeu_ps(ys, _mm_mul_ps(KY, D));
469  _mm_storeu_ps(zs, _mm_mul_ps(KZ, D));
470 
471  for (int q = 0; q < 4; q++)
472  if ((valid_range_maski & (1 << (q * 4))) != 0)
473  {
474  pca.setPointXYZ(idx, xs[q], ys[q], zs[q]);
475  idxs_x[idx] = (c << 2) + q;
476  idxs_y[idx] = r;
477  ++idx;
478  }
479  else if (!MAKE_DENSE)
480  {
481  pca.setInvalidPoint(idx);
482  ++idx;
483  }
484  }
485  else if (!MAKE_DENSE)
486  {
487  for (int q = 0; q < 4; q++)
488  {
489  pca.setInvalidPoint(idx);
490  ++idx;
491  }
492  }
493  D_ptr += 4;
494  if (Dgt_ptr) Dgt_ptr += 4;
495  if (Dlt_ptr) Dlt_ptr += 4;
496  kys += 4;
497  kzs += 4;
498  }
499  }
500  pca.resize(idx);
501 #endif
502 }
503 
504 } // namespace detail
505 } // namespace obs
506 } // namespace mrpt
507 #endif
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
Mainly for internal use within CObservation3DRangeScan::project3DPointsFromDepthImageInto() ...
#define MRPT_MAX_ALIGN_BYTES
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:165
An adapter to different kinds of point cloud object.
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
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.
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:167
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.
const mrpt::math::CMatrix * rangeMask_min
(Default: nullptr) If provided, each data range will be tested to be greater-than (rangeMask_min) or ...
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:892
uint8_t B
Definition: TColor.h:48
uint8_t G
Definition: TColor.h:48
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
unsigned char uint8_t
Definition: rptypes.h:41
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
void do_project_3d_pointcloud(const int H, const int W, const float *kys, const float *kzs, const mrpt::math::CMatrix &rangeImage, mrpt::opengl::PointCloudAdapter< POINTMAP > &pca, std::vector< uint16_t > &idxs_x, std::vector< uint16_t > &idxs_y, const mrpt::obs::TRangeImageFilterParams &filterParams, bool MAKE_DENSE)
A numeric matrix of compile-time fixed size.
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:864
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:163
An adapter to different kinds of point cloud object.
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
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:565
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
const GLubyte * c
Definition: glext.h:6313
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
uint8_t R
Definition: TColor.h:48
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:161
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
Definition: CImage.cpp:906
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
const mrpt::math::CMatrix * rangeMask_max
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...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
static TCached3DProjTables & get_3dproj_lut()
3D point cloud projection look-up-table
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:275
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)
A RGB color - 8bit.
Definition: TColor.h:22
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:232
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
bool MAKE_DENSE
(Default:true) set to false if you want to preserve the organization of the point cloud ...
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:24
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
void do_project_3d_pointcloud_SSE2(const int H, const int W, const float *kys, const float *kzs, const mrpt::math::CMatrix &rangeImage, mrpt::opengl::PointCloudAdapter< POINTMAP > &pca, std::vector< uint16_t > &idxs_x, std::vector< uint16_t > &idxs_y, const mrpt::obs::TRangeImageFilterParams &filterParams, bool MAKE_DENSE)
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated.
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:491
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:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019