Go to the documentation of this file.
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;
An adapter to different kinds of point cloud object.
int round(const T value)
Returns the closer integer (int) to x.
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
#define ASSERT_EQUAL_(__A, __B)
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 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)
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,...
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
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 project3DPointsFromDepthImageInto(CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const bool takeIntoAccountSensorPoseOnRobot, const mrpt::poses::CPose3D *robotPoseInTheWorld, const bool PROJ3D_USE_LUT)
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.
double cy() const
Get the value of the principal point y-coordinate (in pixels).
mrpt::utils::TCamera cameraParams
Projection parameters of the depth camera.
mrpt::math::CVectorFloat Kzs
static TCached3DProjTables m_3dproj_lut
3D point cloud projection look-up-table
double cx() const
Get the value of the principal point x-coordinate (in pixels).
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,...
double fx() const
Get the value of the focal length x-value (in pixels).
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,...
size_t getWidth() const MRPT_OVERRIDE
Returns the width of the image in pixels.
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
size_t getHeight() const MRPT_OVERRIDE
Returns the height of the image in pixels.
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::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage".
std::vector< uint16_t > points3D_idxs_x
A numeric matrix of compile-time fixed size.
double fy() const
Get the value of the focal length y-value (in pixels).
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters)
This base provides a set of functions for maths stuff.
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...
mrpt::math::CVectorFloat Kys
mrpt::utils::TCamera prev_camParams
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
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.4.0 SVN: at Sat Jan 18 22:37:07 UTC 2020 | | |