Go to the documentation of this file.
9 #ifndef CObservation3DRangeScan_project3D_impl_H
10 #define CObservation3DRangeScan_project3D_impl_H
22 template <
class POINTMAP>
24 const int H,
const int W,
const float* kys,
const float* kzs,
27 std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
29 template <
class POINTMAP>
31 const int H,
const int W,
const float* kys,
const float* kzs,
34 std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
37 template <
class POINTMAP>
55 const size_t WH = W * H;
86 for (
int r = 0;
r < H;
r++)
87 for (
int c = 0;
c < W;
c++)
89 *kys++ = (r_cx -
c) * r_fx_inv;
90 *kzs++ = (r_cy -
r) * r_fy_inv;
118 if ((W & 0x07) == 0 && projectParams.
USE_SSE2)
146 for (
int r = 0;
r < H;
r++)
147 for (
int c = 0;
c < W;
c++)
152 const float Kz = (r_cy -
r) * r_fy_inv;
153 const float Ky = (r_cx -
c) * r_fx_inv;
184 for (
int r = 0;
r < H;
r++)
185 for (
int c = 0;
c < W;
c++)
190 const float Ky = (r_cx -
c) * r_fx_inv;
191 const float Kz = (r_cy -
r) * r_fy_inv;
194 D / std::sqrt(1 + Ky * Ky + Kz * Kz),
221 const bool isDirectCorresp =
228 if (!isDirectCorresp)
237 T_inv.block<3, 3>(0, 0) = R_inv.cast<
float>();
238 T_inv.block<3, 1>(0, 3) = t_inv.cast<
float>();
241 Eigen::Matrix<float, 4, 1> pt_wrt_color, pt_wrt_depth;
247 const size_t nPts = pca.size();
248 for (
size_t i = 0; i < nPts; i++)
250 int img_idx_x, img_idx_y;
252 bool pointWithinImage =
false;
255 pointWithinImage =
true;
264 i, pt_wrt_depth[0], pt_wrt_depth[1], pt_wrt_depth[2]);
265 pt_wrt_color = T_inv * pt_wrt_depth;
271 cx + fx * pt_wrt_color[0] / pt_wrt_color[2]);
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;
279 if (pointWithinImage)
281 if (hasColorIntensityImg)
284 img_idx_x, img_idx_y, 0);
292 img_idx_x, img_idx_y, 0);
293 pCol.
R = pCol.
G = pCol.
B =
c;
298 pCol.
R = pCol.
G = pCol.
B = 255;
301 pca.setPointRGBu8(i, pCol.
R, pCol.
G, pCol.
B);
323 const Eigen::Matrix<float, 4, 4> HM =
327 Eigen::Matrix<float, 4, 1> pt, pt_transf;
330 const size_t nPts = pca.size();
331 for (
size_t i = 0; i < nPts; i++)
333 pca.getPointXYZ(i, pt[0], pt[1], pt[2]);
335 pca.setPointXYZ(i, pt_transf[0], pt_transf[1], pt_transf[2]);
341 template <
class POINTMAP>
343 const int H,
const int W,
const float* kys,
const float* kzs,
346 std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
352 for (
int r = 0;
r < H;
r++)
353 for (
int c = 0;
c < W;
c++)
355 const float D = rangeImage.coeff(
r,
c);
360 pca.setInvalidPoint(idx);
366 pca.setPointXYZ(idx, D , *kys++ * D , *kzs++ * D );
375 template <
class POINTMAP>
377 const int H,
const int W,
const float* kys,
const float* kzs,
380 std::vector<uint16_t>& idxs_x, std::vector<uint16_t>& idxs_y,
386 const int W_4 = W >> 2;
389 const __m128 D_zeros = _mm_set_ps(.0f, .0f, .0f, .0f);
390 const __m128 xormask =
392 ? _mm_cmpneq_ps(D_zeros, D_zeros)
397 for (
int r = 0;
r < H;
r++)
400 &rangeImage.coeffRef(
r, 0);
401 const float* Dgt_ptr =
405 const float* Dlt_ptr =
410 for (
int c = 0;
c < W_4;
c++)
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;
417 valid_range_mask = nz_mask;
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));
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));
435 valid_range_mask = _mm_and_ps(
436 valid_range_mask, nz_mask);
442 const __m128 Dmin = _mm_load_ps(Dgt_ptr);
443 const __m128 Dmax = _mm_load_ps(Dlt_ptr);
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);
449 _mm_and_ps(gt_mask, lt_mask);
450 valid_range_mask = _mm_xor_ps(valid_range_mask, xormask);
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)));
457 valid_range_mask = _mm_and_ps(valid_range_mask, nz_mask);
460 const int valid_range_maski = _mm_movemask_epi8(
461 _mm_castps_si128(valid_range_mask));
462 if (valid_range_maski != 0)
464 const __m128 KY = _mm_load_ps(kys);
465 const __m128 KZ = _mm_load_ps(kzs);
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));
471 for (
int q = 0;
q < 4;
q++)
472 if ((valid_range_maski & (1 << (
q * 4))) != 0)
474 pca.setPointXYZ(idx, xs[
q], ys[
q], zs[
q]);
475 idxs_x[idx] = (
c << 2) +
q;
479 else if (!MAKE_DENSE)
481 pca.setInvalidPoint(idx);
485 else if (!MAKE_DENSE)
487 for (
int q = 0;
q < 4;
q++)
489 pca.setInvalidPoint(idx);
494 if (Dgt_ptr) Dgt_ptr += 4;
495 if (Dlt_ptr) Dlt_ptr += 4;
An adapter to different kinds of point cloud object.
const mrpt::math::CMatrix * rangeMask_max
Mainly for internal use within CObservation3DRangeScan::project3DPointsFromDepthImageInto()
size_t getWidth() const override
Returns the width of the image in pixels.
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
bool rangeCheckBetween
Only used if both rangeMask_min and rangeMask_max are present.
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...
static TCached3DProjTables & get_3dproj_lut()
3D point cloud projection look-up-table
GLdouble GLdouble GLdouble GLdouble q
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
double fx() const
Get the value of the focal length x-value (in pixels).
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.
mrpt::img::TCamera prev_camParams
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...
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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)
#define ASSERT_(f)
Defines an assertion mechanism.
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
double cy() const
Get the value of the principal point y-coordinate (in pixels).
mrpt::math::CVectorFloat Kzs
void resizePoints3DVectors(const size_t nPoints)
Use this method instead of resizing all three points3D_x, points3D_y & points3D_z to allow the usage ...
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
double fy() const
Get the value of the focal length y-value (in pixels).
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
GLdouble GLdouble GLdouble r
const mrpt::math::CMatrix * rangeMask_min
(Default: nullptr) If provided, each data range will be tested to be greater-than (rangeMask_min) or ...
bool PROJ3D_USE_LUT
(Default:true) [Only used when range_is_depth=true] Whether to use a Look-up-table (LUT) to speed up ...
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage".
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement,...
double cx() const
Get the value of the principal point x-coordinate (in pixels).
int round(const T value)
Returns the closer integer (int) to x.
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)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
bool hasIntensityImage
true means the field intensityImage contains valid data
bool hasRangeImage
true means the field rangeImage contains valid data
bool USE_SSE2
(Default:true) If possible, use SSE2 optimized code.
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,...
std::vector< uint16_t > points3D_idxs_y
std::vector< uint16_t > points3D_idxs_x
If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x,...
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
A numeric matrix of compile-time fixed size.
void project3DPointsFromDepthImageInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
const mrpt::poses::CPose3D * robotPoseInTheWorld
(Default: nullptr) Read takeIntoAccountSensorPoseOnRobot
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters)
bool MAKE_DENSE
(Default:true) set to false if you want to preserve the organization of the point cloud
size_t getHeight() const override
Returns the height of the image in pixels.
bool doDepthAndIntensityCamerasCoincide() const
Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,...
This base provides a set of functions for maths stuff.
mrpt::math::CVectorFloat Kys
#define MRPT_MAX_ALIGN_BYTES
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated.
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods.
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |