Main MRPT website > C++ reference
MRPT logo
CObservation3DRangeScan_project3D_impl.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CObservation3DRangeScan_project3D_impl_H
29 #define CObservation3DRangeScan_project3D_impl_H
30 
31 namespace mrpt {
32 namespace slam {
33 namespace detail {
34  // Auxiliary functions which implement SSE-optimized proyection of 3D point cloud:
35  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);
36  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);
37 
38  template <class POINTMAP>
39  void project3DPointsFromDepthImageInto(
40  CObservation3DRangeScan & src_obs,
41  POINTMAP & dest_pointcloud,
42  const bool takeIntoAccountSensorPoseOnRobot,
43  const mrpt::poses::CPose3D * robotPoseInTheWorld,
44  const bool PROJ3D_USE_LUT)
45  {
46  using namespace mrpt::math;
47 
48  if (!src_obs.hasRangeImage) return;
49 
50  mrpt::utils::PointCloudAdapter<POINTMAP> pca(dest_pointcloud);
51 
52  // ------------------------------------------------------------
53  // Stage 1/3: Create 3D point cloud local coordinates
54  // ------------------------------------------------------------
55  const int W = src_obs.rangeImage.cols();
56  const int H = src_obs.rangeImage.rows();
57  const size_t WH = W*H;
58 
59  // Reserve memory for 3D points:
60  pca.resize(WH);
61 
62  if (src_obs.range_is_depth)
63  {
64  // range_is_depth = true
65 
66  // Use cached tables?
67  if (PROJ3D_USE_LUT)
68  {
69  // Use LUT:
70  if (src_obs.m_3dproj_lut.prev_camParams!=src_obs.cameraParams || WH!=size_t(src_obs.m_3dproj_lut.Kys.size()))
71  {
72  src_obs.m_3dproj_lut.prev_camParams = src_obs.cameraParams;
73  src_obs.m_3dproj_lut.Kys.resize(WH);
74  src_obs.m_3dproj_lut.Kzs.resize(WH);
75 
76  const float r_cx = src_obs.cameraParams.cx();
77  const float r_cy = src_obs.cameraParams.cy();
78  const float r_fx_inv = 1.0f/src_obs.cameraParams.fx();
79  const float r_fy_inv = 1.0f/src_obs.cameraParams.fy();
80 
81  float *kys = &src_obs.m_3dproj_lut.Kys[0];
82  float *kzs = &src_obs.m_3dproj_lut.Kzs[0];
83  for (int r=0;r<H;r++)
84  for (int c=0;c<W;c++)
85  {
86  *kys++ = (r_cx - c) * r_fx_inv;
87  *kzs++ = (r_cy - r) * r_fy_inv;
88  }
89  } // end update LUT.
90 
91  ASSERT_EQUAL_(WH,size_t(src_obs.m_3dproj_lut.Kys.size()))
92  ASSERT_EQUAL_(WH,size_t(src_obs.m_3dproj_lut.Kzs.size()))
93  float *kys = &src_obs.m_3dproj_lut.Kys[0];
94  float *kzs = &src_obs.m_3dproj_lut.Kzs[0];
95 
96  #if MRPT_HAS_SSE2
97  if ((W & 0x07)==0)
98  do_project_3d_pointcloud_SSE2(H,W,kys,kzs,src_obs.rangeImage,pca);
99  else do_project_3d_pointcloud(H,W,kys,kzs,src_obs.rangeImage,pca); // if image width is not 8*N, use standard method
100  #else
101  do_project_3d_pointcloud(H,W,kys,kzs,src_obs.rangeImage,pca);
102  #endif
103  }
104  else
105  {
106  // Without LUT:
107  const float r_cx = src_obs.cameraParams.cx();
108  const float r_cy = src_obs.cameraParams.cy();
109  const float r_fx_inv = 1.0f/src_obs.cameraParams.fx();
110  const float r_fy_inv = 1.0f/src_obs.cameraParams.fy();
111  size_t idx=0;
112  for (int r=0;r<H;r++)
113  for (int c=0;c<W;c++)
114  {
115  const float Kz = (r_cy - r) * r_fy_inv;
116  const float Ky = (r_cx - c) * r_fx_inv;
117  const float D = src_obs.rangeImage.coeff(r,c);
118  pca.setPointXYZ(idx++,
119  D, // x
120  Ky * D, // y
121  Kz * D // z
122  );
123  }
124  }
125  }
126  else
127  {
128  /* range_is_depth = false :
129  * Ky = (r_cx - c)/r_fx
130  * Kz = (r_cy - r)/r_fy
131  *
132  * x(i) = rangeImage(r,c) / sqrt( 1 + Ky^2 + Kz^2 )
133  * y(i) = Ky * x(i)
134  * z(i) = Kz * x(i)
135  */
136  const float r_cx = src_obs.cameraParams.cx();
137  const float r_cy = src_obs.cameraParams.cy();
138  const float r_fx_inv = 1.0f/src_obs.cameraParams.fx();
139  const float r_fy_inv = 1.0f/src_obs.cameraParams.fy();
140  size_t idx=0;
141  for (int r=0;r<H;r++)
142  for (int c=0;c<W;c++)
143  {
144  const float Ky = (r_cx - c) * r_fx_inv;
145  const float Kz = (r_cy - r) * r_fy_inv;
146  const float D = src_obs.rangeImage.coeff(r,c);
147  pca.setPointXYZ(idx++,
148  D / std::sqrt(1+Ky*Ky+Kz*Kz), // x
149  Ky * D, // y
150  Kz * D // z
151  );
152  }
153  }
154 
155  // -------------------------------------------------------------
156  // Stage 2/3: Project local points into RGB image to get colors
157  // -------------------------------------------------------------
158  if (src_obs.hasIntensityImage)
159  {
160  const int imgW = src_obs.intensityImage.getWidth();
161  const int imgH = src_obs.intensityImage.getHeight();
162  const bool hasColorIntensityImg = src_obs.intensityImage.isColor();
163 
164  const float cx = src_obs.cameraParamsIntensity.cx();
165  const float cy = src_obs.cameraParamsIntensity.cy();
166  const float fx = src_obs.cameraParamsIntensity.fx();
167  const float fy = src_obs.cameraParamsIntensity.fy();
168 
169  // Unless we are in a special case (both depth & RGB images coincide)...
170  const bool isDirectCorresp = src_obs.doDepthAndIntensityCamerasCoincide();
171 
172  // ...precompute the inverse of the pose transformation out of the loop,
173  // store as a 4x4 homogeneous matrix to exploit SSE optimizations below:
174  CMatrixFixedNumeric<float,4,4> T_inv;
175  if (!isDirectCorresp)
176  {
177  CMatrixFixedNumeric<double,3,3> R_inv;
178  CMatrixFixedNumeric<double,3,1> t_inv;
179  mrpt::math::homogeneousMatrixInverse(
180  src_obs.relativePoseIntensityWRTDepth.getRotationMatrix(),src_obs.relativePoseIntensityWRTDepth.m_coords,
181  R_inv,t_inv);
182 
183  T_inv(3,3)=1;
184  T_inv.block<3,3>(0,0)=R_inv.cast<float>();
185  T_inv.block<3,1>(0,3)=t_inv.cast<float>();
186  }
187 
188  Eigen::Matrix<float,4,1> pt_wrt_color, pt_wrt_depth;
189  pt_wrt_depth[3]=1;
190 
191  int img_idx_x=0, img_idx_y=0; // projected pixel coordinates, in the RGB image plane
192  mrpt::utils::TColor pCol;
193 
194  // For each local point:
195  for (size_t i=0;i<WH;i++)
196  {
197  bool pointWithinImage = false;
198  if (isDirectCorresp)
199  {
200  pointWithinImage=true;
201  img_idx_x++;
202  if (img_idx_x>=imgW) {
203  img_idx_x=0;
204  img_idx_y++;
205  }
206  }
207  else
208  {
209  // Project point, which is now in "pca" in local coordinates wrt the depth camera, into the intensity camera:
210  pca.getPointXYZ(i,pt_wrt_depth[0],pt_wrt_depth[1],pt_wrt_depth[2]);
211  pt_wrt_color.noalias() = T_inv*pt_wrt_depth;
212 
213  // Project to image plane:
214  if (pt_wrt_color[2]) {
215  img_idx_x = mrpt::utils::round( cx + fx * pt_wrt_color[0]/pt_wrt_color[2] );
216  img_idx_y = mrpt::utils::round( cy + fy * pt_wrt_color[1]/pt_wrt_color[2] );
217  pointWithinImage=
218  img_idx_x>=0 && img_idx_x<imgW &&
219  img_idx_y>=0 && img_idx_y<imgH;
220  }
221  }
222 
223  if (pointWithinImage)
224  {
225  if (hasColorIntensityImg) {
226  const uint8_t *c= src_obs.intensityImage.get_unsafe(img_idx_x, img_idx_y, 0);
227  pCol.R = c[2];
228  pCol.G = c[1];
229  pCol.B = c[0];
230  }
231  else{
232  uint8_t c= *src_obs.intensityImage.get_unsafe(img_idx_x, img_idx_y, 0);
233  pCol.R = pCol.G = pCol.B = c;
234  }
235  }
236  else
237  {
238  pCol.R = pCol.G = pCol.B = 255;
239  }
240  // Set color:
241  pca.setPointRGBu8(i,pCol.R,pCol.G,pCol.B);
242  } // end for each point
243  } // end if src_obs has intensity image
244 
245  // ...
246 
247  // ------------------------------------------------------------
248  // Stage 3/3: Apply 6D transformations
249  // ------------------------------------------------------------
250  if (takeIntoAccountSensorPoseOnRobot || robotPoseInTheWorld)
251  {
252  mrpt::poses::CPose3D transf_to_apply; // Either ROBOTPOSE or ROBOTPOSE(+)SENSORPOSE or SENSORPOSE
253  if (takeIntoAccountSensorPoseOnRobot)
254  transf_to_apply = src_obs.sensorPose;
255  if (robotPoseInTheWorld)
256  transf_to_apply.composeFrom(*robotPoseInTheWorld, CPose3D(transf_to_apply));
257 
258  const CMatrixFixedNumeric<float,4,4> HM = transf_to_apply.getHomogeneousMatrixVal().cast<float>();
259  Eigen::Matrix<float,4,1> pt, pt_transf;
260  pt[3]=1;
261 
262  for (size_t i=0;i<WH;i++)
263  {
264  pca.getPointXYZ(i,pt[0],pt[1],pt[2]);
265  pt_transf.noalias() = HM*pt;
266  pca.setPointXYZ(i,pt_transf[0],pt_transf[1],pt_transf[2]);
267  }
268  }
269  } // end of project3DPointsFromDepthImageInto
270 
271  // Auxiliary functions which implement proyection of 3D point clouds:
272  template <class POINTMAP>
273  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)
274  {
275  size_t idx=0;
276  for (int r=0;r<H;r++)
277  for (int c=0;c<W;c++)
278  {
279  const float D = rangeImage.coeff(r,c);
280  pca.setPointXYZ(idx++,
281  D, // x
282  *kys++ * D, // y
283  *kzs++ * D // z
284  );
285  }
286  }
287 
288  // Auxiliary functions which implement proyection of 3D point clouds:
289  template <class POINTMAP>
290  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)
291  {
292  #if MRPT_HAS_SSE2
293  // Use optimized version:
294  const int W_4 = W >> 2; // /=4 , since we process 4 values at a time.
295  size_t idx=0;
296  EIGEN_ALIGN16 float xs[4],ys[4],zs[4];
297  for (int r=0;r<H;r++)
298  {
299  const float *D_ptr = &rangeImage.coeffRef(r,0); // Matrices are 16-aligned
300 
301  for (int c=0;c<W_4;c++)
302  {
303  const __m128 D = _mm_load_ps(D_ptr);
304 
305  const __m128 KY = _mm_load_ps(kys);
306  const __m128 KZ = _mm_load_ps(kzs);
307 
308  _mm_storeu_ps(xs , D);
309  _mm_storeu_ps(ys , _mm_mul_ps(KY,D));
310  _mm_storeu_ps(zs , _mm_mul_ps(KZ,D));
311 
312  D_ptr+=4;
313  kys+=4;
314  kzs+=4;
315  pca.setPointXYZ(idx++,xs[0],ys[0],zs[0]);
316  pca.setPointXYZ(idx++,xs[1],ys[1],zs[1]);
317  pca.setPointXYZ(idx++,xs[2],ys[2],zs[2]);
318  pca.setPointXYZ(idx++,xs[3],ys[3],zs[3]);
319  }
320  }
321  #endif
322  }
323 
324 
325 } // End of namespace
326 } // End of namespace
327 } // End of namespace
328 
329 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013