9 #ifndef CObservation3DRangeScan_project3D_impl_H
10 #define CObservation3DRangeScan_project3D_impl_H
21 template <
class POINTMAP>
24 POINTMAP & dest_pointcloud,
25 const bool takeIntoAccountSensorPoseOnRobot,
27 const bool PROJ3D_USE_LUT)
40 const size_t WH = W*H;
68 *kys++ = (r_cx - c) * r_fx_inv;
69 *kzs++ = (r_cy - r) * r_fy_inv;
99 const float Kz = (r_cy - r) * r_fy_inv;
100 const float Ky = (r_cx - c) * r_fx_inv;
129 for (
int r=0;r<H;r++)
130 for (
int c=0;c<W;c++)
132 const float D = src_obs.
rangeImage.coeff(r,c);
134 const float Ky = (r_cx - c) * r_fx_inv;
135 const float Kz = (r_cy - r) * r_fy_inv;
137 D / std::sqrt(1+Ky*Ky+Kz*Kz),
169 if (!isDirectCorresp)
178 T_inv.block<3,3>(0,0)=R_inv.cast<
float>();
179 T_inv.block<3,1>(0,3)=t_inv.cast<
float>();
182 Eigen::Matrix<float,4,1> pt_wrt_color, pt_wrt_depth;
188 const size_t nPts = pca.size();
189 for (
size_t i=0;i<nPts;i++)
191 int img_idx_x, img_idx_y;
192 bool pointWithinImage =
false;
195 pointWithinImage=
true;
202 pca.getPointXYZ(i,pt_wrt_depth[0],pt_wrt_depth[1],pt_wrt_depth[2]);
203 pt_wrt_color.noalias() = T_inv*pt_wrt_depth;
206 if (pt_wrt_color[2]) {
210 img_idx_x>=0 && img_idx_x<imgW &&
211 img_idx_y>=0 && img_idx_y<imgH;
215 if (pointWithinImage)
217 if (hasColorIntensityImg) {
225 pCol.
R = pCol.
G = pCol.
B = c;
230 pCol.
R = pCol.
G = pCol.
B = 255;
233 pca.setPointRGBu8(i,pCol.
R,pCol.
G,pCol.
B);
242 if (takeIntoAccountSensorPoseOnRobot || robotPoseInTheWorld)
245 if (takeIntoAccountSensorPoseOnRobot)
247 if (robotPoseInTheWorld)
251 Eigen::Matrix<float,4,1> pt, pt_transf;
254 const size_t nPts = pca.size();
255 for (
size_t i=0;i<nPts;i++)
257 pca.getPointXYZ(i,pt[0],pt[1],pt[2]);
258 pt_transf.noalias() = HM*pt;
259 pca.setPointXYZ(i,pt_transf[0],pt_transf[1],pt_transf[2]);
265 template <
class POINTMAP>
269 for (
int r=0;r<H;r++)
270 for (
int c=0;c<W;c++)
272 const float D = rangeImage.coeff(r,c);
274 pca.setPointXYZ(idx, D , *kys++ * D , *kzs++ * D );
284 template <
class POINTMAP>
289 const int W_4 = W >> 2;
292 for (
int r=0;r<H;r++)
294 const float *D_ptr = &rangeImage.coeffRef(r,0);
296 for (
int c=0;c<W_4;c++)
298 const __m128 D = _mm_load_ps(D_ptr);
300 const __m128 KY = _mm_load_ps(kys);
301 const __m128 KZ = _mm_load_ps(kzs);
303 _mm_storeu_ps(xs , D);
304 _mm_storeu_ps(ys , _mm_mul_ps(KY,D));
305 _mm_storeu_ps(zs , _mm_mul_ps(KZ,D));
307 for (
int q=0;q<4;q++)
309 pca.setPointXYZ(idx,xs[q],ys[q],zs[q]);
310 idxs_x[idx]=(c<<2)+q;
A numeric matrix of compile-time fixed size.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement,...
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
std::vector< uint16_t > points3D_idxs_x
mrpt::utils::TCamera cameraParams
Projection parameters of the depth camera.
bool hasIntensityImage
true means the field intensityImage contains valid data
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
bool doDepthAndIntensityCamerasCoincide() const
Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,...
std::vector< uint16_t > points3D_idxs_y
//!< If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x,...
bool hasRangeImage
true means the field rangeImage contains valid data
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters)
static TCached3DProjTables m_3dproj_lut
3D point cloud projection look-up-table
mrpt::utils::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.
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods.
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...
size_t getWidth() const MRPT_OVERRIDE
Returns the width of the image in pixels.
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
size_t getHeight() const MRPT_OVERRIDE
Returns the height of the image in pixels.
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,...
An adapter to different kinds of point cloud object.
double cx() const
Get the value of the principal point x-coordinate (in pixels).
double fx() const
Get the value of the focal length x-value (in pixels).
double cy() const
Get the value of the principal point y-coordinate (in pixels).
double fy() const
Get the value of the focal length y-value (in pixels).
int round(const T value)
Returns the closer integer (int) to x.
#define ASSERT_EQUAL_(__A, __B)
This base provides a set of functions for maths stuff.
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 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)
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)
void project3DPointsFromDepthImageInto(CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const bool takeIntoAccountSensorPoseOnRobot, const mrpt::poses::CPose3D *robotPoseInTheWorld, const bool PROJ3D_USE_LUT)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::utils::TCamera prev_camParams
mrpt::math::CVectorFloat Kys
mrpt::math::CVectorFloat Kzs