Main MRPT website > C++ reference for MRPT 1.3.2
CObservation3DRangeScan_project3D_impl.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CObservation3DRangeScan_project3D_impl_H
10 #define CObservation3DRangeScan_project3D_impl_H
11 
12 #include <mrpt/utils/round.h> // round()
13 
14 namespace mrpt {
15 namespace obs {
16 namespace detail {
17  // Auxiliary functions which implement SSE-optimized proyection of 3D point cloud:
18  template <class POINTMAP> 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);
19  template <class POINTMAP> 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);
20 
21  template <class POINTMAP>
23  CObservation3DRangeScan & src_obs,
24  POINTMAP & dest_pointcloud,
25  const bool takeIntoAccountSensorPoseOnRobot,
26  const mrpt::poses::CPose3D * robotPoseInTheWorld,
27  const bool PROJ3D_USE_LUT)
28  {
29  using namespace mrpt::math;
30 
31  if (!src_obs.hasRangeImage) return;
32 
33  mrpt::utils::PointCloudAdapter<POINTMAP> pca(dest_pointcloud);
34 
35  // ------------------------------------------------------------
36  // Stage 1/3: Create 3D point cloud local coordinates
37  // ------------------------------------------------------------
38  const int W = src_obs.rangeImage.cols();
39  const int H = src_obs.rangeImage.rows();
40  const size_t WH = W*H;
41 
42  // Reserve memory for 3D points:
43  pca.resize(WH);
44 
45  if (src_obs.range_is_depth)
46  {
47  // range_is_depth = true
48 
49  // Use cached tables?
50  if (PROJ3D_USE_LUT)
51  {
52  // Use LUT:
53  if (src_obs.m_3dproj_lut.prev_camParams!=src_obs.cameraParams || WH!=size_t(src_obs.m_3dproj_lut.Kys.size()))
54  {
55  src_obs.m_3dproj_lut.prev_camParams = src_obs.cameraParams;
56  src_obs.m_3dproj_lut.Kys.resize(WH);
57  src_obs.m_3dproj_lut.Kzs.resize(WH);
58 
59  const float r_cx = src_obs.cameraParams.cx();
60  const float r_cy = src_obs.cameraParams.cy();
61  const float r_fx_inv = 1.0f/src_obs.cameraParams.fx();
62  const float r_fy_inv = 1.0f/src_obs.cameraParams.fy();
63 
64  float *kys = &src_obs.m_3dproj_lut.Kys[0];
65  float *kzs = &src_obs.m_3dproj_lut.Kzs[0];
66  for (int r=0;r<H;r++)
67  for (int c=0;c<W;c++)
68  {
69  *kys++ = (r_cx - c) * r_fx_inv;
70  *kzs++ = (r_cy - r) * r_fy_inv;
71  }
72  } // end update LUT.
73 
74  ASSERT_EQUAL_(WH,size_t(src_obs.m_3dproj_lut.Kys.size()))
75  ASSERT_EQUAL_(WH,size_t(src_obs.m_3dproj_lut.Kzs.size()))
76  float *kys = &src_obs.m_3dproj_lut.Kys[0];
77  float *kzs = &src_obs.m_3dproj_lut.Kzs[0];
78 
79  #if MRPT_HAS_SSE2
80  if ((W & 0x07)==0)
81  do_project_3d_pointcloud_SSE2(H,W,kys,kzs,src_obs.rangeImage,pca);
82  else do_project_3d_pointcloud(H,W,kys,kzs,src_obs.rangeImage,pca); // if image width is not 8*N, use standard method
83  #else
84  do_project_3d_pointcloud(H,W,kys,kzs,src_obs.rangeImage,pca);
85  #endif
86  }
87  else
88  {
89  // Without LUT:
90  const float r_cx = src_obs.cameraParams.cx();
91  const float r_cy = src_obs.cameraParams.cy();
92  const float r_fx_inv = 1.0f/src_obs.cameraParams.fx();
93  const float r_fy_inv = 1.0f/src_obs.cameraParams.fy();
94  size_t idx=0;
95  for (int r=0;r<H;r++)
96  for (int c=0;c<W;c++)
97  {
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++,
102  D, // x
103  Ky * D, // y
104  Kz * D // z
105  );
106  }
107  }
108  }
109  else
110  {
111  /* range_is_depth = false :
112  * Ky = (r_cx - c)/r_fx
113  * Kz = (r_cy - r)/r_fy
114  *
115  * x(i) = rangeImage(r,c) / sqrt( 1 + Ky^2 + Kz^2 )
116  * y(i) = Ky * x(i)
117  * z(i) = Kz * x(i)
118  */
119  const float r_cx = src_obs.cameraParams.cx();
120  const float r_cy = src_obs.cameraParams.cy();
121  const float r_fx_inv = 1.0f/src_obs.cameraParams.fx();
122  const float r_fy_inv = 1.0f/src_obs.cameraParams.fy();
123  size_t idx=0;
124  for (int r=0;r<H;r++)
125  for (int c=0;c<W;c++)
126  {
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), // x
132  Ky * D, // y
133  Kz * D // z
134  );
135  }
136  }
137 
138  // -------------------------------------------------------------
139  // Stage 2/3: Project local points into RGB image to get colors
140  // -------------------------------------------------------------
141  if (src_obs.hasIntensityImage)
142  {
143  const int imgW = src_obs.intensityImage.getWidth();
144  const int imgH = src_obs.intensityImage.getHeight();
145  const bool hasColorIntensityImg = src_obs.intensityImage.isColor();
146 
147  const float cx = src_obs.cameraParamsIntensity.cx();
148  const float cy = src_obs.cameraParamsIntensity.cy();
149  const float fx = src_obs.cameraParamsIntensity.fx();
150  const float fy = src_obs.cameraParamsIntensity.fy();
151 
152  // Unless we are in a special case (both depth & RGB images coincide)...
153  const bool isDirectCorresp = src_obs.doDepthAndIntensityCamerasCoincide();
154 
155  // ...precompute the inverse of the pose transformation out of the loop,
156  // store as a 4x4 homogeneous matrix to exploit SSE optimizations below:
158  if (!isDirectCorresp)
159  {
164  R_inv,t_inv);
165 
166  T_inv(3,3)=1;
167  T_inv.block<3,3>(0,0)=R_inv.cast<float>();
168  T_inv.block<3,1>(0,3)=t_inv.cast<float>();
169  }
170 
171  Eigen::Matrix<float,4,1> pt_wrt_color, pt_wrt_depth;
172  pt_wrt_depth[3]=1;
173 
174  int img_idx_x=0, img_idx_y=0; // projected pixel coordinates, in the RGB image plane
175  mrpt::utils::TColor pCol;
176 
177  // For each local point:
178  for (size_t i=0;i<WH;i++)
179  {
180  bool pointWithinImage = false;
181  if (isDirectCorresp)
182  {
183  pointWithinImage=true;
184  img_idx_x++;
185  if (img_idx_x>=imgW) {
186  img_idx_x=0;
187  img_idx_y++;
188  }
189  }
190  else
191  {
192  // Project point, which is now in "pca" in local coordinates wrt the depth camera, into the intensity camera:
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;
195 
196  // Project to image plane:
197  if (pt_wrt_color[2]) {
198  img_idx_x = mrpt::utils::round( cx + fx * pt_wrt_color[0]/pt_wrt_color[2] );
199  img_idx_y = mrpt::utils::round( cy + fy * pt_wrt_color[1]/pt_wrt_color[2] );
200  pointWithinImage=
201  img_idx_x>=0 && img_idx_x<imgW &&
202  img_idx_y>=0 && img_idx_y<imgH;
203  }
204  }
205 
206  if (pointWithinImage)
207  {
208  if (hasColorIntensityImg) {
209  const uint8_t *c= src_obs.intensityImage.get_unsafe(img_idx_x, img_idx_y, 0);
210  pCol.R = c[2];
211  pCol.G = c[1];
212  pCol.B = c[0];
213  }
214  else{
215  uint8_t c= *src_obs.intensityImage.get_unsafe(img_idx_x, img_idx_y, 0);
216  pCol.R = pCol.G = pCol.B = c;
217  }
218  }
219  else
220  {
221  pCol.R = pCol.G = pCol.B = 255;
222  }
223  // Set color:
224  pca.setPointRGBu8(i,pCol.R,pCol.G,pCol.B);
225  } // end for each point
226  } // end if src_obs has intensity image
227 
228  // ...
229 
230  // ------------------------------------------------------------
231  // Stage 3/3: Apply 6D transformations
232  // ------------------------------------------------------------
233  if (takeIntoAccountSensorPoseOnRobot || robotPoseInTheWorld)
234  {
235  mrpt::poses::CPose3D transf_to_apply; // Either ROBOTPOSE or ROBOTPOSE(+)SENSORPOSE or SENSORPOSE
236  if (takeIntoAccountSensorPoseOnRobot)
237  transf_to_apply = src_obs.sensorPose;
238  if (robotPoseInTheWorld)
239  transf_to_apply.composeFrom(*robotPoseInTheWorld, mrpt::poses::CPose3D(transf_to_apply));
240 
241  const mrpt::math::CMatrixFixedNumeric<float,4,4> HM = transf_to_apply.getHomogeneousMatrixVal().cast<float>();
242  Eigen::Matrix<float,4,1> pt, pt_transf;
243  pt[3]=1;
244 
245  for (size_t i=0;i<WH;i++)
246  {
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]);
250  }
251  }
252  } // end of project3DPointsFromDepthImageInto
253 
254  // Auxiliary functions which implement proyection of 3D point clouds:
255  template <class POINTMAP>
256  inline 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)
257  {
258  size_t idx=0;
259  for (int r=0;r<H;r++)
260  for (int c=0;c<W;c++)
261  {
262  const float D = rangeImage.coeff(r,c);
263  pca.setPointXYZ(idx++,
264  D, // x
265  *kys++ * D, // y
266  *kzs++ * D // z
267  );
268  }
269  }
270 
271  // Auxiliary functions which implement proyection of 3D point clouds:
272  template <class POINTMAP>
273  inline 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)
274  {
275  #if MRPT_HAS_SSE2
276  // Use optimized version:
277  const int W_4 = W >> 2; // /=4 , since we process 4 values at a time.
278  size_t idx=0;
279  MRPT_ALIGN16 float xs[4],ys[4],zs[4];
280  for (int r=0;r<H;r++)
281  {
282  const float *D_ptr = &rangeImage.coeffRef(r,0); // Matrices are 16-aligned
283 
284  for (int c=0;c<W_4;c++)
285  {
286  const __m128 D = _mm_load_ps(D_ptr);
287 
288  const __m128 KY = _mm_load_ps(kys);
289  const __m128 KZ = _mm_load_ps(kzs);
290 
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));
294 
295  D_ptr+=4;
296  kys+=4;
297  kzs+=4;
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]);
302  }
303  }
304  #endif
305  }
306 
307 
308 } // End of namespace
309 } // End of namespace
310 } // End of namespace
311 
312 #endif
#define ASSERT_EQUAL_(__A, __B)
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
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.
Definition: CPose3D.h:176
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).
Definition: TCamera.h:158
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArray.h:18
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...
Definition: CPose3D.h:81
A RGB color - 8bit.
Definition: TColor.h:25
bool hasRangeImage
true means the field rangeImage contains valid data
An adapter to different kinds of point cloud object.
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).
Definition: CPose3D.h:72
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.
Definition: round.h:26
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:156
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:173
An adapter to different kinds of point cloud object.
Definition: adapters.h:38
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".
Definition: CMatrix.h:30
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).
Definition: TCamera.h:152
static TCached3DProjTables m_3dproj_lut
3D point cloud projection look-up-table
#define MRPT_ALIGN16
size_t getHeight() const
Returns the height of the image in pixels.
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:154



Page generated by Doxygen 1.8.9.1 for MRPT 1.3.2 SVN:Unversioned directory at Thu Dec 10 00:07:55 UTC 2015