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;
#define ASSERT_EQUAL_(__A, __B)
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,...
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
mrpt::math::CVectorFloat Kzs
std::vector< uint16_t > points3D_idxs_x
double cy() const
Get the value of the principal point y-coordinate (in pixels).
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,...
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
size_t getWidth() const MRPT_OVERRIDE
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 fx() const
Get the value of the focal length x-value (in pixels).
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.
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)
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
std::vector< uint16_t > points3D_idxs_y
//!< If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x,...
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods.
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
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)
size_t getHeight() const MRPT_OVERRIDE
Returns the height of the image in pixels.
bool hasRangeImage
true means the field rangeImage contains valid data
An adapter to different kinds of point cloud object.
mrpt::math::CVectorFloat Kys
double cx() const
Get the value of the principal point x-coordinate (in pixels).
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
int round(const T value)
Returns the closer integer (int) to x.
bool doDepthAndIntensityCamerasCoincide() const
Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,...
An adapter to different kinds of point cloud object.
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage".
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...
static TCached3DProjTables m_3dproj_lut
3D point cloud projection look-up-table
mrpt::utils::TCamera prev_camParams