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;
69 *kys++ = (r_cx - c) * r_fx_inv;
70 *kzs++ = (r_cy - r) * r_fy_inv;
98 const float Kz = (r_cy - r) * r_fy_inv;
99 const float Ky = (r_cx - c) * r_fx_inv;
100 const float D = src_obs.
rangeImage.coeff(r,c);
101 pca.setPointXYZ(idx++,
124 for (
int r=0;r<H;r++)
125 for (
int c=0;c<W;c++)
127 const float Ky = (r_cx - c) * r_fx_inv;
128 const float Kz = (r_cy - r) * r_fy_inv;
129 const float D = src_obs.
rangeImage.coeff(r,c);
130 pca.setPointXYZ(idx++,
131 D / std::sqrt(1+Ky*Ky+Kz*Kz),
158 if (!isDirectCorresp)
167 T_inv.block<3,3>(0,0)=R_inv.cast<
float>();
168 T_inv.block<3,1>(0,3)=t_inv.cast<
float>();
171 Eigen::Matrix<float,4,1> pt_wrt_color, pt_wrt_depth;
174 int img_idx_x=0, img_idx_y=0;
178 for (
size_t i=0;i<WH;i++)
180 bool pointWithinImage =
false;
183 pointWithinImage=
true;
185 if (img_idx_x>=imgW) {
193 pca.getPointXYZ(i,pt_wrt_depth[0],pt_wrt_depth[1],pt_wrt_depth[2]);
194 pt_wrt_color.noalias() = T_inv*pt_wrt_depth;
197 if (pt_wrt_color[2]) {
201 img_idx_x>=0 && img_idx_x<imgW &&
202 img_idx_y>=0 && img_idx_y<imgH;
206 if (pointWithinImage)
208 if (hasColorIntensityImg) {
216 pCol.
R = pCol.
G = pCol.
B = c;
221 pCol.
R = pCol.
G = pCol.
B = 255;
224 pca.setPointRGBu8(i,pCol.
R,pCol.
G,pCol.
B);
233 if (takeIntoAccountSensorPoseOnRobot || robotPoseInTheWorld)
236 if (takeIntoAccountSensorPoseOnRobot)
238 if (robotPoseInTheWorld)
242 Eigen::Matrix<float,4,1> pt, pt_transf;
245 for (
size_t i=0;i<WH;i++)
247 pca.getPointXYZ(i,pt[0],pt[1],pt[2]);
248 pt_transf.noalias() = HM*pt;
249 pca.setPointXYZ(i,pt_transf[0],pt_transf[1],pt_transf[2]);
255 template <
class POINTMAP>
259 for (
int r=0;r<H;r++)
260 for (
int c=0;c<W;c++)
262 const float D = rangeImage.coeff(r,c);
263 pca.setPointXYZ(idx++,
272 template <
class POINTMAP>
277 const int W_4 = W >> 2;
280 for (
int r=0;r<H;r++)
282 const float *D_ptr = &rangeImage.coeffRef(r,0);
284 for (
int c=0;c<W_4;c++)
286 const __m128 D = _mm_load_ps(D_ptr);
288 const __m128 KY = _mm_load_ps(kys);
289 const __m128 KZ = _mm_load_ps(kzs);
291 _mm_storeu_ps(xs , D);
292 _mm_storeu_ps(ys , _mm_mul_ps(KY,D));
293 _mm_storeu_ps(zs , _mm_mul_ps(KZ,D));
298 pca.setPointXYZ(idx++,xs[0],ys[0],zs[0]);
299 pca.setPointXYZ(idx++,xs[1],ys[1],zs[1]);
300 pca.setPointXYZ(idx++,xs[2],ys[2],zs[2]);
301 pca.setPointXYZ(idx++,xs[3],ys[3],zs[3]);
#define ASSERT_EQUAL_(__A, __B)
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
mrpt::math::CVectorFloat Kzs
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)
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
void project3DPointsFromDepthImageInto(CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const bool takeIntoAccountSensorPoseOnRobot, const mrpt::poses::CPose3D *robotPoseInTheWorld, const bool PROJ3D_USE_LUT)
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.
bool hasIntensityImage
true means the field intensityImage contains valid data
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
size_t getWidth() const
Returns the width of the image in pixels.
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
mrpt::utils::TCamera cameraParams
Projection parameters of the depth camera.
double fy() const
Get the value of the focal length y-value (in pixels).
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
bool hasRangeImage
true means the field rangeImage contains valid data
An adapter to different kinds of point cloud object.
mrpt::math::CVectorFloat Kys
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)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
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...
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.
int round(const T value)
Returns the closer integer (int) to x.
double fx() const
Get the value of the focal length x-value (in pixels).
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
An adapter to different kinds of point cloud object.
bool doDepthAndIntensityCamerasCoincide() const
Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
This class is a "CSerializable" wrapper for "CMatrixFloat".
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
double cx() const
Get the value of the principal point x-coordinate (in pixels).
static TCached3DProjTables m_3dproj_lut
3D point cloud projection look-up-table
size_t getHeight() const
Returns the height of the image in pixels.
mrpt::utils::TCamera prev_camParams
double cy() const
Get the value of the principal point y-coordinate (in pixels).