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)
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 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...
bool hasIntensityImage
true means the field intensityImage contains valid data
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.
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
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).