util3d.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/util3d.h>
29 #include <rtabmap/core/util3d_transforms.h>
30 #include <rtabmap/core/util3d_filtering.h>
31 #include <rtabmap/core/util3d_surface.h>
32 #include <rtabmap/core/util2d.h>
33 #include <rtabmap/core/util2d.h>
34 #include <rtabmap/utilite/ULogger.h>
35 #include <rtabmap/utilite/UMath.h>
36 #include <rtabmap/utilite/UConversion.h>
37 #include <rtabmap/utilite/UFile.h>
38 #include <rtabmap/utilite/UTimer.h>
39 #include <pcl/io/pcd_io.h>
40 #include <pcl/io/ply_io.h>
41 #include <pcl/common/transforms.h>
42 #include <pcl/common/common.h>
43 #include <opencv2/imgproc/imgproc.hpp>
44 #include <opencv2/imgproc/types_c.h>
45 
46 namespace rtabmap
47 {
48 
49 namespace util3d
50 {
51 
52 cv::Mat bgrFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder)
53 {
54  cv::Mat frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
55 
56  for(unsigned int h = 0; h < cloud.height; h++)
57  {
58  for(unsigned int w = 0; w < cloud.width; w++)
59  {
60  if(bgrOrder)
61  {
62  frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).b;
63  frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
64  frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).r;
65  }
66  else
67  {
68  frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).r;
69  frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
70  frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).b;
71  }
72  }
73  }
74  return frameBGR;
75 }
76 
77 // return float image in meter
78 cv::Mat depthFromCloud(
79  const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
80  float & fx,
81  float & fy,
82  bool depth16U)
83 {
84  cv::Mat frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
85  fx = 0.0f; // needed to reconstruct the cloud
86  fy = 0.0f; // needed to reconstruct the cloud
87  for(unsigned int h = 0; h < cloud.height; h++)
88  {
89  for(unsigned int w = 0; w < cloud.width; w++)
90  {
91  float depth = cloud.at(h*cloud.width + w).z;
92  if(depth16U)
93  {
94  depth *= 1000.0f;
95  unsigned short depthMM = 0;
96  if(depth <= (float)USHRT_MAX)
97  {
98  depthMM = (unsigned short)depth;
99  }
100  frameDepth.at<unsigned short>(h,w) = depthMM;
101  }
102  else
103  {
104  frameDepth.at<float>(h,w) = depth;
105  }
106 
107  // update constants
108  if(fx == 0.0f &&
109  uIsFinite(cloud.at(h*cloud.width + w).x) &&
110  uIsFinite(depth) &&
111  w != cloud.width/2 &&
112  depth > 0)
113  {
114  fx = cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth);
115  if(depth16U)
116  {
117  fx*=1000.0f;
118  }
119  }
120  if(fy == 0.0f &&
121  uIsFinite(cloud.at(h*cloud.width + w).y) &&
122  uIsFinite(depth) &&
123  h != cloud.height/2 &&
124  depth > 0)
125  {
126  fy = cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth);
127  if(depth16U)
128  {
129  fy*=1000.0f;
130  }
131  }
132  }
133  }
134  return frameDepth;
135 }
136 
137 // return (unsigned short 16bits image in mm) (float 32bits image in m)
138 void rgbdFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
139  cv::Mat & frameBGR,
140  cv::Mat & frameDepth,
141  float & fx,
142  float & fy,
143  bool bgrOrder,
144  bool depth16U)
145 {
146  frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
147  frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
148 
149  fx = 0.0f; // needed to reconstruct the cloud
150  fy = 0.0f; // needed to reconstruct the cloud
151  for(unsigned int h = 0; h < cloud.height; h++)
152  {
153  for(unsigned int w = 0; w < cloud.width; w++)
154  {
155  //rgb
156  if(bgrOrder)
157  {
158  frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).b;
159  frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
160  frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).r;
161  }
162  else
163  {
164  frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).r;
165  frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
166  frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).b;
167  }
168 
169  //depth
170  float depth = cloud.at(h*cloud.width + w).z;
171  if(depth16U)
172  {
173  depth *= 1000.0f;
174  unsigned short depthMM = 0;
175  if(depth <= (float)USHRT_MAX)
176  {
177  depthMM = (unsigned short)depth;
178  }
179  frameDepth.at<unsigned short>(h,w) = depthMM;
180  }
181  else
182  {
183  frameDepth.at<float>(h,w) = depth;
184  }
185 
186  // update constants
187  if(fx == 0.0f &&
188  uIsFinite(cloud.at(h*cloud.width + w).x) &&
189  uIsFinite(depth) &&
190  w != cloud.width/2 &&
191  depth > 0)
192  {
193  fx = 1.0f/(cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth));
194  if(depth16U)
195  {
196  fx/=1000.0f;
197  }
198  }
199  if(fy == 0.0f &&
200  uIsFinite(cloud.at(h*cloud.width + w).y) &&
201  uIsFinite(depth) &&
202  h != cloud.height/2 &&
203  depth > 0)
204  {
205  fy = 1.0f/(cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth));
206  if(depth16U)
207  {
208  fy/=1000.0f;
209  }
210  }
211  }
212  }
213 }
214 
215 pcl::PointXYZ projectDepthTo3D(
216  const cv::Mat & depthImage,
217  float x, float y,
218  float cx, float cy,
219  float fx, float fy,
220  bool smoothing,
221  float depthErrorRatio)
222 {
223  UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
224 
225  pcl::PointXYZ pt;
226 
227  float depth = util2d::getDepth(depthImage, x, y, smoothing, depthErrorRatio);
228  if(depth > 0.0f)
229  {
230  // Use correct principal point from calibration
231  cx = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
232  cy = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5)
233 
234  // Fill in XYZ
235  pt.x = (x - cx) * depth / fx;
236  pt.y = (y - cy) * depth / fy;
237  pt.z = depth;
238  }
239  else
240  {
241  pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
242  }
243  return pt;
244 }
245 
246 Eigen::Vector3f projectDepthTo3DRay(
247  const cv::Size & imageSize,
248  float x, float y,
249  float cx, float cy,
250  float fx, float fy)
251 {
252  Eigen::Vector3f ray;
253 
254  // Use correct principal point from calibration
255  cx = cx > 0.0f ? cx : float(imageSize.width/2) - 0.5f; //cameraInfo.K.at(2)
256  cy = cy > 0.0f ? cy : float(imageSize.height/2) - 0.5f; //cameraInfo.K.at(5)
257 
258  // Fill in XYZ
259  ray[0] = (x - cx) / fx;
260  ray[1] = (y - cy) / fy;
261  ray[2] = 1.0f;
262 
263  return ray;
264 }
265 
266 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
267  const cv::Mat & imageDepth,
268  float cx, float cy,
269  float fx, float fy,
270  int decimation,
271  float maxDepth,
272  float minDepth,
273  std::vector<int> * validIndices)
274 {
275  CameraModel model(fx, fy, cx, cy);
276  return cloudFromDepth(imageDepth, model, decimation, maxDepth, minDepth, validIndices);
277 }
278 
279 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
280  const cv::Mat & imageDepthIn,
281  const CameraModel & model,
282  int decimation,
283  float maxDepth,
284  float minDepth,
285  std::vector<int> * validIndices)
286 {
287  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
288  if(decimation == 0)
289  {
290  decimation = 1;
291  }
292  float rgbToDepthFactorX = 1.0f;
293  float rgbToDepthFactorY = 1.0f;
294 
295  UASSERT(model.isValidForProjection());
296  UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
297 
298  cv::Mat imageDepth = imageDepthIn;
299  if(model.imageHeight()>0 && model.imageWidth()>0)
300  {
301  UASSERT(model.imageHeight() % imageDepthIn.rows == 0 && model.imageWidth() % imageDepthIn.cols == 0);
302 
303  if(decimation < 0)
304  {
305  UDEBUG("Decimation from model (%d)", decimation);
306  if(model.imageHeight() % decimation != 0)
307  {
308  UERROR("Decimation is not valid for current image size (model.imageHeight()=%d decimation=%d). The cloud is not created.", model.imageHeight(), decimation);
309  return cloud;
310  }
311  if(model.imageWidth() % decimation != 0)
312  {
313  UERROR("Decimation is not valid for current image size (model.imageWidth()=%d decimation=%d). The cloud is not created.", model.imageWidth(), decimation);
314  return cloud;
315  }
316 
317  // decimate from RGB image size, upsample depth if needed
318  decimation = -1*decimation;
319 
320  int targetSize = model.imageHeight() / decimation;
321  if(targetSize > imageDepthIn.rows)
322  {
323  UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
324  imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows);
325  decimation = 1;
326  }
327  else if(targetSize == imageDepthIn.rows)
328  {
329  decimation = 1;
330  }
331  else
332  {
333  UASSERT(imageDepthIn.rows % targetSize == 0);
334  decimation = imageDepthIn.rows / targetSize;
335  }
336  }
337  else
338  {
339  if(imageDepthIn.rows % decimation != 0)
340  {
341  UERROR("Decimation is not valid for current image size (imageDepth.rows=%d decimation=%d). The cloud is not created.", imageDepthIn.rows, decimation);
342  return cloud;
343  }
344  if(imageDepthIn.cols % decimation != 0)
345  {
346  UERROR("Decimation is not valid for current image size (imageDepth.cols=%d decimation=%d). The cloud is not created.", imageDepthIn.cols, decimation);
347  return cloud;
348  }
349  }
350 
351  rgbToDepthFactorX = 1.0f/float((model.imageWidth() / imageDepth.cols));
352  rgbToDepthFactorY = 1.0f/float((model.imageHeight() / imageDepth.rows));
353  }
354  else
355  {
356  decimation = abs(decimation);
357  UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
358  UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("cols=%d decimation=%d", imageDepth.cols, decimation).c_str());
359  }
360 
361  //cloud.header = cameraInfo.header;
362  cloud->height = imageDepth.rows/decimation;
363  cloud->width = imageDepth.cols/decimation;
364  cloud->is_dense = false;
365  cloud->resize(cloud->height * cloud->width);
366  if(validIndices)
367  {
368  validIndices->resize(cloud->size());
369  }
370 
371  float depthFx = model.fx() * rgbToDepthFactorX;
372  float depthFy = model.fy() * rgbToDepthFactorY;
373  float depthCx = model.cx() * rgbToDepthFactorX;
374  float depthCy = model.cy() * rgbToDepthFactorY;
375 
376  UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
377  imageDepth.cols, imageDepth.rows,
378  model.fx(), model.fy(), model.cx(), model.cy(),
379  rgbToDepthFactorX,
380  rgbToDepthFactorY,
381  decimation);
382 
383  int oi = 0;
384  for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
385  {
386  for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
387  {
388  pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
389 
390  pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
391  if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
392  {
393  pt.x = ptXYZ.x;
394  pt.y = ptXYZ.y;
395  pt.z = ptXYZ.z;
396  if(validIndices)
397  {
398  validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
399  }
400  }
401  else
402  {
403  pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
404  }
405  }
406  }
407 
408  if(validIndices)
409  {
410  validIndices->resize(oi);
411  }
412 
413  return cloud;
414 }
415 
416 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
417  const cv::Mat & imageRgb,
418  const cv::Mat & imageDepth,
419  float cx, float cy,
420  float fx, float fy,
421  int decimation,
422  float maxDepth,
423  float minDepth,
424  std::vector<int> * validIndices)
425 {
426  CameraModel model(fx, fy, cx, cy);
427  return cloudFromDepthRGB(imageRgb, imageDepth, model, decimation, maxDepth, minDepth, validIndices);
428 }
429 
430 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
431  const cv::Mat & imageRgb,
432  const cv::Mat & imageDepthIn,
433  const CameraModel & model,
434  int decimation,
435  float maxDepth,
436  float minDepth,
437  std::vector<int> * validIndices)
438 {
439  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
440  if(decimation == 0)
441  {
442  decimation = 1;
443  }
444  UDEBUG("");
445  UASSERT(model.isValidForProjection());
446  UASSERT_MSG((model.imageHeight() == 0 && model.imageWidth() == 0) ||
447  (model.imageHeight() == imageRgb.rows && model.imageWidth() == imageRgb.cols),
448  uFormat("model=%dx%d rgb=%dx%d", model.imageWidth(), model.imageHeight(), imageRgb.cols, imageRgb.rows).c_str());
449  //UASSERT_MSG(imageRgb.rows % imageDepthIn.rows == 0 && imageRgb.cols % imageDepthIn.cols == 0,
450  // uFormat("rgb=%dx%d depth=%dx%d", imageRgb.cols, imageRgb.rows, imageDepthIn.cols, imageDepthIn.rows).c_str());
451  UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
452  if(decimation < 0)
453  {
454  if(imageRgb.rows % decimation != 0 || imageRgb.cols % decimation != 0)
455  {
456  int oldDecimation = decimation;
457  while(decimation <= -1)
458  {
459  if(imageRgb.rows % decimation == 0 && imageRgb.cols % decimation == 0)
460  {
461  break;
462  }
463  ++decimation;
464  }
465 
466  if(imageRgb.rows % oldDecimation != 0 || imageRgb.cols % oldDecimation != 0)
467  {
468  UWARN("Decimation (%d) is not valid for current image size (rgb=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageRgb.cols, imageRgb.rows, decimation);
469  }
470  }
471  }
472  else
473  {
474  if(imageDepthIn.rows % decimation != 0 || imageDepthIn.cols % decimation != 0)
475  {
476  int oldDecimation = decimation;
477  while(decimation >= 1)
478  {
479  if(imageDepthIn.rows % decimation == 0 && imageDepthIn.cols % decimation == 0)
480  {
481  break;
482  }
483  --decimation;
484  }
485 
486  if(imageDepthIn.rows % oldDecimation != 0 || imageDepthIn.cols % oldDecimation != 0)
487  {
488  UWARN("Decimation (%d) is not valid for current image size (depth=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageDepthIn.cols, imageDepthIn.rows, decimation);
489  }
490  }
491  }
492 
493  cv::Mat imageDepth = imageDepthIn;
494  if(decimation < 0)
495  {
496  UDEBUG("Decimation from RGB image (%d)", decimation);
497  // decimate from RGB image size, upsample depth if needed
498  decimation = -1*decimation;
499 
500  int targetSize = imageRgb.rows / decimation;
501  if(targetSize > imageDepthIn.rows)
502  {
503  UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
504  imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows);
505  decimation = 1;
506  }
507  else if(targetSize == imageDepthIn.rows)
508  {
509  decimation = 1;
510  }
511  else
512  {
513  UASSERT(imageDepthIn.rows % targetSize == 0);
514  decimation = imageDepthIn.rows / targetSize;
515  }
516  }
517 
518  bool mono;
519  if(imageRgb.channels() == 3) // BGR
520  {
521  mono = false;
522  }
523  else if(imageRgb.channels() == 1) // Mono
524  {
525  mono = true;
526  }
527  else
528  {
529  return cloud;
530  }
531 
532  //cloud.header = cameraInfo.header;
533  cloud->height = imageDepth.rows/decimation;
534  cloud->width = imageDepth.cols/decimation;
535  cloud->is_dense = false;
536  cloud->resize(cloud->height * cloud->width);
537  if(validIndices)
538  {
539  validIndices->resize(cloud->size());
540  }
541 
542  float rgbToDepthFactorX = float(imageRgb.cols) / float(imageDepth.cols);
543  float rgbToDepthFactorY = float(imageRgb.rows) / float(imageDepth.rows);
544  float depthFx = model.fx() / rgbToDepthFactorX;
545  float depthFy = model.fy() / rgbToDepthFactorY;
546  float depthCx = model.cx() / rgbToDepthFactorX;
547  float depthCy = model.cy() / rgbToDepthFactorY;
548 
549  UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
550  imageRgb.cols, imageRgb.rows,
551  imageDepth.cols, imageDepth.rows,
552  model.fx(), model.fy(), model.cx(), model.cy(),
553  rgbToDepthFactorX,
554  rgbToDepthFactorY,
555  decimation);
556 
557  int oi = 0;
558  for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
559  {
560  for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
561  {
562  pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
563 
564  int x = int(w*rgbToDepthFactorX);
565  int y = int(h*rgbToDepthFactorY);
566  UASSERT(x >=0 && x<imageRgb.cols && y >=0 && y<imageRgb.rows);
567  if(!mono)
568  {
569  const unsigned char * bgr = imageRgb.ptr<unsigned char>(y,x);
570  pt.b = bgr[0];
571  pt.g = bgr[1];
572  pt.r = bgr[2];
573  }
574  else
575  {
576  unsigned char v = imageRgb.at<unsigned char>(y,x);
577  pt.b = v;
578  pt.g = v;
579  pt.r = v;
580  }
581 
582  pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
583  if (pcl::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
584  {
585  pt.x = ptXYZ.x;
586  pt.y = ptXYZ.y;
587  pt.z = ptXYZ.z;
588  if (validIndices)
589  {
590  validIndices->at(oi) = (h / decimation)*cloud->width + (w / decimation);
591  }
592  ++oi;
593  }
594  else
595  {
596  pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
597  }
598  }
599  }
600  if(validIndices)
601  {
602  validIndices->resize(oi);
603  }
604  if(oi == 0)
605  {
606  UWARN("Cloud with only NaN values created!");
607  }
608  UDEBUG("");
609  return cloud;
610 }
611 
612 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
613  const cv::Mat & imageDisparity,
614  const StereoCameraModel & model,
615  int decimation,
616  float maxDepth,
617  float minDepth,
618  std::vector<int> * validIndices)
619 {
620  UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
621  UASSERT(decimation >= 1);
622 
623  if(imageDisparity.rows % decimation != 0 || imageDisparity.cols % decimation != 0)
624  {
625  int oldDecimation = decimation;
626  while(decimation >= 1)
627  {
628  if(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0)
629  {
630  break;
631  }
632  --decimation;
633  }
634 
635  if(imageDisparity.rows % oldDecimation != 0 || imageDisparity.cols % oldDecimation != 0)
636  {
637  UWARN("Decimation (%d) is not valid for current image size (depth=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageDisparity.cols, imageDisparity.rows, decimation);
638  }
639  }
640 
641  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
642 
643  //cloud.header = cameraInfo.header;
644  cloud->height = imageDisparity.rows/decimation;
645  cloud->width = imageDisparity.cols/decimation;
646  cloud->is_dense = false;
647  cloud->resize(cloud->height * cloud->width);
648  if(validIndices)
649  {
650  validIndices->resize(cloud->size());
651  }
652 
653  int oi = 0;
654  if(imageDisparity.type()==CV_16SC1)
655  {
656  for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
657  {
658  for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
659  {
660  float disp = float(imageDisparity.at<short>(h,w))/16.0f;
661  cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
662  if(pt.z >= minDepth && (maxDepth <= 0.0f || pt.z <= maxDepth))
663  {
664  cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
665  if(validIndices)
666  {
667  validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
668  }
669  }
670  else
671  {
672  cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(
673  std::numeric_limits<float>::quiet_NaN(),
674  std::numeric_limits<float>::quiet_NaN(),
675  std::numeric_limits<float>::quiet_NaN());
676  }
677  }
678  }
679  }
680  else
681  {
682  for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
683  {
684  for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
685  {
686  float disp = imageDisparity.at<float>(h,w);
687  cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
688  if(pt.z > minDepth && (maxDepth <= 0.0f || pt.z <= maxDepth))
689  {
690  cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
691  if(validIndices)
692  {
693  validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
694  }
695  }
696  else
697  {
698  cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(
699  std::numeric_limits<float>::quiet_NaN(),
700  std::numeric_limits<float>::quiet_NaN(),
701  std::numeric_limits<float>::quiet_NaN());
702  }
703  }
704  }
705  }
706  if(validIndices)
707  {
708  validIndices->resize(oi);
709  }
710  return cloud;
711 }
712 
713 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
714  const cv::Mat & imageRgb,
715  const cv::Mat & imageDisparity,
716  const StereoCameraModel & model,
717  int decimation,
718  float maxDepth,
719  float minDepth,
720  std::vector<int> * validIndices)
721 {
722  UASSERT(!imageRgb.empty() && !imageDisparity.empty());
723  UASSERT(imageRgb.rows == imageDisparity.rows &&
724  imageRgb.cols == imageDisparity.cols &&
725  (imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1));
726  UASSERT(imageRgb.channels() == 3 || imageRgb.channels() == 1);
727  UASSERT(decimation >= 1);
728 
729  if(imageDisparity.rows % decimation != 0 || imageDisparity.cols % decimation != 0)
730  {
731  int oldDecimation = decimation;
732  while(decimation >= 1)
733  {
734  if(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0)
735  {
736  break;
737  }
738  --decimation;
739  }
740 
741  if(imageDisparity.rows % oldDecimation != 0 || imageDisparity.cols % oldDecimation != 0)
742  {
743  UWARN("Decimation (%d) is not valid for current image size (depth=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageDisparity.cols, imageDisparity.rows, decimation);
744  }
745  }
746 
747  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
748 
749  bool mono;
750  if(imageRgb.channels() == 3) // BGR
751  {
752  mono = false;
753  }
754  else // Mono
755  {
756  mono = true;
757  }
758 
759  //cloud.header = cameraInfo.header;
760  cloud->height = imageRgb.rows/decimation;
761  cloud->width = imageRgb.cols/decimation;
762  cloud->is_dense = false;
763  cloud->resize(cloud->height * cloud->width);
764  if(validIndices)
765  {
766  validIndices->resize(cloud->size());
767  }
768 
769  int oi=0;
770  for(int h = 0; h < imageRgb.rows && h/decimation < (int)cloud->height; h+=decimation)
771  {
772  for(int w = 0; w < imageRgb.cols && w/decimation < (int)cloud->width; w+=decimation)
773  {
774  pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
775  if(!mono)
776  {
777  pt.b = imageRgb.at<cv::Vec3b>(h,w)[0];
778  pt.g = imageRgb.at<cv::Vec3b>(h,w)[1];
779  pt.r = imageRgb.at<cv::Vec3b>(h,w)[2];
780  }
781  else
782  {
783  unsigned char v = imageRgb.at<unsigned char>(h,w);
784  pt.b = v;
785  pt.g = v;
786  pt.r = v;
787  }
788 
789  float disp = imageDisparity.type()==CV_16SC1?float(imageDisparity.at<short>(h,w))/16.0f:imageDisparity.at<float>(h,w);
790  cv::Point3f ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
791  if(util3d::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
792  {
793  pt.x = ptXYZ.x;
794  pt.y = ptXYZ.y;
795  pt.z = ptXYZ.z;
796  if(validIndices)
797  {
798  validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
799  }
800  }
801  else
802  {
803  pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
804  }
805  }
806  }
807  if(validIndices)
808  {
809  validIndices->resize(oi);
810  }
811  return cloud;
812 }
813 
814 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
815  const cv::Mat & imageLeft,
816  const cv::Mat & imageRight,
817  const StereoCameraModel & model,
818  int decimation,
819  float maxDepth,
820  float minDepth,
821  std::vector<int> * validIndices,
822  const ParametersMap & parameters)
823 {
824  UASSERT(!imageLeft.empty() && !imageRight.empty());
825  UASSERT(imageRight.type() == CV_8UC1);
826  UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1);
827  UASSERT(imageLeft.rows == imageRight.rows &&
828  imageLeft.cols == imageRight.cols);
829  UASSERT(decimation >= 1.0f);
830 
831  cv::Mat leftColor = imageLeft;
832  cv::Mat rightMono = imageRight;
833 
834  cv::Mat leftMono;
835  if(leftColor.channels() == 3)
836  {
837  cv::cvtColor(leftColor, leftMono, CV_BGR2GRAY);
838  }
839  else
840  {
841  leftMono = leftColor;
842  }
843 
844  return cloudFromDisparityRGB(
845  leftColor,
846  util2d::disparityFromStereoImages(leftMono, rightMono, parameters),
847  model,
848  decimation,
849  maxDepth,
850  minDepth,
851  validIndices);
852 }
853 
854 std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
855  const SensorData & sensorData,
856  int decimation,
857  float maxDepth,
858  float minDepth,
859  std::vector<pcl::IndicesPtr> * validIndices,
860  const ParametersMap & stereoParameters,
861  const std::vector<float> & roiRatios)
862 {
863  if(decimation == 0)
864  {
865  decimation = 1;
866  }
867 
868  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
869 
870  if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
871  {
872  //depth
873  UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
874  int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
875  for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
876  {
877  clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
878  if(validIndices)
879  {
880  validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
881  }
882  if(sensorData.cameraModels()[i].isValidForProjection())
883  {
884  cv::Mat depth = cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows));
885  CameraModel model = sensorData.cameraModels()[i];
886  if( roiRatios.size() == 4 &&
887  (roiRatios[0] > 0.0f ||
888  roiRatios[1] > 0.0f ||
889  roiRatios[2] > 0.0f ||
890  roiRatios[3] > 0.0f))
891  {
892  cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios);
893  cv::Rect roiRgb;
894  if(model.imageWidth() && model.imageHeight())
895  {
896  roiRgb = util2d::computeRoi(model.imageSize(), roiRatios);
897  }
898  if( roiDepth.width%decimation==0 &&
899  roiDepth.height%decimation==0 &&
900  (roiRgb.width != 0 ||
901  (roiRgb.width%decimation==0 &&
902  roiRgb.height%decimation==0)))
903  {
904  depth = cv::Mat(depth, roiDepth);
905  if(model.imageWidth() != 0 && model.imageHeight() != 0)
906  {
907  model = model.roi(util2d::computeRoi(model.imageSize(), roiRatios));
908  }
909  else
910  {
911  model = model.roi(roiDepth);
912  }
913  }
914  else
915  {
916  UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
917  "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
918  "by decimation parameter (%d). Ignoring ROI ratios...",
919  roiRatios[0],
920  roiRatios[1],
921  roiRatios[2],
922  roiRatios[3],
923  roiDepth.width,
924  roiDepth.height,
925  roiRgb.width,
926  roiRgb.height,
927  decimation);
928  }
929  }
930 
931  pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
932  depth,
933  model,
934  decimation,
935  maxDepth,
936  minDepth,
937  validIndices?validIndices->back().get():0);
938 
939  if(tmp->size())
940  {
941  if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
942  {
943  tmp = util3d::transformPointCloud(tmp, model.localTransform());
944  }
945  clouds.back() = tmp;
946  }
947  }
948  else
949  {
950  UERROR("Camera model %d is invalid", i);
951  }
952  }
953  }
954  else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && !sensorData.stereoCameraModels().empty())
955  {
956  //stereo
957  UASSERT(sensorData.rightRaw().type() == CV_8UC1);
958 
959  cv::Mat leftMono;
960  if(sensorData.imageRaw().channels() == 3)
961  {
962  cv::cvtColor(sensorData.imageRaw(), leftMono, CV_BGR2GRAY);
963  }
964  else
965  {
966  leftMono = sensorData.imageRaw();
967  }
968 
969  UASSERT(int((sensorData.imageRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.imageRaw().cols);
970  UASSERT(int((sensorData.rightRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.rightRaw().cols);
971  int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
972  for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
973  {
974  clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
975  if(validIndices)
976  {
977  validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
978  }
979  if(sensorData.stereoCameraModels()[i].isValidForProjection())
980  {
981  cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows));
982  cv::Mat right(sensorData.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.rightRaw().rows));
983  StereoCameraModel model = sensorData.stereoCameraModels()[i];
984  if( roiRatios.size() == 4 &&
985  ((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
986  (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
987  (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
988  (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
989  {
990  cv::Rect roi = util2d::computeRoi(left, roiRatios);
991  if( roi.width%decimation==0 &&
992  roi.height%decimation==0)
993  {
994  left = cv::Mat(left, roi);
995  right = cv::Mat(right, roi);
996  model.roi(roi);
997  }
998  else
999  {
1000  UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
1001  "dimension (left=%dx%d) cannot be divided exactly "
1002  "by decimation parameter (%d). Ignoring ROI ratios...",
1003  roiRatios[0],
1004  roiRatios[1],
1005  roiRatios[2],
1006  roiRatios[3],
1007  roi.width,
1008  roi.height,
1009  decimation);
1010  }
1011  }
1012 
1013  pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = cloudFromDisparity(
1014  util2d::disparityFromStereoImages(left, right, stereoParameters),
1015  model,
1016  decimation,
1017  maxDepth,
1018  minDepth,
1019  validIndices?validIndices->back().get():0);
1020 
1021  if(tmp->size())
1022  {
1023  if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
1024  {
1025  tmp = util3d::transformPointCloud(tmp, model.localTransform());
1026  }
1027  clouds.back() = tmp;
1028  }
1029  }
1030  else
1031  {
1032  UERROR("Stereo camera model %d is invalid", i);
1033  }
1034  }
1035  }
1036 
1037  return clouds;
1038 }
1039 
1040 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
1041  const SensorData & sensorData,
1042  int decimation,
1043  float maxDepth,
1044  float minDepth,
1045  std::vector<int> * validIndices,
1046  const ParametersMap & stereoParameters,
1047  const std::vector<float> & roiRatios)
1048 {
1049  std::vector<pcl::IndicesPtr> validIndicesV;
1050  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds = cloudsFromSensorData(
1051  sensorData,
1052  decimation,
1053  maxDepth,
1054  minDepth,
1055  validIndices?&validIndicesV:0,
1056  stereoParameters,
1057  roiRatios);
1058 
1059  if(validIndices)
1060  {
1061  UASSERT(validIndicesV.size() == clouds.size());
1062  }
1063 
1064  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
1065 
1066  if(clouds.size() == 1)
1067  {
1068  cloud = clouds[0];
1069  if(validIndices)
1070  {
1071  *validIndices = *validIndicesV[0];
1072  }
1073  }
1074  else
1075  {
1076  for(size_t i=0; i<clouds.size(); ++i)
1077  {
1078  *cloud += *util3d::removeNaNFromPointCloud(clouds[i]);
1079  }
1080  if(validIndices)
1081  {
1082  //generate indices for all points (they are all valid)
1083  validIndices->resize(cloud->size());
1084  for(size_t i=0; i<cloud->size(); ++i)
1085  {
1086  validIndices->at(i) = i;
1087  }
1088  }
1089  }
1090  return cloud;
1091 }
1092 
1093 std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
1094  const SensorData & sensorData,
1095  int decimation,
1096  float maxDepth,
1097  float minDepth,
1098  std::vector<pcl::IndicesPtr> * validIndices,
1099  const ParametersMap & stereoParameters,
1100  const std::vector<float> & roiRatios)
1101 {
1102  if(decimation == 0)
1103  {
1104  decimation = 1;
1105  }
1106 
1107  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
1108 
1109  if(!sensorData.imageRaw().empty() && !sensorData.depthRaw().empty() && sensorData.cameraModels().size())
1110  {
1111  //depth
1112  UDEBUG("");
1113  UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
1114  UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
1115  //UASSERT_MSG(sensorData.imageRaw().cols % sensorData.depthRaw().cols == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().cols, sensorData.depthRaw().cols).c_str());
1116  //UASSERT_MSG(sensorData.imageRaw().rows % sensorData.depthRaw().rows == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().rows, sensorData.depthRaw().rows).c_str());
1117  int subRGBWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
1118  int subDepthWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
1119 
1120  for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
1121  {
1122  clouds.push_back(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>));
1123  if(validIndices)
1124  {
1125  validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
1126  }
1127  if(sensorData.cameraModels()[i].isValidForProjection())
1128  {
1129  cv::Mat rgb(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows));
1130  cv::Mat depth(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows));
1131  CameraModel model = sensorData.cameraModels()[i];
1132  if( roiRatios.size() == 4 &&
1133  ((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
1134  (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
1135  (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
1136  (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
1137  {
1138  cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios);
1139  cv::Rect roiRgb = util2d::computeRoi(rgb, roiRatios);
1140  if( roiDepth.width%decimation==0 &&
1141  roiDepth.height%decimation==0 &&
1142  roiRgb.width%decimation==0 &&
1143  roiRgb.height%decimation==0)
1144  {
1145  depth = cv::Mat(depth, roiDepth);
1146  rgb = cv::Mat(rgb, roiRgb);
1147  model = model.roi(roiRgb);
1148  }
1149  else
1150  {
1151  UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
1152  "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
1153  "by decimation parameter (%d). Ignoring ROI ratios...",
1154  roiRatios[0],
1155  roiRatios[1],
1156  roiRatios[2],
1157  roiRatios[3],
1158  roiDepth.width,
1159  roiDepth.height,
1160  roiRgb.width,
1161  roiRgb.height,
1162  decimation);
1163  }
1164  }
1165 
1166  pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
1167  rgb,
1168  depth,
1169  model,
1170  decimation,
1171  maxDepth,
1172  minDepth,
1173  validIndices?validIndices->back().get():0);
1174 
1175  if(tmp->size())
1176  {
1177  if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
1178  {
1179  tmp = util3d::transformPointCloud(tmp, model.localTransform());
1180  }
1181  clouds.back() = tmp;
1182  }
1183  }
1184  else
1185  {
1186  UERROR("Camera model %d is invalid", i);
1187  }
1188  }
1189  }
1190  else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && !sensorData.stereoCameraModels().empty())
1191  {
1192  //stereo
1193  UDEBUG("");
1194 
1195  UASSERT(int((sensorData.imageRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.imageRaw().cols);
1196  UASSERT(int((sensorData.rightRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.rightRaw().cols);
1197  int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
1198  for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
1199  {
1200  clouds.push_back(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>));
1201  if(validIndices)
1202  {
1203  validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
1204  }
1205  if(sensorData.stereoCameraModels()[i].isValidForProjection())
1206  {
1207  cv::Mat left(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows));
1208  cv::Mat right(sensorData.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.rightRaw().rows));
1209  StereoCameraModel model = sensorData.stereoCameraModels()[i];
1210  if( roiRatios.size() == 4 &&
1211  ((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
1212  (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
1213  (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
1214  (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
1215  {
1216  cv::Rect roi = util2d::computeRoi(left, roiRatios);
1217  if( roi.width%decimation==0 &&
1218  roi.height%decimation==0)
1219  {
1220  left = cv::Mat(left, roi);
1221  right = cv::Mat(right, roi);
1222  model.roi(roi);
1223  }
1224  else
1225  {
1226  UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
1227  "dimension (left=%dx%d) cannot be divided exactly "
1228  "by decimation parameter (%d). Ignoring ROI ratios...",
1229  roiRatios[0],
1230  roiRatios[1],
1231  roiRatios[2],
1232  roiRatios[3],
1233  roi.width,
1234  roi.height,
1235  decimation);
1236  }
1237  }
1238 
1239  pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = cloudFromStereoImages(
1240  left,
1241  right,
1242  model,
1243  decimation,
1244  maxDepth,
1245  minDepth,
1246  validIndices?validIndices->back().get():0,
1247  stereoParameters);
1248 
1249  if(tmp->size())
1250  {
1251  if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
1252  {
1253  tmp = util3d::transformPointCloud(tmp, model.localTransform());
1254  }
1255  clouds.back() = tmp;
1256  }
1257  }
1258  else
1259  {
1260  UERROR("Stereo camera model %d is invalid", i);
1261  }
1262  }
1263  }
1264 
1265  return clouds;
1266 }
1267 
1268 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
1269  const SensorData & sensorData,
1270  int decimation,
1271  float maxDepth,
1272  float minDepth,
1273  std::vector<int> * validIndices,
1274  const ParametersMap & stereoParameters,
1275  const std::vector<float> & roiRatios)
1276 {
1277  std::vector<pcl::IndicesPtr> validIndicesV;
1278  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds = cloudsRGBFromSensorData(
1279  sensorData,
1280  decimation,
1281  maxDepth,
1282  minDepth,
1283  validIndices?&validIndicesV:0,
1284  stereoParameters,
1285  roiRatios);
1286 
1287  if(validIndices)
1288  {
1289  UASSERT(validIndicesV.size() == clouds.size());
1290  }
1291 
1292  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
1293 
1294  if(clouds.size() == 1)
1295  {
1296  cloud = clouds[0];
1297  if(validIndices)
1298  {
1299  *validIndices = *validIndicesV[0];
1300  }
1301  }
1302  else
1303  {
1304  for(size_t i=0; i<clouds.size(); ++i)
1305  {
1306  *cloud += *util3d::removeNaNFromPointCloud(clouds[i]);
1307  }
1308  if(validIndices)
1309  {
1310  //generate indices for all points (they are all valid)
1311  validIndices->resize(cloud->size());
1312  for(size_t i=0; i<cloud->size(); ++i)
1313  {
1314  validIndices->at(i) = i;
1315  }
1316  }
1317  }
1318  return cloud;
1319 }
1320 
1321 pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
1322  const cv::Mat & depthImage,
1323  float fx,
1324  float fy,
1325  float cx,
1326  float cy,
1327  float maxDepth,
1328  float minDepth,
1329  const Transform & localTransform)
1330 {
1331  UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
1332  UASSERT(!localTransform.isNull());
1333 
1334  pcl::PointCloud<pcl::PointXYZ> scan;
1335  int middle = depthImage.rows/2;
1336  if(middle)
1337  {
1338  scan.resize(depthImage.cols);
1339  int oi = 0;
1340  for(int i=depthImage.cols-1; i>=0; --i)
1341  {
1342  pcl::PointXYZ pt = util3d::projectDepthTo3D(depthImage, i, middle, cx, cy, fx, fy, false);
1343  if(pcl::isFinite(pt) && pt.z >= minDepth && (maxDepth == 0 || pt.z < maxDepth))
1344  {
1345  if(!localTransform.isIdentity())
1346  {
1347  pt = util3d::transformPoint(pt, localTransform);
1348  }
1349  scan[oi++] = pt;
1350  }
1351  }
1352  scan.resize(oi);
1353  }
1354  return scan;
1355 }
1356 
1357 pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImages(
1358  const cv::Mat & depthImages,
1359  const std::vector<CameraModel> & cameraModels,
1360  float maxDepth,
1361  float minDepth)
1362 {
1363  pcl::PointCloud<pcl::PointXYZ> scan;
1364  UASSERT(int((depthImages.cols/cameraModels.size())*cameraModels.size()) == depthImages.cols);
1365  int subImageWidth = depthImages.cols/cameraModels.size();
1366  for(int i=(int)cameraModels.size()-1; i>=0; --i)
1367  {
1368  if(cameraModels[i].isValidForProjection())
1369  {
1370  cv::Mat depth = cv::Mat(depthImages, cv::Rect(subImageWidth*i, 0, subImageWidth, depthImages.rows));
1371 
1372  scan += laserScanFromDepthImage(
1373  depth,
1374  cameraModels[i].fx(),
1375  cameraModels[i].fy(),
1376  cameraModels[i].cx(),
1377  cameraModels[i].cy(),
1378  maxDepth,
1379  minDepth,
1380  cameraModels[i].localTransform());
1381  }
1382  else
1383  {
1384  UERROR("Camera model %d is invalid", i);
1385  }
1386  }
1387  return scan;
1388 }
1389 
1390 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
1391 {
1392  return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
1393 }
1394 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
1395 {
1396  cv::Mat laserScan;
1397  bool nullTransform = transform.isNull() || transform.isIdentity();
1398  Eigen::Affine3f transform3f = transform.toEigen3f();
1399  int oi = 0;
1400  if(indices.get())
1401  {
1402  laserScan = cv::Mat(1, (int)indices->size(), CV_32FC3);
1403  for(unsigned int i=0; i<indices->size(); ++i)
1404  {
1405  int index = indices->at(i);
1406  if(!filterNaNs || pcl::isFinite(cloud.at(index)))
1407  {
1408  float * ptr = laserScan.ptr<float>(0, oi++);
1409  if(!nullTransform)
1410  {
1411  pcl::PointXYZ pt = pcl::transformPoint(cloud.at(index), transform3f);
1412  ptr[0] = pt.x;
1413  ptr[1] = pt.y;
1414  ptr[2] = pt.z;
1415  }
1416  else
1417  {
1418  ptr[0] = cloud.at(index).x;
1419  ptr[1] = cloud.at(index).y;
1420  ptr[2] = cloud.at(index).z;
1421  }
1422  }
1423  }
1424  }
1425  else
1426  {
1427  laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC3);
1428  for(unsigned int i=0; i<cloud.size(); ++i)
1429  {
1430  if(!filterNaNs || pcl::isFinite(cloud.at(i)))
1431  {
1432  float * ptr = laserScan.ptr<float>(0, oi++);
1433  if(!nullTransform)
1434  {
1435  pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
1436  ptr[0] = pt.x;
1437  ptr[1] = pt.y;
1438  ptr[2] = pt.z;
1439  }
1440  else
1441  {
1442  ptr[0] = cloud.at(i).x;
1443  ptr[1] = cloud.at(i).y;
1444  ptr[2] = cloud.at(i).z;
1445  }
1446  }
1447  }
1448  }
1449  if(oi == 0)
1450  {
1451  return LaserScan();
1452  }
1453  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZ);
1454 }
1455 
1456 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
1457 {
1458  return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
1459 }
1460 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
1461 {
1462  cv::Mat laserScan;
1463  bool nullTransform = transform.isNull() || transform.isIdentity();
1464  int oi=0;
1465  if(indices.get())
1466  {
1467  laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(6));
1468  for(unsigned int i=0; i<indices->size(); ++i)
1469  {
1470  int index = indices->at(i);
1471  if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
1472  uIsFinite(cloud.at(index).normal_x) &&
1473  uIsFinite(cloud.at(index).normal_y) &&
1474  uIsFinite(cloud.at(index).normal_z)))
1475  {
1476  float * ptr = laserScan.ptr<float>(0, oi++);
1477  if(!nullTransform)
1478  {
1479  pcl::PointNormal pt = util3d::transformPoint(cloud.at(index), transform);
1480  ptr[0] = pt.x;
1481  ptr[1] = pt.y;
1482  ptr[2] = pt.z;
1483  ptr[3] = pt.normal_x;
1484  ptr[4] = pt.normal_y;
1485  ptr[5] = pt.normal_z;
1486  }
1487  else
1488  {
1489  ptr[0] = cloud.at(index).x;
1490  ptr[1] = cloud.at(index).y;
1491  ptr[2] = cloud.at(index).z;
1492  ptr[3] = cloud.at(index).normal_x;
1493  ptr[4] = cloud.at(index).normal_y;
1494  ptr[5] = cloud.at(index).normal_z;
1495  }
1496  }
1497  }
1498  }
1499  else
1500  {
1501  laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
1502  for(unsigned int i=0; i<cloud.size(); ++i)
1503  {
1504  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
1505  uIsFinite(cloud.at(i).normal_x) &&
1506  uIsFinite(cloud.at(i).normal_y) &&
1507  uIsFinite(cloud.at(i).normal_z)))
1508  {
1509  float * ptr = laserScan.ptr<float>(0, oi++);
1510  if(!nullTransform)
1511  {
1512  pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
1513  ptr[0] = pt.x;
1514  ptr[1] = pt.y;
1515  ptr[2] = pt.z;
1516  ptr[3] = pt.normal_x;
1517  ptr[4] = pt.normal_y;
1518  ptr[5] = pt.normal_z;
1519  }
1520  else
1521  {
1522  ptr[0] = cloud.at(i).x;
1523  ptr[1] = cloud.at(i).y;
1524  ptr[2] = cloud.at(i).z;
1525  ptr[3] = cloud.at(i).normal_x;
1526  ptr[4] = cloud.at(i).normal_y;
1527  ptr[5] = cloud.at(i).normal_z;
1528  }
1529  }
1530  }
1531  }
1532  if(oi == 0)
1533  {
1534  return LaserScan();
1535  }
1536  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZNormal);
1537 }
1538 
1539 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
1540 {
1541  UASSERT(cloud.size() == normals.size());
1542  cv::Mat laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
1543  bool nullTransform = transform.isNull() || transform.isIdentity();
1544  int oi =0;
1545  for(unsigned int i=0; i<cloud.size(); ++i)
1546  {
1547  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
1548  {
1549  float * ptr = laserScan.ptr<float>(0, oi++);
1550  if(!nullTransform)
1551  {
1552  pcl::PointNormal pt;
1553  pt.x = cloud.at(i).x;
1554  pt.y = cloud.at(i).y;
1555  pt.z = cloud.at(i).z;
1556  pt.normal_x = normals.at(i).normal_x;
1557  pt.normal_y = normals.at(i).normal_y;
1558  pt.normal_z = normals.at(i).normal_z;
1559  pt = util3d::transformPoint(pt, transform);
1560  ptr[0] = pt.x;
1561  ptr[1] = pt.y;
1562  ptr[2] = pt.z;
1563  ptr[3] = pt.normal_x;
1564  ptr[4] = pt.normal_y;
1565  ptr[5] = pt.normal_z;
1566  }
1567  else
1568  {
1569  ptr[0] = cloud.at(i).x;
1570  ptr[1] = cloud.at(i).y;
1571  ptr[2] = cloud.at(i).z;
1572  ptr[3] = normals.at(i).normal_x;
1573  ptr[4] = normals.at(i).normal_y;
1574  ptr[5] = normals.at(i).normal_z;
1575  }
1576  }
1577  }
1578  if(oi == 0)
1579  {
1580  return LaserScan();
1581  }
1582  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZNormal);
1583 }
1584 
1585 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform, bool filterNaNs)
1586 {
1587  return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
1588 }
1589 
1590 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
1591 {
1592  cv::Mat laserScan;
1593  bool nullTransform = transform.isNull() || transform.isIdentity();
1594  Eigen::Affine3f transform3f = transform.toEigen3f();
1595  int oi=0;
1596  if(indices.get())
1597  {
1598  laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4));
1599  for(unsigned int i=0; i<indices->size(); ++i)
1600  {
1601  int index = indices->at(i);
1602  if(!filterNaNs || pcl::isFinite(cloud.at(index)))
1603  {
1604  float * ptr = laserScan.ptr<float>(0, oi++);
1605  if(!nullTransform)
1606  {
1607  pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(index), transform3f);
1608  ptr[0] = pt.x;
1609  ptr[1] = pt.y;
1610  ptr[2] = pt.z;
1611  }
1612  else
1613  {
1614  ptr[0] = cloud.at(index).x;
1615  ptr[1] = cloud.at(index).y;
1616  ptr[2] = cloud.at(index).z;
1617  }
1618  int * ptrInt = (int*)ptr;
1619  ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16);
1620  }
1621  }
1622  }
1623  else
1624  {
1625  laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
1626  for(unsigned int i=0; i<cloud.size(); ++i)
1627  {
1628  if(!filterNaNs || pcl::isFinite(cloud.at(i)))
1629  {
1630  float * ptr = laserScan.ptr<float>(0, oi++);
1631  if(!nullTransform)
1632  {
1633  pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(i), transform3f);
1634  ptr[0] = pt.x;
1635  ptr[1] = pt.y;
1636  ptr[2] = pt.z;
1637  }
1638  else
1639  {
1640  ptr[0] = cloud.at(i).x;
1641  ptr[1] = cloud.at(i).y;
1642  ptr[2] = cloud.at(i).z;
1643  }
1644  int * ptrInt = (int*)ptr;
1645  ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
1646  }
1647  }
1648  }
1649  if(oi == 0)
1650  {
1651  return LaserScan();
1652  }
1653  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZRGB);
1654 }
1655 
1656 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
1657 {
1658  return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
1659 }
1660 
1661 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
1662 {
1663  cv::Mat laserScan;
1664  bool nullTransform = transform.isNull() || transform.isIdentity();
1665  Eigen::Affine3f transform3f = transform.toEigen3f();
1666  int oi=0;
1667  if(indices.get())
1668  {
1669  laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4));
1670  for(unsigned int i=0; i<indices->size(); ++i)
1671  {
1672  int index = indices->at(i);
1673  if(!filterNaNs || pcl::isFinite(cloud.at(index)))
1674  {
1675  float * ptr = laserScan.ptr<float>(0, oi++);
1676  if(!nullTransform)
1677  {
1678  pcl::PointXYZI pt = pcl::transformPoint(cloud.at(index), transform3f);
1679  ptr[0] = pt.x;
1680  ptr[1] = pt.y;
1681  ptr[2] = pt.z;
1682  }
1683  else
1684  {
1685  ptr[0] = cloud.at(index).x;
1686  ptr[1] = cloud.at(index).y;
1687  ptr[2] = cloud.at(index).z;
1688  }
1689  ptr[3] = cloud.at(index).intensity;
1690  }
1691  }
1692  }
1693  else
1694  {
1695  laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
1696  for(unsigned int i=0; i<cloud.size(); ++i)
1697  {
1698  if(!filterNaNs || pcl::isFinite(cloud.at(i)))
1699  {
1700  float * ptr = laserScan.ptr<float>(0, oi++);
1701  if(!nullTransform)
1702  {
1703  pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f);
1704  ptr[0] = pt.x;
1705  ptr[1] = pt.y;
1706  ptr[2] = pt.z;
1707  }
1708  else
1709  {
1710  ptr[0] = cloud.at(i).x;
1711  ptr[1] = cloud.at(i).y;
1712  ptr[2] = cloud.at(i).z;
1713  }
1714  ptr[3] = cloud.at(i).intensity;
1715  }
1716  }
1717  }
1718  if(oi == 0)
1719  {
1720  return LaserScan();
1721  }
1722  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZI);
1723 }
1724 
1725 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
1726 {
1727  UASSERT(cloud.size() == normals.size());
1728  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
1729  bool nullTransform = transform.isNull() || transform.isIdentity();
1730  int oi = 0;
1731  for(unsigned int i=0; i<cloud.size(); ++i)
1732  {
1733  if(!filterNaNs || pcl::isFinite(cloud.at(i)))
1734  {
1735  float * ptr = laserScan.ptr<float>(0, oi++);
1736  if(!nullTransform)
1737  {
1738  pcl::PointXYZRGBNormal pt;
1739  pt.x = cloud.at(i).x;
1740  pt.y = cloud.at(i).y;
1741  pt.z = cloud.at(i).z;
1742  pt.normal_x = normals.at(i).normal_x;
1743  pt.normal_y = normals.at(i).normal_y;
1744  pt.normal_z = normals.at(i).normal_z;
1745  pt = util3d::transformPoint(pt, transform);
1746  ptr[0] = pt.x;
1747  ptr[1] = pt.y;
1748  ptr[2] = pt.z;
1749  ptr[4] = pt.normal_x;
1750  ptr[5] = pt.normal_y;
1751  ptr[6] = pt.normal_z;
1752  }
1753  else
1754  {
1755  ptr[0] = cloud.at(i).x;
1756  ptr[1] = cloud.at(i).y;
1757  ptr[2] = cloud.at(i).z;
1758  ptr[4] = normals.at(i).normal_x;
1759  ptr[5] = normals.at(i).normal_y;
1760  ptr[6] = normals.at(i).normal_z;
1761  }
1762  int * ptrInt = (int*)ptr;
1763  ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
1764  }
1765  }
1766  if(oi == 0)
1767  {
1768  return LaserScan();
1769  }
1770  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZRGBNormal);
1771 }
1772 
1773 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform, bool filterNaNs)
1774 {
1775  return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
1776 }
1777 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
1778 {
1779  cv::Mat laserScan;
1780  bool nullTransform = transform.isNull() || transform.isIdentity();
1781  int oi = 0;
1782  if(indices.get())
1783  {
1784  laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(7));
1785  for(unsigned int i=0; i<indices->size(); ++i)
1786  {
1787  int index = indices->at(i);
1788  if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
1789  uIsFinite(cloud.at(index).normal_x) &&
1790  uIsFinite(cloud.at(index).normal_y) &&
1791  uIsFinite(cloud.at(index).normal_z)))
1792  {
1793  float * ptr = laserScan.ptr<float>(0, oi++);
1794  if(!nullTransform)
1795  {
1796  pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(index), transform);
1797  ptr[0] = pt.x;
1798  ptr[1] = pt.y;
1799  ptr[2] = pt.z;
1800  ptr[4] = pt.normal_x;
1801  ptr[5] = pt.normal_y;
1802  ptr[6] = pt.normal_z;
1803  }
1804  else
1805  {
1806  ptr[0] = cloud.at(index).x;
1807  ptr[1] = cloud.at(index).y;
1808  ptr[2] = cloud.at(index).z;
1809  ptr[4] = cloud.at(index).normal_x;
1810  ptr[5] = cloud.at(index).normal_y;
1811  ptr[6] = cloud.at(index).normal_z;
1812  }
1813  int * ptrInt = (int*)ptr;
1814  ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16);
1815  }
1816  }
1817  }
1818  else
1819  {
1820  laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(7));
1821  for(unsigned int i=0; i<cloud.size(); ++i)
1822  {
1823  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
1824  uIsFinite(cloud.at(i).normal_x) &&
1825  uIsFinite(cloud.at(i).normal_y) &&
1826  uIsFinite(cloud.at(i).normal_z)))
1827  {
1828  float * ptr = laserScan.ptr<float>(0, oi++);
1829  if(!nullTransform)
1830  {
1831  pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(i), transform);
1832  ptr[0] = pt.x;
1833  ptr[1] = pt.y;
1834  ptr[2] = pt.z;
1835  ptr[4] = pt.normal_x;
1836  ptr[5] = pt.normal_y;
1837  ptr[6] = pt.normal_z;
1838  }
1839  else
1840  {
1841  ptr[0] = cloud.at(i).x;
1842  ptr[1] = cloud.at(i).y;
1843  ptr[2] = cloud.at(i).z;
1844  ptr[4] = cloud.at(i).normal_x;
1845  ptr[5] = cloud.at(i).normal_y;
1846  ptr[6] = cloud.at(i).normal_z;
1847  }
1848  int * ptrInt = (int*)ptr;
1849  ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
1850  }
1851  }
1852  }
1853  if(oi == 0)
1854  {
1855  return LaserScan();
1856  }
1857  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZRGBNormal);
1858 }
1859 
1860 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
1861 {
1862  UASSERT(cloud.size() == normals.size());
1863  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
1864  bool nullTransform = transform.isNull() || transform.isIdentity();
1865  int oi=0;
1866  for(unsigned int i=0; i<cloud.size(); ++i)
1867  {
1868  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
1869  {
1870  float * ptr = laserScan.ptr<float>(0, oi++);
1871  if(!nullTransform)
1872  {
1873  pcl::PointXYZINormal pt;
1874  pt.x = cloud.at(i).x;
1875  pt.y = cloud.at(i).y;
1876  pt.z = cloud.at(i).z;
1877  pt.normal_x = normals.at(i).normal_x;
1878  pt.normal_y = normals.at(i).normal_y;
1879  pt.normal_z = normals.at(i).normal_z;
1880  pt = util3d::transformPoint(pt, transform);
1881  ptr[0] = pt.x;
1882  ptr[1] = pt.y;
1883  ptr[2] = pt.z;
1884  ptr[4] = pt.normal_x;
1885  ptr[5] = pt.normal_y;
1886  ptr[6] = pt.normal_z;
1887  }
1888  else
1889  {
1890  ptr[0] = cloud.at(i).x;
1891  ptr[1] = cloud.at(i).y;
1892  ptr[2] = cloud.at(i).z;
1893  ptr[4] = normals.at(i).normal_x;
1894  ptr[5] = normals.at(i).normal_y;
1895  ptr[6] = normals.at(i).normal_z;
1896  }
1897  ptr[3] = cloud.at(i).intensity;
1898  }
1899  }
1900  if(oi == 0)
1901  {
1902  return LaserScan();
1903  }
1904  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZINormal);
1905 }
1906 
1907 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
1908 {
1909  return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
1910 }
1911 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
1912 {
1913  cv::Mat laserScan;
1914  bool nullTransform = transform.isNull() || transform.isIdentity();
1915  int oi = 0;
1916  if(indices.get())
1917  {
1918  laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(7));
1919  for(unsigned int i=0; i<indices->size(); ++i)
1920  {
1921  int index = indices->at(i);
1922  if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
1923  uIsFinite(cloud.at(index).normal_x) &&
1924  uIsFinite(cloud.at(index).normal_y) &&
1925  uIsFinite(cloud.at(index).normal_z)))
1926  {
1927  float * ptr = laserScan.ptr<float>(0, oi++);
1928  if(!nullTransform)
1929  {
1930  pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(index), transform);
1931  ptr[0] = pt.x;
1932  ptr[1] = pt.y;
1933  ptr[2] = pt.z;
1934  ptr[4] = pt.normal_x;
1935  ptr[5] = pt.normal_y;
1936  ptr[6] = pt.normal_z;
1937  }
1938  else
1939  {
1940  ptr[0] = cloud.at(index).x;
1941  ptr[1] = cloud.at(index).y;
1942  ptr[2] = cloud.at(index).z;
1943  ptr[4] = cloud.at(index).normal_x;
1944  ptr[5] = cloud.at(index).normal_y;
1945  ptr[6] = cloud.at(index).normal_z;
1946  }
1947  ptr[3] = cloud.at(i).intensity;
1948  }
1949  }
1950  }
1951  else
1952  {
1953  laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(7));
1954  for(unsigned int i=0; i<cloud.size(); ++i)
1955  {
1956  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
1957  uIsFinite(cloud.at(i).normal_x) &&
1958  uIsFinite(cloud.at(i).normal_y) &&
1959  uIsFinite(cloud.at(i).normal_z)))
1960  {
1961  float * ptr = laserScan.ptr<float>(0, oi++);
1962  if(!nullTransform)
1963  {
1964  pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform);
1965  ptr[0] = pt.x;
1966  ptr[1] = pt.y;
1967  ptr[2] = pt.z;
1968  ptr[4] = pt.normal_x;
1969  ptr[5] = pt.normal_y;
1970  ptr[6] = pt.normal_z;
1971  }
1972  else
1973  {
1974  ptr[0] = cloud.at(i).x;
1975  ptr[1] = cloud.at(i).y;
1976  ptr[2] = cloud.at(i).z;
1977  ptr[4] = cloud.at(i).normal_x;
1978  ptr[5] = cloud.at(i).normal_y;
1979  ptr[6] = cloud.at(i).normal_z;
1980  }
1981  ptr[3] = cloud.at(i).intensity;
1982  }
1983  }
1984  }
1985  if(oi == 0)
1986  {
1987  return LaserScan();
1988  }
1989  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZINormal);
1990 }
1991 
1992 LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
1993 {
1994  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
1995  bool nullTransform = transform.isNull();
1996  Eigen::Affine3f transform3f = transform.toEigen3f();
1997  int oi=0;
1998  for(unsigned int i=0; i<cloud.size(); ++i)
1999  {
2000  if(!filterNaNs || pcl::isFinite(cloud.at(i)))
2001  {
2002  float * ptr = laserScan.ptr<float>(0, oi++);
2003  if(!nullTransform)
2004  {
2005  pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
2006  ptr[0] = pt.x;
2007  ptr[1] = pt.y;
2008  }
2009  else
2010  {
2011  ptr[0] = cloud.at(i).x;
2012  ptr[1] = cloud.at(i).y;
2013  }
2014  }
2015 
2016  }
2017  if(oi == 0)
2018  {
2019  return LaserScan();
2020  }
2021  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXY);
2022 }
2023 
2024 LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
2025 {
2026  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
2027  bool nullTransform = transform.isNull();
2028  Eigen::Affine3f transform3f = transform.toEigen3f();
2029  int oi=0;
2030  for(unsigned int i=0; i<cloud.size(); ++i)
2031  {
2032  if(!filterNaNs || pcl::isFinite(cloud.at(i)))
2033  {
2034  float * ptr = laserScan.ptr<float>(0, oi++);
2035  if(!nullTransform)
2036  {
2037  pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f);
2038  ptr[0] = pt.x;
2039  ptr[1] = pt.y;
2040  ptr[2] = pt.intensity;
2041  }
2042  else
2043  {
2044  ptr[0] = cloud.at(i).x;
2045  ptr[1] = cloud.at(i).y;
2046  ptr[2] = cloud.at(i).intensity;
2047  }
2048  }
2049 
2050  }
2051  if(oi == 0)
2052  {
2053  return LaserScan();
2054  }
2055  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYI);
2056 }
2057 
2058 LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
2059 {
2060  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
2061  bool nullTransform = transform.isNull();
2062  int oi=0;
2063  for(unsigned int i=0; i<cloud.size(); ++i)
2064  {
2065  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
2066  uIsFinite(cloud.at(i).normal_x) &&
2067  uIsFinite(cloud.at(i).normal_y) &&
2068  uIsFinite(cloud.at(i).normal_z)))
2069  {
2070  float * ptr = laserScan.ptr<float>(0, oi++);
2071  if(!nullTransform)
2072  {
2073  pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
2074  ptr[0] = pt.x;
2075  ptr[1] = pt.y;
2076  ptr[2] = pt.normal_x;
2077  ptr[3] = pt.normal_y;
2078  ptr[4] = pt.normal_z;
2079  }
2080  else
2081  {
2082  const pcl::PointNormal & pt = cloud.at(i);
2083  ptr[0] = pt.x;
2084  ptr[1] = pt.y;
2085  ptr[2] = pt.normal_x;
2086  ptr[3] = pt.normal_y;
2087  ptr[4] = pt.normal_z;
2088  }
2089  }
2090  }
2091  if(oi == 0)
2092  {
2093  return LaserScan();
2094  }
2095  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYNormal);
2096 }
2097 
2098 LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
2099 {
2100  UASSERT(cloud.size() == normals.size());
2101  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
2102  bool nullTransform = transform.isNull() || transform.isIdentity();
2103  int oi=0;
2104  for(unsigned int i=0; i<cloud.size(); ++i)
2105  {
2106  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
2107  {
2108  float * ptr = laserScan.ptr<float>(0, oi++);
2109  if(!nullTransform)
2110  {
2111  pcl::PointNormal pt;
2112  pt.x = cloud.at(i).x;
2113  pt.y = cloud.at(i).y;
2114  pt.z = cloud.at(i).z;
2115  pt.normal_x = normals.at(i).normal_x;
2116  pt.normal_y = normals.at(i).normal_y;
2117  pt.normal_z = normals.at(i).normal_z;
2118  pt = util3d::transformPoint(pt, transform);
2119  ptr[0] = pt.x;
2120  ptr[1] = pt.y;
2121  ptr[2] = pt.normal_x;
2122  ptr[3] = pt.normal_y;
2123  ptr[4] = pt.normal_z;
2124  }
2125  else
2126  {
2127  ptr[0] = cloud.at(i).x;
2128  ptr[1] = cloud.at(i).y;
2129  ptr[2] = normals.at(i).normal_x;
2130  ptr[3] = normals.at(i).normal_y;
2131  ptr[4] = normals.at(i).normal_z;
2132  }
2133  }
2134  }
2135  if(oi == 0)
2136  {
2137  return LaserScan();
2138  }
2139  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYNormal);
2140 }
2141 
2142 LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
2143 {
2144  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
2145  bool nullTransform = transform.isNull();
2146  int oi=0;
2147  for(unsigned int i=0; i<cloud.size(); ++i)
2148  {
2149  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
2150  uIsFinite(cloud.at(i).normal_x) &&
2151  uIsFinite(cloud.at(i).normal_y) &&
2152  uIsFinite(cloud.at(i).normal_z)))
2153  {
2154  float * ptr = laserScan.ptr<float>(0, oi++);
2155  if(!nullTransform)
2156  {
2157  pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform);
2158  ptr[0] = pt.x;
2159  ptr[1] = pt.y;
2160  ptr[2] = pt.intensity;
2161  ptr[3] = pt.normal_x;
2162  ptr[4] = pt.normal_y;
2163  ptr[5] = pt.normal_z;
2164  }
2165  else
2166  {
2167  const pcl::PointXYZINormal & pt = cloud.at(i);
2168  ptr[0] = pt.x;
2169  ptr[1] = pt.y;
2170  ptr[2] = pt.intensity;
2171  ptr[3] = pt.normal_x;
2172  ptr[4] = pt.normal_y;
2173  ptr[5] = pt.normal_z;
2174  }
2175  }
2176  }
2177  if(oi == 0)
2178  {
2179  return LaserScan();
2180  }
2181  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYINormal);
2182 }
2183 
2184 LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
2185 {
2186  UASSERT(cloud.size() == normals.size());
2187  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
2188  bool nullTransform = transform.isNull() || transform.isIdentity();
2189  int oi=0;
2190  for(unsigned int i=0; i<cloud.size(); ++i)
2191  {
2192  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
2193  {
2194  float * ptr = laserScan.ptr<float>(0, oi++);
2195  if(!nullTransform)
2196  {
2197  pcl::PointXYZINormal pt;
2198  pt.x = cloud.at(i).x;
2199  pt.y = cloud.at(i).y;
2200  pt.z = cloud.at(i).z;
2201  pt.normal_x = normals.at(i).normal_x;
2202  pt.normal_y = normals.at(i).normal_y;
2203  pt.normal_z = normals.at(i).normal_z;
2204  pt = util3d::transformPoint(pt, transform);
2205  ptr[0] = pt.x;
2206  ptr[1] = pt.y;
2207  ptr[2] = pt.intensity;
2208  ptr[3] = pt.normal_x;
2209  ptr[4] = pt.normal_y;
2210  ptr[5] = pt.normal_z;
2211  }
2212  else
2213  {
2214  ptr[0] = cloud.at(i).x;
2215  ptr[1] = cloud.at(i).y;
2216  ptr[2] = cloud.at(i).intensity;
2217  ptr[3] = normals.at(i).normal_x;
2218  ptr[4] = normals.at(i).normal_y;
2219  ptr[5] = normals.at(i).normal_z;
2220  }
2221  }
2222  }
2223  if(oi == 0)
2224  {
2225  return LaserScan();
2226  }
2227  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYINormal);
2228 }
2229 
2230 pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform)
2231 {
2232  pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
2233  if(laserScan.isEmpty())
2234  {
2235  return cloud;
2236  }
2237 
2238  if(laserScan.format() == LaserScan::kXY || laserScan.format() == LaserScan::kXYZ)
2239  {
2240  pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
2241  }
2242  else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT)
2243  {
2244  pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
2245  }
2246  else if(laserScan.format() == LaserScan::kXYNormal || laserScan.format() == LaserScan::kXYZNormal)
2247  {
2248  pcl::toPCLPointCloud2(*laserScanToPointCloudNormal(laserScan, transform), *cloud);
2249  }
2250  else if(laserScan.format() == LaserScan::kXYINormal || laserScan.format() == LaserScan::kXYZINormal)
2251  {
2252  pcl::toPCLPointCloud2(*laserScanToPointCloudINormal(laserScan, transform), *cloud);
2253  }
2254  else if(laserScan.format() == LaserScan::kXYZRGB)
2255  {
2256  pcl::toPCLPointCloud2(*laserScanToPointCloudRGB(laserScan, transform), *cloud);
2257  }
2258  else if(laserScan.format() == LaserScan::kXYZRGBNormal)
2259  {
2260  pcl::toPCLPointCloud2(*laserScanToPointCloudRGBNormal(laserScan, transform), *cloud);
2261  }
2262  else
2263  {
2264  UERROR("Unknown conversion from LaserScan format %d to PointCloud2.", laserScan.format());
2265  }
2266  return cloud;
2267 }
2268 
2269 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform)
2270 {
2271  pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
2272  if(laserScan.isOrganized())
2273  {
2274  output->width = laserScan.data().cols;
2275  output->height = laserScan.data().rows;
2276  output->is_dense = false;
2277  }
2278  else
2279  {
2280  output->is_dense = true;
2281  }
2282  output->resize(laserScan.size());
2283  bool nullTransform = transform.isNull();
2284  Eigen::Affine3f transform3f = transform.toEigen3f();
2285  for(int i=0; i<laserScan.size(); ++i)
2286  {
2287  output->at(i) = util3d::laserScanToPoint(laserScan, i);
2288  if(!nullTransform)
2289  {
2290  output->at(i) = pcl::transformPoint(output->at(i), transform3f);
2291  }
2292  }
2293  return output;
2294 }
2295 
2296 pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform)
2297 {
2298  pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
2299  if(laserScan.isOrganized())
2300  {
2301  output->width = laserScan.data().cols;
2302  output->height = laserScan.data().rows;
2303  output->is_dense = false;
2304  }
2305  else
2306  {
2307  output->is_dense = true;
2308  }
2309  output->resize(laserScan.size());
2310  bool nullTransform = transform.isNull();
2311  for(int i=0; i<laserScan.size(); ++i)
2312  {
2313  output->at(i) = laserScanToPointNormal(laserScan, i);
2314  if(!nullTransform)
2315  {
2316  output->at(i) = util3d::transformPoint(output->at(i), transform);
2317  }
2318  }
2319  return output;
2320 }
2321 
2322 pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
2323 {
2324  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
2325  if(laserScan.isOrganized())
2326  {
2327  output->width = laserScan.data().cols;
2328  output->height = laserScan.data().rows;
2329  output->is_dense = false;
2330  }
2331  else
2332  {
2333  output->is_dense = true;
2334  }
2335  output->resize(laserScan.size());
2336  bool nullTransform = transform.isNull() || transform.isIdentity();
2337  Eigen::Affine3f transform3f = transform.toEigen3f();
2338  for(int i=0; i<laserScan.size(); ++i)
2339  {
2340  output->at(i) = util3d::laserScanToPointRGB(laserScan, i, r, g, b);
2341  if(!nullTransform)
2342  {
2343  output->at(i) = pcl::transformPoint(output->at(i), transform3f);
2344  }
2345  }
2346  return output;
2347 }
2348 
2349 pcl::PointCloud<pcl::PointXYZI>::Ptr laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform, float intensity)
2350 {
2351  pcl::PointCloud<pcl::PointXYZI>::Ptr output(new pcl::PointCloud<pcl::PointXYZI>);
2352  if(laserScan.isOrganized())
2353  {
2354  output->width = laserScan.data().cols;
2355  output->height = laserScan.data().rows;
2356  output->is_dense = false;
2357  }
2358  else
2359  {
2360  output->is_dense = true;
2361  }
2362  output->resize(laserScan.size());
2363  bool nullTransform = transform.isNull() || transform.isIdentity();
2364  Eigen::Affine3f transform3f = transform.toEigen3f();
2365  for(int i=0; i<laserScan.size(); ++i)
2366  {
2367  output->at(i) = util3d::laserScanToPointI(laserScan, i, intensity);
2368  if(!nullTransform)
2369  {
2370  output->at(i) = pcl::transformPoint(output->at(i), transform3f);
2371  }
2372  }
2373  return output;
2374 }
2375 
2376 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
2377 {
2378  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
2379  if(laserScan.isOrganized())
2380  {
2381  output->width = laserScan.data().cols;
2382  output->height = laserScan.data().rows;
2383  output->is_dense = false;
2384  }
2385  else
2386  {
2387  output->is_dense = true;
2388  }
2389  output->resize(laserScan.size());
2390  bool nullTransform = transform.isNull() || transform.isIdentity();
2391  for(int i=0; i<laserScan.size(); ++i)
2392  {
2393  output->at(i) = util3d::laserScanToPointRGBNormal(laserScan, i, r, g, b);
2394  if(!nullTransform)
2395  {
2396  output->at(i) = util3d::transformPoint(output->at(i), transform);
2397  }
2398  }
2399  return output;
2400 }
2401 
2402 pcl::PointCloud<pcl::PointXYZINormal>::Ptr laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform, float intensity)
2403 {
2404  pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZINormal>);
2405  if(laserScan.isOrganized())
2406  {
2407  output->width = laserScan.data().cols;
2408  output->height = laserScan.data().rows;
2409  output->is_dense = false;
2410  }
2411  else
2412  {
2413  output->is_dense = true;
2414  }
2415  output->resize(laserScan.size());
2416  bool nullTransform = transform.isNull() || transform.isIdentity();
2417  for(int i=0; i<laserScan.size(); ++i)
2418  {
2419  output->at(i) = util3d::laserScanToPointINormal(laserScan, i, intensity);
2420  if(!nullTransform)
2421  {
2422  output->at(i) = util3d::transformPoint(output->at(i), transform);
2423  }
2424  }
2425  return output;
2426 }
2427 
2428 pcl::PointXYZ laserScanToPoint(const LaserScan & laserScan, int index)
2429 {
2430  UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
2431  pcl::PointXYZ output;
2432  int row = index / laserScan.data().cols;
2433  const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
2434  output.x = ptr[0];
2435  output.y = ptr[1];
2436  if(!laserScan.is2d())
2437  {
2438  output.z = ptr[2];
2439  }
2440  return output;
2441 }
2442 
2443 pcl::PointNormal laserScanToPointNormal(const LaserScan & laserScan, int index)
2444 {
2445  UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
2446  pcl::PointNormal output;
2447  int row = index / laserScan.data().cols;
2448  const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
2449  output.x = ptr[0];
2450  output.y = ptr[1];
2451  if(!laserScan.is2d())
2452  {
2453  output.z = ptr[2];
2454  }
2455  if(laserScan.hasNormals())
2456  {
2457  int offset = laserScan.getNormalsOffset();
2458  output.normal_x = ptr[offset];
2459  output.normal_y = ptr[offset+1];
2460  output.normal_z = ptr[offset+2];
2461  }
2462  return output;
2463 }
2464 
2465 pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
2466 {
2467  UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
2468  pcl::PointXYZRGB output;
2469  int row = index / laserScan.data().cols;
2470  const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
2471  output.x = ptr[0];
2472  output.y = ptr[1];
2473  if(!laserScan.is2d())
2474  {
2475  output.z = ptr[2];
2476  }
2477 
2478  if(laserScan.hasRGB())
2479  {
2480  int * ptrInt = (int*)ptr;
2481  int indexRGB = laserScan.getRGBOffset();
2482  output.b = (unsigned char)(ptrInt[indexRGB] & 0xFF);
2483  output.g = (unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF);
2484  output.r = (unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF);
2485  }
2486  else if(laserScan.hasIntensity())
2487  {
2488  // package intensity float -> rgba
2489  int * ptrInt = (int*)ptr;
2490  int indexIntensity = laserScan.getIntensityOffset();
2491  output.r = (unsigned char)(ptrInt[indexIntensity] & 0xFF);
2492  output.g = (unsigned char)((ptrInt[indexIntensity] >> 8) & 0xFF);
2493  output.b = (unsigned char)((ptrInt[indexIntensity] >> 16) & 0xFF);
2494  output.a = (unsigned char)((ptrInt[indexIntensity] >> 24) & 0xFF);
2495  }
2496  else
2497  {
2498  output.r = r;
2499  output.g = g;
2500  output.b = b;
2501  }
2502  return output;
2503 }
2504 
2505 pcl::PointXYZI laserScanToPointI(const LaserScan & laserScan, int index, float intensity)
2506 {
2507  UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
2508  pcl::PointXYZI output;
2509  int row = index / laserScan.data().cols;
2510  const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
2511  output.x = ptr[0];
2512  output.y = ptr[1];
2513  if(!laserScan.is2d())
2514  {
2515  output.z = ptr[2];
2516  }
2517 
2518  if(laserScan.hasIntensity())
2519  {
2520  int offset = laserScan.getIntensityOffset();
2521  output.intensity = ptr[offset];
2522  }
2523  else
2524  {
2525  output.intensity = intensity;
2526  }
2527 
2528  return output;
2529 }
2530 
2531 pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
2532 {
2533  UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
2534  pcl::PointXYZRGBNormal output;
2535  int row = index / laserScan.data().cols;
2536  const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
2537  output.x = ptr[0];
2538  output.y = ptr[1];
2539  if(!laserScan.is2d())
2540  {
2541  output.z = ptr[2];
2542  }
2543 
2544  if(laserScan.hasRGB())
2545  {
2546  int * ptrInt = (int*)ptr;
2547  int indexRGB = laserScan.getRGBOffset();
2548  output.b = (unsigned char)(ptrInt[indexRGB] & 0xFF);
2549  output.g = (unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF);
2550  output.r = (unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF);
2551  }
2552  else if(laserScan.hasIntensity())
2553  {
2554  int * ptrInt = (int*)ptr;
2555  int indexIntensity = laserScan.getIntensityOffset();
2556  output.r = (unsigned char)(ptrInt[indexIntensity] & 0xFF);
2557  output.g = (unsigned char)((ptrInt[indexIntensity] >> 8) & 0xFF);
2558  output.b = (unsigned char)((ptrInt[indexIntensity] >> 16) & 0xFF);
2559  output.a = (unsigned char)((ptrInt[indexIntensity] >> 24) & 0xFF);
2560  }
2561  else
2562  {
2563  output.r = r;
2564  output.g = g;
2565  output.b = b;
2566  }
2567 
2568  if(laserScan.hasNormals())
2569  {
2570  int offset = laserScan.getNormalsOffset();
2571  output.normal_x = ptr[offset];
2572  output.normal_y = ptr[offset+1];
2573  output.normal_z = ptr[offset+2];
2574  }
2575 
2576  return output;
2577 }
2578 
2579 pcl::PointXYZINormal laserScanToPointINormal(const LaserScan & laserScan, int index, float intensity)
2580 {
2581  UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
2582  pcl::PointXYZINormal output;
2583  int row = index / laserScan.data().cols;
2584  const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
2585  output.x = ptr[0];
2586  output.y = ptr[1];
2587  if(!laserScan.is2d())
2588  {
2589  output.z = ptr[2];
2590  }
2591 
2592  if(laserScan.hasIntensity())
2593  {
2594  int offset = laserScan.getIntensityOffset();
2595  output.intensity = ptr[offset];
2596  }
2597  else
2598  {
2599  output.intensity = intensity;
2600  }
2601 
2602  if(laserScan.hasNormals())
2603  {
2604  int offset = laserScan.getNormalsOffset();
2605  output.normal_x = ptr[offset];
2606  output.normal_y = ptr[offset+1];
2607  output.normal_z = ptr[offset+2];
2608  }
2609 
2610  return output;
2611 }
2612 
2613 void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max)
2614 {
2615  UASSERT(!laserScan.empty());
2616  UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
2617 
2618  const float * ptr = laserScan.ptr<float>(0, 0);
2619  min.x = max.x = ptr[0];
2620  min.y = max.y = ptr[1];
2621  bool is3d = laserScan.channels() >= 3 && laserScan.channels() != 5;
2622  min.z = max.z = is3d?ptr[2]:0.0f;
2623  for(int i=1; i<laserScan.cols; ++i)
2624  {
2625  ptr = laserScan.ptr<float>(0, i);
2626 
2627  if(ptr[0] < min.x) min.x = ptr[0];
2628  else if(ptr[0] > max.x) max.x = ptr[0];
2629 
2630  if(ptr[1] < min.y) min.y = ptr[1];
2631  else if(ptr[1] > max.y) max.y = ptr[1];
2632 
2633  if(is3d)
2634  {
2635  if(ptr[2] < min.z) min.z = ptr[2];
2636  else if(ptr[2] > max.z) max.z = ptr[2];
2637  }
2638  }
2639 }
2640 void getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max)
2641 {
2642  cv::Point3f minCV, maxCV;
2643  getMinMax3D(laserScan, minCV, maxCV);
2644  min.x = minCV.x;
2645  min.y = minCV.y;
2646  min.z = minCV.z;
2647  max.x = maxCV.x;
2648  max.y = maxCV.y;
2649  max.z = maxCV.z;
2650 }
2651 
2652 // inspired from ROS image_geometry/src/stereo_camera_model.cpp
2653 cv::Point3f projectDisparityTo3D(
2654  const cv::Point2f & pt,
2655  float disparity,
2656  const StereoCameraModel & model)
2657 {
2658  if(disparity > 0.0f && model.baseline() > 0.0f && model.left().fx() > 0.0f)
2659  {
2660  //Z = baseline * f / (d + cx1-cx0);
2661  float c = 0.0f;
2662  if(model.right().cx()>0.0f && model.left().cx()>0.0f)
2663  {
2664  c = model.right().cx() - model.left().cx();
2665  }
2666  float W = model.baseline()/(disparity + c);
2667  return cv::Point3f((pt.x - model.left().cx())*W,
2668  (pt.y - model.left().cy())*model.left().fx()/model.left().fy()*W, model.left().fx()*W);
2669  }
2670  float bad_point = std::numeric_limits<float>::quiet_NaN ();
2671  return cv::Point3f(bad_point, bad_point, bad_point);
2672 }
2673 
2674 cv::Point3f projectDisparityTo3D(
2675  const cv::Point2f & pt,
2676  const cv::Mat & disparity,
2677  const StereoCameraModel & model)
2678 {
2679  UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
2680  int u = int(pt.x+0.5f);
2681  int v = int(pt.y+0.5f);
2682  float bad_point = std::numeric_limits<float>::quiet_NaN ();
2683  if(uIsInBounds(u, 0, disparity.cols) &&
2684  uIsInBounds(v, 0, disparity.rows))
2685  {
2686  float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
2687  return projectDisparityTo3D(pt, d, model);
2688  }
2689  return cv::Point3f(bad_point, bad_point, bad_point);
2690 }
2691 
2692 // Register point cloud to camera (return registered depth image)
2693 cv::Mat projectCloudToCamera(
2694  const cv::Size & imageSize,
2695  const cv::Mat & cameraMatrixK,
2696  const cv::Mat & laserScan, // assuming laser scan points are already in /base_link coordinate
2697  const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link
2698 {
2699  UASSERT(!cameraTransform.isNull());
2700  UASSERT(!laserScan.empty());
2701  UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
2702  UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
2703 
2704  float fx = cameraMatrixK.at<double>(0,0);
2705  float fy = cameraMatrixK.at<double>(1,1);
2706  float cx = cameraMatrixK.at<double>(0,2);
2707  float cy = cameraMatrixK.at<double>(1,2);
2708 
2709  cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
2710  Transform t = cameraTransform.inverse();
2711 
2712  int count = 0;
2713  for(int i=0; i<laserScan.cols; ++i)
2714  {
2715  const float* ptr = laserScan.ptr<float>(0, i);
2716 
2717  // Get 3D from laser scan
2718  cv::Point3f ptScan;
2719  if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC(5))
2720  {
2721  // 2D scans
2722  ptScan.x = ptr[0];
2723  ptScan.y = ptr[1];
2724  ptScan.z = 0;
2725  }
2726  else // 3D scans
2727  {
2728  ptScan.x = ptr[0];
2729  ptScan.y = ptr[1];
2730  ptScan.z = ptr[2];
2731  }
2732  ptScan = util3d::transformPoint(ptScan, t);
2733 
2734  // re-project in camera frame
2735  float z = ptScan.z;
2736 
2737  bool set = false;
2738  if(z > 0.0f)
2739  {
2740  float invZ = 1.0f/z;
2741  float dx = (fx*ptScan.x)*invZ + cx;
2742  float dy = (fy*ptScan.y)*invZ + cy;
2743  int dx_low = dx;
2744  int dy_low = dy;
2745  int dx_high = dx + 0.5f;
2746  int dy_high = dy + 0.5f;
2747 
2748  if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
2749  {
2750  float &zReg = registered.at<float>(dy_low, dx_low);
2751  if(zReg == 0 || z < zReg)
2752  {
2753  zReg = z;
2754  }
2755  set = true;
2756  }
2757  if((dx_low != dx_high || dy_low != dy_high) &&
2758  uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
2759  {
2760  float &zReg = registered.at<float>(dy_high, dx_high);
2761  if(zReg == 0 || z < zReg)
2762  {
2763  zReg = z;
2764  }
2765  set = true;
2766  }
2767  }
2768  if(set)
2769  {
2770  count++;
2771  }
2772  }
2773  UDEBUG("Points in camera=%d/%d", count, laserScan.cols);
2774 
2775  return registered;
2776 }
2777 
2778 cv::Mat projectCloudToCamera(
2779  const cv::Size & imageSize,
2780  const cv::Mat & cameraMatrixK,
2781  const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan, // assuming points are already in /base_link coordinate
2782  const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link
2783 {
2784  UASSERT(!cameraTransform.isNull());
2785  UASSERT(!laserScan->empty());
2786  UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
2787 
2788  float fx = cameraMatrixK.at<double>(0,0);
2789  float fy = cameraMatrixK.at<double>(1,1);
2790  float cx = cameraMatrixK.at<double>(0,2);
2791  float cy = cameraMatrixK.at<double>(1,2);
2792 
2793  cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
2794  Transform t = cameraTransform.inverse();
2795 
2796  int count = 0;
2797  for(int i=0; i<(int)laserScan->size(); ++i)
2798  {
2799  // Get 3D from laser scan
2800  pcl::PointXYZ ptScan = laserScan->at(i);
2801  ptScan = util3d::transformPoint(ptScan, t);
2802 
2803  // re-project in camera frame
2804  float z = ptScan.z;
2805  bool set = false;
2806  if(z > 0.0f)
2807  {
2808  float invZ = 1.0f/z;
2809  float dx = (fx*ptScan.x)*invZ + cx;
2810  float dy = (fy*ptScan.y)*invZ + cy;
2811  int dx_low = dx;
2812  int dy_low = dy;
2813  int dx_high = dx + 0.5f;
2814  int dy_high = dy + 0.5f;
2815  if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
2816  {
2817  set = true;
2818  float &zReg = registered.at<float>(dy_low, dx_low);
2819  if(zReg == 0 || z < zReg)
2820  {
2821  zReg = z;
2822  }
2823  }
2824  if((dx_low != dx_high || dy_low != dy_high) &&
2825  uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
2826  {
2827  set = true;
2828  float &zReg = registered.at<float>(dy_high, dx_high);
2829  if(zReg == 0 || z < zReg)
2830  {
2831  zReg = z;
2832  }
2833  }
2834  }
2835  if(set)
2836  {
2837  count++;
2838  }
2839  }
2840  UDEBUG("Points in camera=%d/%d", count, (int)laserScan->size());
2841 
2842  return registered;
2843 }
2844 
2845 cv::Mat projectCloudToCamera(
2846  const cv::Size & imageSize,
2847  const cv::Mat & cameraMatrixK,
2848  const pcl::PCLPointCloud2::Ptr laserScan, // assuming points are already in /base_link coordinate
2849  const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link
2850 {
2851  UASSERT(!cameraTransform.isNull());
2852  UASSERT(!laserScan->data.empty());
2853  UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
2854 
2855  float fx = cameraMatrixK.at<double>(0,0);
2856  float fy = cameraMatrixK.at<double>(1,1);
2857  float cx = cameraMatrixK.at<double>(0,2);
2858  float cy = cameraMatrixK.at<double>(1,2);
2859 
2860  cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
2861  Transform t = cameraTransform.inverse();
2862 
2863  pcl::MsgFieldMap field_map;
2864  pcl::createMapping<pcl::PointXYZ> (laserScan->fields, field_map);
2865 
2866  int count = 0;
2867  if(field_map.size() == 1)
2868  {
2869  for (uint32_t row = 0; row < (uint32_t)laserScan->height; ++row)
2870  {
2871  const uint8_t* row_data = &laserScan->data[row * laserScan->row_step];
2872  for (uint32_t col = 0; col < (uint32_t)laserScan->width; ++col)
2873  {
2874  const uint8_t* msg_data = row_data + col * laserScan->point_step;
2875  pcl::PointXYZ ptScan;
2876  memcpy (&ptScan, msg_data + field_map.front().serialized_offset, field_map.front().size);
2877  ptScan = util3d::transformPoint(ptScan, t);
2878 
2879  // re-project in camera frame
2880  float z = ptScan.z;
2881  bool set = false;
2882  if(z > 0.0f)
2883  {
2884  float invZ = 1.0f/z;
2885  float dx = (fx*ptScan.x)*invZ + cx;
2886  float dy = (fy*ptScan.y)*invZ + cy;
2887  int dx_low = dx;
2888  int dy_low = dy;
2889  int dx_high = dx + 0.5f;
2890  int dy_high = dy + 0.5f;
2891  if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
2892  {
2893  set = true;
2894  float &zReg = registered.at<float>(dy_low, dx_low);
2895  if(zReg == 0 || z < zReg)
2896  {
2897  zReg = z;
2898  }
2899  }
2900  if((dx_low != dx_high || dy_low != dy_high) &&
2901  uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
2902  {
2903  set = true;
2904  float &zReg = registered.at<float>(dy_high, dx_high);
2905  if(zReg == 0 || z < zReg)
2906  {
2907  zReg = z;
2908  }
2909  }
2910  }
2911  if(set)
2912  {
2913  count++;
2914  }
2915  }
2916  }
2917  }
2918  else
2919  {
2920  UERROR("field map pcl::pointXYZ not found!");
2921  }
2922  UDEBUG("Points in camera=%d/%d", count, (int)laserScan->data.size());
2923 
2924  return registered;
2925 }
2926 
2927 void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection, bool fillToBorder)
2928 {
2929  UASSERT(registeredDepth.type() == CV_32FC1);
2930  if(verticalDirection)
2931  {
2932  // vertical, for each column
2933  for(int x=0; x<registeredDepth.cols; ++x)
2934  {
2935  float valueA = 0.0f;
2936  int indexA = -1;
2937  for(int y=0; y<registeredDepth.rows; ++y)
2938  {
2939  float v = registeredDepth.at<float>(y,x);
2940  if(fillToBorder && y == registeredDepth.rows-1 && v<=0.0f && indexA>=0)
2941  {
2942  v = valueA;
2943  }
2944  if(v > 0.0f)
2945  {
2946  if(fillToBorder && indexA < 0)
2947  {
2948  indexA = 0;
2949  valueA = v;
2950  }
2951  if(indexA >=0)
2952  {
2953  int range = y-indexA;
2954  if(range > 1)
2955  {
2956  float slope = (v-valueA)/(range);
2957  for(int k=1; k<range; ++k)
2958  {
2959  registeredDepth.at<float>(indexA+k,x) = valueA+slope*float(k);
2960  }
2961  }
2962  }
2963  valueA = v;
2964  indexA = y;
2965  }
2966  }
2967  }
2968  }
2969  else
2970  {
2971  // horizontal, for each row
2972  for(int y=0; y<registeredDepth.rows; ++y)
2973  {
2974  float valueA = 0.0f;
2975  int indexA = -1;
2976  for(int x=0; x<registeredDepth.cols; ++x)
2977  {
2978  float v = registeredDepth.at<float>(y,x);
2979  if(fillToBorder && x == registeredDepth.cols-1 && v<=0.0f && indexA>=0)
2980  {
2981  v = valueA;
2982  }
2983  if(v > 0.0f)
2984  {
2985  if(fillToBorder && indexA < 0)
2986  {
2987  indexA = 0;
2988  valueA = v;
2989  }
2990  if(indexA >=0)
2991  {
2992  int range = x-indexA;
2993  if(range > 1)
2994  {
2995  float slope = (v-valueA)/(range);
2996  for(int k=1; k<range; ++k)
2997  {
2998  registeredDepth.at<float>(y,indexA+k) = valueA+slope*float(k);
2999  }
3000  }
3001  }
3002  valueA = v;
3003  indexA = x;
3004  }
3005  }
3006  }
3007  }
3008 }
3009 
3010 class ProjectionInfo {
3011 public:
3012  ProjectionInfo():
3013  nodeID(-1),
3014  cameraIndex(-1),
3015  distance(-1)
3016  {}
3017  int nodeID;
3018  int cameraIndex;
3019  pcl::PointXY uv;
3020  float distance;
3021 };
3022 
3027 template<class PointT>
3028 std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamerasImpl (
3029  const typename pcl::PointCloud<PointT> & cloud,
3030  const std::map<int, Transform> & cameraPoses,
3031  const std::map<int, std::vector<CameraModel> > & cameraModels,
3032  float maxDistance,
3033  float maxAngle,
3034  const std::vector<float> & roiRatios,
3035  const cv::Mat & projMask,
3036  bool distanceToCamPolicy,
3037  const ProgressState * state)
3038 {
3039  UINFO("cloud=%d points", (int)cloud.size());
3040  UINFO("cameraPoses=%d", (int)cameraPoses.size());
3041  UINFO("cameraModels=%d", (int)cameraModels.size());
3042  UINFO("maxDistance=%f", maxDistance);
3043  UINFO("maxAngle=%f", maxAngle);
3044  UINFO("distanceToCamPolicy=%s", distanceToCamPolicy?"true":"false");
3045  UINFO("roiRatios=%s", roiRatios.size() == 4?uFormat("%f %f %f %f", roiRatios[0], roiRatios[1], roiRatios[2], roiRatios[3]).c_str():"");
3046  UINFO("projMask=%dx%d", projMask.cols, projMask.rows);
3047  std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
3048 
3049  if (cloud.empty() || cameraPoses.empty() || cameraModels.empty())
3050  return pointToPixel;
3051 
3052  std::string msg = uFormat("Computing visible points per cam (%d points, %d cams)", (int)cloud.size(), (int)cameraPoses.size());
3053  UINFO(msg.c_str());
3054  if(state && !state->callback(msg))
3055  {
3056  //cancelled!
3057  UWARN("Projecting to cameras cancelled!");
3058  return pointToPixel;
3059  }
3060 
3061  std::vector<ProjectionInfo> invertedIndex(cloud.size()); // For each point: list of cameras
3062  int cameraProcessed = 0;
3063  bool wrongMaskFormatWarned = false;
3064  for(std::map<int, Transform>::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter)
3065  {
3066  std::map<int, std::vector<CameraModel> >::const_iterator iter=cameraModels.find(pter->first);
3067  if(iter!=cameraModels.end() && !iter->second.empty())
3068  {
3069  cv::Mat validProjMask;
3070  if(!projMask.empty())
3071  {
3072  if(projMask.type() != CV_8UC1)
3073  {
3074  if(!wrongMaskFormatWarned)
3075  UERROR("Wrong camera projection mask type %d, should be CV_8UC1", projMask.type());
3076  wrongMaskFormatWarned = true;
3077  }
3078  else if(projMask.cols == iter->second[0].imageWidth() * (int)iter->second.size() &&
3079  projMask.rows == iter->second[0].imageHeight())
3080  {
3081  validProjMask = projMask;
3082  }
3083  else
3084  {
3085  UWARN("Camera projection mask (%dx%d) is not valid for current "
3086  "camera model(s) (count=%ld, image size=%dx%d). It will be "
3087  "ignored for node %d",
3088  projMask.cols, projMask.rows,
3089  iter->second.size(),
3090  iter->second[0].imageWidth(),
3091  iter->second[0].imageHeight(),
3092  pter->first);
3093  }
3094  }
3095 
3096  for(size_t camIndex=0; camIndex<iter->second.size(); ++camIndex)
3097  {
3098  Transform cameraTransform = (pter->second * iter->second[camIndex].localTransform());
3099  UASSERT(!cameraTransform.isNull());
3100  cv::Mat cameraMatrixK = iter->second[camIndex].K();
3101  UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
3102  const cv::Size & imageSize = iter->second[camIndex].imageSize();
3103 
3104  float fx = cameraMatrixK.at<double>(0,0);
3105  float fy = cameraMatrixK.at<double>(1,1);
3106  float cx = cameraMatrixK.at<double>(0,2);
3107  float cy = cameraMatrixK.at<double>(1,2);
3108 
3109  // depth: 2 channels UINT: [depthMM, indexPt]
3110  cv::Mat registered = cv::Mat::zeros(imageSize, CV_32SC2);
3111  Transform t = cameraTransform.inverse();
3112 
3113  cv::Rect roi(0,0,imageSize.width, imageSize.height);
3114  if(roiRatios.size()==4)
3115  {
3116  roi = util2d::computeRoi(imageSize, roiRatios);
3117  }
3118 
3119  int count = 0;
3120  for(size_t i=0; i<cloud.size(); ++i)
3121  {
3122  // Get 3D from laser scan
3123  PointT ptScan = cloud.at(i);
3124  ptScan = util3d::transformPoint(ptScan, t);
3125 
3126  // re-project in camera frame
3127  float z = ptScan.z;
3128  bool set = false;
3129  if(z > 0.0f && (maxDistance<=0 || z<maxDistance))
3130  {
3131  float invZ = 1.0f/z;
3132  float dx = (fx*ptScan.x)*invZ + cx;
3133  float dy = (fy*ptScan.y)*invZ + cy;
3134  int dx_low = dx;
3135  int dy_low = dy;
3136  int dx_high = dx + 0.5f;
3137  int dy_high = dy + 0.5f;
3138  int zMM = z * 1000;
3139  if(uIsInBounds(dx_low, roi.x, roi.x+roi.width) && uIsInBounds(dy_low, roi.y, roi.y+roi.height) &&
3140  (validProjMask.empty() || validProjMask.at<unsigned char>(dy_low, imageSize.width*camIndex+dx_low) > 0))
3141  {
3142  set = true;
3143  cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_low, dx_low);
3144  if(zReg[0] == 0 || zMM < zReg[0])
3145  {
3146  zReg[0] = zMM;
3147  zReg[1] = i;
3148  }
3149  }
3150  if((dx_low != dx_high || dy_low != dy_high) &&
3151  uIsInBounds(dx_high, roi.x, roi.x+roi.width) && uIsInBounds(dy_high, roi.y, roi.y+roi.height) &&
3152  (validProjMask.empty() || validProjMask.at<unsigned char>(dy_high, imageSize.width*camIndex+dx_high) > 0))
3153  {
3154  set = true;
3155  cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_high, dx_high);
3156  if(zReg[0] == 0 || zMM < zReg[0])
3157  {
3158  zReg[0] = zMM;
3159  zReg[1] = i;
3160  }
3161  }
3162  }
3163  if(set)
3164  {
3165  count++;
3166  }
3167  }
3168  if(count == 0)
3169  {
3170  registered = cv::Mat();
3171  UINFO("No points projected in camera %d/%d", pter->first, camIndex);
3172  }
3173  else
3174  {
3175  UDEBUG("%d points projected in camera %d/%d", count, pter->first, camIndex);
3176  }
3177  for(int u=0; u<registered.cols; ++u)
3178  {
3179  for(int v=0; v<registered.rows; ++v)
3180  {
3181  cv::Vec2i &zReg = registered.at<cv::Vec2i>(v, u);
3182  if(zReg[0] > 0)
3183  {
3184  ProjectionInfo info;
3185  info.nodeID = pter->first;
3186  info.cameraIndex = camIndex;
3187  info.uv.x = float(u)/float(imageSize.width);
3188  info.uv.y = float(v)/float(imageSize.height);
3189  const Transform & cam = cameraPoses.at(info.nodeID);
3190  const PointT & pt = cloud.at(zReg[1]);
3191  Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
3192  Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
3193  float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
3194  float distanceToCam = zReg[0]/1000.0f;
3195  if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
3196  (maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
3197  {
3198  float vx = info.uv.x-0.5f;
3199  float vy = info.uv.y-0.5f;
3200 
3201  float distanceToCenter = vx*vx+vy*vy;
3202  float distance = distanceToCenter;
3203  if(distanceToCamPolicy)
3204  {
3205  distance = distanceToCam;
3206  }
3207 
3208  info.distance = distance;
3209 
3210  if(invertedIndex[zReg[1]].distance != -1.0f)
3211  {
3212  if(distance <= invertedIndex[zReg[1]].distance)
3213  {
3214  invertedIndex[zReg[1]] = info;
3215  }
3216  }
3217  else
3218  {
3219  invertedIndex[zReg[1]] = info;
3220  }
3221  }
3222  }
3223  }
3224  }
3225  }
3226  }
3227 
3228  msg = uFormat("Processed camera %d/%d", (int)cameraProcessed+1, (int)cameraPoses.size());
3229  UINFO(msg.c_str());
3230  if(state && !state->callback(msg))
3231  {
3232  //cancelled!
3233  UWARN("Projecting to cameras cancelled!");
3234  return pointToPixel;
3235  }
3236  ++cameraProcessed;
3237  }
3238 
3239  msg = uFormat("Select best camera for %d points...", (int)cloud.size());
3240  UINFO(msg.c_str());
3241  if(state && !state->callback(msg))
3242  {
3243  //cancelled!
3244  UWARN("Projecting to cameras cancelled!");
3245  return pointToPixel;
3246  }
3247 
3248  pointToPixel.resize(invertedIndex.size());
3249  int colorized = 0;
3250 
3251  // For each point
3252  for(size_t i=0; i<invertedIndex.size(); ++i)
3253  {
3254  int nodeID = -1;
3255  int cameraIndex = -1;
3256  pcl::PointXY uv_coords;
3257  if(invertedIndex[i].distance > -1.0f)
3258  {
3259  nodeID = invertedIndex[i].nodeID;
3260  cameraIndex = invertedIndex[i].cameraIndex;
3261  uv_coords = invertedIndex[i].uv;
3262  }
3263 
3264  if(nodeID>-1 && cameraIndex> -1)
3265  {
3266  pointToPixel[i].first.first = nodeID;
3267  pointToPixel[i].first.second = cameraIndex;
3268  pointToPixel[i].second = uv_coords;
3269  ++colorized;
3270  }
3271  }
3272 
3273  msg = uFormat("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, colorized*100/cloud.size());
3274  UINFO(msg.c_str());
3275  if(state)
3276  {
3277  state->callback(msg);
3278  }
3279 
3280  return pointToPixel;
3281 }
3282 
3283 std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCameras (
3284  const typename pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
3285  const std::map<int, Transform> & cameraPoses,
3286  const std::map<int, std::vector<CameraModel> > & cameraModels,
3287  float maxDistance,
3288  float maxAngle,
3289  const std::vector<float> & roiRatios,
3290  const cv::Mat & projMask,
3291  bool distanceToCamPolicy,
3292  const ProgressState * state)
3293 {
3294  return projectCloudToCamerasImpl(cloud,
3295  cameraPoses,
3296  cameraModels,
3297  maxDistance,
3298  maxAngle,
3299  roiRatios,
3300  projMask,
3301  distanceToCamPolicy,
3302  state);
3303 }
3304 
3305 std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCameras (
3306  const typename pcl::PointCloud<pcl::PointXYZINormal> & cloud,
3307  const std::map<int, Transform> & cameraPoses,
3308  const std::map<int, std::vector<CameraModel> > & cameraModels,
3309  float maxDistance,
3310  float maxAngle,
3311  const std::vector<float> & roiRatios,
3312  const cv::Mat & projMask,
3313  bool distanceToCamPolicy,
3314  const ProgressState * state)
3315 {
3316  return projectCloudToCamerasImpl(cloud,
3317  cameraPoses,
3318  cameraModels,
3319  maxDistance,
3320  maxAngle,
3321  roiRatios,
3322  projMask,
3323  distanceToCamPolicy,
3324  state);
3325 }
3326 
3327 bool isFinite(const cv::Point3f & pt)
3328 {
3329  return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z);
3330 }
3331 
3332 pcl::PointCloud<pcl::PointXYZ>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds)
3333 {
3334  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
3335  for(std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
3336  {
3337  *cloud += *(*iter);
3338  }
3339  return cloud;
3340 }
3341 
3342 pcl::PointCloud<pcl::PointXYZRGB>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds)
3343 {
3344  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
3345  for(std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
3346  {
3347  *cloud+=*(*iter);
3348  }
3349  return cloud;
3350 }
3351 
3352 pcl::IndicesPtr concatenate(const std::vector<pcl::IndicesPtr> & indices)
3353 {
3354  //compute total size
3355  unsigned int totalSize = 0;
3356  for(unsigned int i=0; i<indices.size(); ++i)
3357  {
3358  totalSize += (unsigned int)indices[i]->size();
3359  }
3360  pcl::IndicesPtr ind(new std::vector<int>(totalSize));
3361  unsigned int io = 0;
3362  for(unsigned int i=0; i<indices.size(); ++i)
3363  {
3364  for(unsigned int j=0; j<indices[i]->size(); ++j)
3365  {
3366  ind->at(io++) = indices[i]->at(j);
3367  }
3368  }
3369  return ind;
3370 }
3371 
3372 pcl::IndicesPtr concatenate(const pcl::IndicesPtr & indicesA, const pcl::IndicesPtr & indicesB)
3373 {
3374  pcl::IndicesPtr ind(new std::vector<int>(*indicesA));
3375  ind->resize(ind->size()+indicesB->size());
3376  unsigned int oi = (unsigned int)indicesA->size();
3377  for(unsigned int i=0; i<indicesB->size(); ++i)
3378  {
3379  ind->at(oi++) = indicesB->at(i);
3380  }
3381  return ind;
3382 }
3383 
3384 void savePCDWords(
3385  const std::string & fileName,
3386  const std::multimap<int, pcl::PointXYZ> & words,
3387  const Transform & transform)
3388 {
3389  if(words.size())
3390  {
3391  pcl::PointCloud<pcl::PointXYZ> cloud;
3392  cloud.resize(words.size());
3393  int i=0;
3394  for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
3395  {
3396  cloud[i++] = transformPoint(iter->second, transform);
3397  }
3398  pcl::io::savePCDFile(fileName, cloud);
3399  }
3400 }
3401 
3402 void savePCDWords(
3403  const std::string & fileName,
3404  const std::multimap<int, cv::Point3f> & words,
3405  const Transform & transform)
3406 {
3407  if(words.size())
3408  {
3409  pcl::PointCloud<pcl::PointXYZ> cloud;
3410  cloud.resize(words.size());
3411  int i=0;
3412  for(std::multimap<int, cv::Point3f>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
3413  {
3414  cv::Point3f pt = transformPoint(iter->second, transform);
3415  cloud[i++] = pcl::PointXYZ(pt.x, pt.y, pt.z);
3416  }
3417  pcl::io::savePCDFile(fileName, cloud);
3418  }
3419 }
3420 
3421 cv::Mat loadBINScan(const std::string & fileName)
3422 {
3423  cv::Mat output;
3424  long bytes = UFile::length(fileName);
3425  if(bytes)
3426  {
3427  int dim = 4;
3428  UASSERT(bytes % sizeof(float) == 0);
3429  size_t num = bytes/sizeof(float);
3430  UASSERT(num % dim == 0);
3431  output = cv::Mat(1, num/dim, CV_32FC(dim));
3432 
3433  // load point cloud
3434  FILE *stream;
3435  stream = fopen (fileName.c_str(),"rb");
3436  size_t actualReadNum = fread(output.data,sizeof(float),num,stream);
3437  UASSERT(num == actualReadNum);
3438  fclose(stream);
3439  }
3440 
3441  return output;
3442 }
3443 
3444 pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName)
3445 {
3446  return laserScanToPointCloud(loadScan(fileName));
3447 }
3448 
3449 pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, int dim)
3450 {
3451  return loadBINCloud(fileName);
3452 }
3453 
3454 LaserScan loadScan(const std::string & path)
3455 {
3456  std::string fileName = UFile::getName(path);
3457  if(UFile::getExtension(fileName).compare("bin") == 0)
3458  {
3459  return LaserScan(loadBINScan(path), 0, 0, LaserScan::kXYZI);
3460  }
3461  else
3462  {
3463  pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
3464 
3465  if(UFile::getExtension(fileName).compare("pcd") == 0)
3466  {
3467  pcl::io::loadPCDFile(path, *cloud);
3468  }
3469  else // PLY
3470  {
3471  pcl::io::loadPLYFile(path, *cloud);
3472  }
3473  if(cloud->height > 1)
3474  {
3475  cloud->is_dense = false;
3476  }
3477 
3478  bool is2D = false;
3479  if(!cloud->data.empty())
3480  {
3481  // If all z values are zeros, we assume it is a 2D scan
3482  int zOffset = -1;
3483  for(unsigned int i=0; i<cloud->fields.size(); ++i)
3484  {
3485  if(cloud->fields[i].name.compare("z") == 0)
3486  {
3487  zOffset = cloud->fields[i].offset;
3488  break;
3489  }
3490  }
3491  if(zOffset>=0)
3492  {
3493  is2D = true;
3494  for (uint32_t row = 0; row < (uint32_t)cloud->height && is2D; ++row)
3495  {
3496  const uint8_t* row_data = &cloud->data[row * cloud->row_step];
3497  for (uint32_t col = 0; col < (uint32_t)cloud->width && is2D; ++col)
3498  {
3499  const uint8_t* msg_data = row_data + col * cloud->point_step;
3500  float z = *(float*)(msg_data + zOffset);
3501  is2D = z == 0.0f;
3502  }
3503  }
3504  }
3505  }
3506 
3507  return laserScanFromPointCloud(*cloud, true, is2D);
3508  }
3509  return LaserScan();
3510 }
3511 
3512 pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
3513  const std::string & path,
3514  const Transform & transform,
3515  int downsampleStep,
3516  float voxelSize)
3517 {
3518  UASSERT(!transform.isNull());
3519  UDEBUG("Loading cloud (step=%d, voxel=%f m) : %s", downsampleStep, voxelSize, path.c_str());
3520  std::string fileName = UFile::getName(path);
3521  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
3522  if(UFile::getExtension(fileName).compare("bin") == 0)
3523  {
3524  cloud = util3d::loadBINCloud(path); // Assume KITTI velodyne format
3525  }
3526  else if(UFile::getExtension(fileName).compare("pcd") == 0)
3527  {
3528  pcl::io::loadPCDFile(path, *cloud);
3529  }
3530  else
3531  {
3532  pcl::io::loadPLYFile(path, *cloud);
3533  }
3534  int previousSize = (int)cloud->size();
3535  if(downsampleStep > 1 && cloud->size())
3536  {
3537  cloud = util3d::downsample(cloud, downsampleStep);
3538  UDEBUG("Downsampling scan (step=%d): %d -> %d", downsampleStep, previousSize, (int)cloud->size());
3539  }
3540  previousSize = (int)cloud->size();
3541  if(voxelSize > 0.0f && cloud->size())
3542  {
3543  cloud = util3d::voxelize(cloud, voxelSize);
3544  UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", voxelSize, previousSize, (int)cloud->size());
3545  }
3546  if(transform.isIdentity())
3547  {
3548  return cloud;
3549  }
3550  return util3d::transformPointCloud(cloud, transform);
3551 }
3552 
3553 LaserScan deskew(
3554  const LaserScan & input,
3555  double inputStamp,
3556  const rtabmap::Transform & velocity)
3557 {
3558  if(velocity.isNull())
3559  {
3560  UERROR("velocity should be valid!");
3561  return LaserScan();
3562  }
3563 
3564  if(input.format() != LaserScan::kXYZIT)
3565  {
3566  UERROR("input scan doesn't have \"time\" channel! Only format \"%s\" supported yet.", LaserScan::formatName(LaserScan::kXYZIT).c_str());
3567  return LaserScan();
3568  }
3569 
3570  if(input.empty())
3571  {
3572  UERROR("input scan is empty!");
3573  return LaserScan();
3574  }
3575 
3576  int offsetTime = input.getTimeOffset();
3577 
3578  // Get latest timestamp
3579  double firstStamp;
3580  double lastStamp;
3581  firstStamp = inputStamp + input.data().ptr<float>(0, 0)[offsetTime];
3582  lastStamp = inputStamp + input.data().ptr<float>(0, input.size()-1)[offsetTime];
3583 
3584  if(lastStamp <= firstStamp)
3585  {
3586  UERROR("First and last stamps in the scan are the same!");
3587  return LaserScan();
3588  }
3589 
3590  rtabmap::Transform firstPose;
3591  rtabmap::Transform lastPose;
3592 
3593  float vx,vy,vz, vroll,vpitch,vyaw;
3594  velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
3595 
3596  // 1- The pose of base frame in odom frame at first stamp
3597  // 2- The pose of base frame in odom frame at last stamp
3598  double dt1 = firstStamp - inputStamp;
3599  double dt2 = lastStamp - inputStamp;
3600 
3601  firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
3602  lastPose = rtabmap::Transform(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);
3603 
3604  if(firstPose.isNull())
3605  {
3606  UERROR("Could not get transform between stamps %f and %f!",
3607  firstStamp,
3608  inputStamp);
3609  return LaserScan();
3610  }
3611  if(lastPose.isNull())
3612  {
3613  UERROR("Could not get transform between stamps %f and %f!",
3614  lastStamp,
3615  inputStamp);
3616  return LaserScan();
3617  }
3618 
3619  double stamp;
3620  UTimer processingTime;
3621  double scanTime = lastStamp - firstStamp;
3622  cv::Mat output(1, input.size(), CV_32FC4); // XYZI - Dense
3623  int offsetIntensity = input.getIntensityOffset();
3624  bool isLocalTransformIdentity = input.localTransform().isIdentity();
3625  Transform localTransformInv = input.localTransform().inverse();
3626 
3627  bool timeOnColumns = input.data().cols > input.data().rows;
3628  int oi = 0;
3629  if(timeOnColumns)
3630  {
3631  // t1 t2 ...
3632  // ring1 ring1 ...
3633  // ring2 ring2 ...
3634  // ring3 ring4 ...
3635  // ring4 ring3 ...
3636  for(int u=0; u<input.data().cols; ++u)
3637  {
3638  const float * inputPtr = input.data().ptr<float>(0, u);
3639  stamp = inputStamp + inputPtr[offsetTime];
3640  rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);
3641 
3642  for(int v=0; v<input.data().rows; ++v)
3643  {
3644  inputPtr = input.data().ptr<float>(v, u);
3645  pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]);
3646  if(pcl::isFinite(pt))
3647  {
3648  if(!isLocalTransformIdentity)
3649  {
3650  pt = rtabmap::util3d::transformPoint(pt, input.localTransform());
3651  }
3652  pt = rtabmap::util3d::transformPoint(pt, transform);
3653  if(!isLocalTransformIdentity)
3654  {
3655  pt = rtabmap::util3d::transformPoint(pt, localTransformInv);
3656  }
3657  float * dataPtr = output.ptr<float>(0, oi++);
3658  dataPtr[0] = pt.x;
3659  dataPtr[1] = pt.y;
3660  dataPtr[2] = pt.z;
3661  dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
3662  }
3663  }
3664  }
3665  }
3666  else // time on rows
3667  {
3668  // t1 ring1 ring2 ring3 ring4
3669  // t2 ring1 ring2 ring3 ring4
3670  // t3 ring1 ring2 ring3 ring4
3671  // t4 ring1 ring2 ring3 ring4
3672  // ... ... ... ... ...
3673  for(int v=0; v<input.data().rows; ++v)
3674  {
3675  const float * inputPtr = input.data().ptr<float>(v, 0);
3676  stamp = inputStamp + inputPtr[offsetTime];
3677  rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);
3678 
3679  for(int u=0; u<input.data().cols; ++u)
3680  {
3681  inputPtr = input.data().ptr<float>(v, u);
3682  pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]);
3683  if(pcl::isFinite(pt))
3684  {
3685  if(!isLocalTransformIdentity)
3686  {
3687  pt = rtabmap::util3d::transformPoint(pt, input.localTransform());
3688  }
3689  pt = rtabmap::util3d::transformPoint(pt, transform);
3690  if(!isLocalTransformIdentity)
3691  {
3692  pt = rtabmap::util3d::transformPoint(pt, localTransformInv);
3693  }
3694  float * dataPtr = output.ptr<float>(0, oi++);
3695  dataPtr[0] = pt.x;
3696  dataPtr[1] = pt.y;
3697  dataPtr[2] = pt.z;
3698  dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
3699  }
3700  }
3701  }
3702  }
3703  output = cv::Mat(output, cv::Range::all(), cv::Range(0, oi));
3704  UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed());
3705  return LaserScan(output, input.maxPoints(), input.rangeMax(), LaserScan::kXYZI, input.localTransform());
3706 }
3707 
3708 
3709 }
3710 
3711 }
rtabmap::SensorData
Definition: SensorData.h:51
ind
std::vector< int > ind
w
RowVector3d w
rtabmap::util3d::ProjectionInfo::nodeID
int nodeID
Definition: util3d.cpp:3017
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
int
int
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
rtabmap::util3d::laserScanToPointCloudINormal
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudINormal(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2402
rtabmap::util3d::cloudFromDepthRGB
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(const cv::Mat &imageRgb, const cv::Mat &imageDepth, float cx, float cy, float fx, float fy, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:416
compare
bool compare
rtabmap::util2d::computeRoi
cv::Rect RTABMAP_CORE_EXPORT computeRoi(const cv::Mat &image, const std::string &roiRatios)
Definition: util2d.cpp:1113
UFile::getName
std::string getName()
Definition: UFile.h:135
UINFO
#define UINFO(...)
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
cy
const double cy
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2230
rtabmap::LaserScan::getTimeOffset
int getTimeOffset() const
Definition: LaserScan.h:146
util3d_surface.h
bytes
set
rtabmap::util3d::laserScanToPointI
pcl::PointXYZI RTABMAP_CORE_EXPORT laserScanToPointI(const LaserScan &laserScan, int index, float intensity)
Definition: util3d.cpp:2505
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:120
rtabmap::util2d::interpolate
cv::Mat RTABMAP_CORE_EXPORT interpolate(const cv::Mat &image, int factor, float depthErrorRatio=0.02f)
Definition: util2d.cpp:1250
rtabmap::LaserScan::kXYZ
@ kXYZ
Definition: LaserScan.h:45
Eigen::Transform
rtabmap::util3d::rgbdFromCloud
void RTABMAP_CORE_EXPORT rgbdFromCloud(const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, cv::Mat &rgb, cv::Mat &depth, float &fx, float &fy, bool bgrOrder=true, bool depth16U=true)
Definition: util3d.cpp:138
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:122
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:135
b
Array< int, 3, 1 > b
rtabmap::util3d::ProjectionInfo::distance
float distance
Definition: util3d.cpp:3020
stream
stream
UFile::length
long length()
Definition: UFile.h:110
fx
const double fx
rtabmap::util3d::cloudFromStereoImages
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudFromStereoImages(const cv::Mat &imageLeft, const cv::Mat &imageRight, const StereoCameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &parameters=ParametersMap())
Definition: util3d.cpp:814
c
Scalar Scalar * c
h
const double h
rtabmap::util3d::ProjectionInfo
Definition: util3d.cpp:3010
uint32_t
::uint32_t uint32_t
size
Index size
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::util3d::ProjectionInfo::uv
pcl::PointXY uv
Definition: util3d.cpp:3019
rtabmap::util3d::fillProjectedCloudHoles
void RTABMAP_CORE_EXPORT fillProjectedCloudHoles(cv::Mat &depthRegistered, bool verticalDirection, bool fillToBorder)
Definition: util3d.cpp:2927
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:116
rtabmap::LaserScan::kXYZIT
@ kXYZIT
Definition: LaserScan.h:51
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:613
rtabmap::util3d::projectCloudToCameras
std::vector< std::pair< std::pair< int, int >, pcl::PointXY > > RTABMAP_CORE_EXPORT projectCloudToCameras(const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, const std::map< int, Transform > &cameraPoses, const std::map< int, std::vector< CameraModel > > &cameraModels, float maxDistance=0.0f, float maxAngle=0.0f, const std::vector< float > &roiRatios=std::vector< float >(), const cv::Mat &projMask=cv::Mat(), bool distanceToCamPolicy=false, const ProgressState *state=0)
count
Index count
rtabmap::LaserScan::getNormalsOffset
int getNormalsOffset() const
Definition: LaserScan.h:145
col
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ColXpr col(Index i) const
state
RecoveryProgressState state
Definition: tools/Recovery/main.cpp:56
rtabmap::LaserScan::kXYZI
@ kXYZI
Definition: LaserScan.h:46
y
Matrix3f y
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:134
rtabmap::util3d::concatenate
pcl::IndicesPtr RTABMAP_CORE_EXPORT concatenate(const std::vector< pcl::IndicesPtr > &indices)
Concatenate a vector of indices to a single vector.
Definition: util3d.cpp:3352
util3d.h
rtabmap::util2d::disparityFromStereoImages
cv::Mat RTABMAP_CORE_EXPORT disparityFromStereoImages(const cv::Mat &leftImage, const cv::Mat &rightImage, const ParametersMap &parameters=ParametersMap())
Definition: util2d.cpp:738
rtabmap::LaserScan
Definition: LaserScan.h:37
rtabmap::util3d::bgrFromCloud
cv::Mat bgrFromCloud(const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, bool bgrOrder)
Definition: util3d.cpp:52
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
UTimer.h
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:131
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::util3d::projectDepthTo3DRay
Eigen::Vector3f RTABMAP_CORE_EXPORT projectDepthTo3DRay(const cv::Size &imageSize, float x, float y, float cx, float cy, float fx, float fy)
Definition: util3d.cpp:246
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:240
cam
rtabmap::SensorCaptureThread * cam
Definition: tools/DataRecorder/main.cpp:58
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:130
UMath.h
Basic mathematics functions.
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:241
indices
indices
rtabmap::util3d::loadScan
LaserScan RTABMAP_CORE_EXPORT loadScan(const std::string &path)
Definition: util3d.cpp:3454
range
Double_ range(const Point2_ &p, const Point2_ &q)
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:117
rtabmap::LaserScan::kXYI
@ kXYI
Definition: LaserScan.h:42
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::LaserScan::kXYZRGB
@ kXYZRGB
Definition: LaserScan.h:47
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1268
glm::uint32_t
detail::uint32 uint32_t
Definition: fwd.hpp:916
util3d_transforms.h
vy
StridedVectorType vy(make_vector(y, *n, std::abs(*incy)))
g
float g
j
std::ptrdiff_t j
rtabmap::util3d::projectCloudToCamera
cv::Mat RTABMAP_CORE_EXPORT projectCloudToCamera(const cv::Size &imageSize, const cv::Mat &cameraMatrixK, const cv::Mat &laserScan, const rtabmap::Transform &cameraTransform)
Definition: util3d.cpp:2693
UConversion.h
Some conversion functions.
rtabmap::util3d::laserScanToPoint
pcl::PointXYZ RTABMAP_CORE_EXPORT laserScanToPoint(const LaserScan &laserScan, int index)
Definition: util3d.cpp:2428
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
util3d_filtering.h
rtabmap::util3d::depthFromCloud
cv::Mat RTABMAP_CORE_EXPORT depthFromCloud(const pcl::PointCloud< pcl::PointXYZRGBA > &cloud, float &fx, float &fy, bool depth16U=true)
Definition: util3d.cpp:78
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2322
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:133
rtabmap::util3d::deskew
LaserScan RTABMAP_CORE_EXPORT deskew(const LaserScan &input, double inputStamp, const rtabmap::Transform &velocity)
Lidar deskewing.
Definition: util3d.cpp:3553
rtabmap::util3d::ProjectionInfo::ProjectionInfo
ProjectionInfo()
Definition: util3d.cpp:3012
all
static const Eigen::internal::all_t all
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
Definition: SensorData.h:211
rtabmap::Transform::interpolate
Transform interpolate(float t, const Transform &other) const
Definition: Transform.cpp:303
rtabmap::util3d::isFinite
bool RTABMAP_CORE_EXPORT isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3327
rtabmap::util3d::ProjectionInfo::cameraIndex
int cameraIndex
Definition: util3d.cpp:3018
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
velocity
Velocity3 velocity(const NavState &X, OptionalJacobian< 3, 9 > H)
glm::row
GLM_FUNC_DECL genType::row_type row(genType const &m, length_t const &index)
rtabmap::LaserScan::kXY
@ kXY
Definition: LaserScan.h:41
rtabmap::util3d::downsample
LaserScan RTABMAP_CORE_EXPORT downsample(const LaserScan &cloud, int step)
Definition: util3d_filtering.cpp:398
rtabmap::util3d::laserScan2dFromPointCloud
LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:1992
info
else if n * info
rtabmap::util3d::loadBINCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT loadBINCloud(const std::string &fileName)
Definition: util3d.cpp:3444
UASSERT
#define UASSERT(condition)
d
d
rtabmap::ProgressState
Definition: ProgressState.h:35
rtabmap::util3d::concatenateClouds
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT concatenateClouds(const std::list< pcl::PointCloud< pcl::PointXYZ >::Ptr > &clouds)
Definition: util3d.cpp:3332
z
z
RecoveryProgressState::callback
virtual bool callback(const std::string &msg) const
Definition: tools/Recovery/main.cpp:49
x
x
rtabmap::util3d::laserScanToPointCloudI
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2349
rtabmap::util3d::laserScanToPointRGBNormal
pcl::PointXYZRGBNormal RTABMAP_CORE_EXPORT laserScanToPointRGBNormal(const LaserScan &laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
Definition: util3d.cpp:2531
rtabmap::util3d::projectDepthTo3D
pcl::PointXYZ RTABMAP_CORE_EXPORT projectDepthTo3D(const cv::Mat &depthImage, float x, float y, float cx, float cy, float fx, float fy, bool smoothing, float depthErrorRatio=0.02f)
Definition: util3d.cpp:215
rtabmap::util2d::getDepth
float RTABMAP_CORE_EXPORT getDepth(const cv::Mat &depthImage, float x, float y, bool smoothing, float depthErrorRatio=0.02f, bool estWithNeighborsIfNull=false)
Definition: util2d.cpp:947
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:932
rtabmap::util3d::cloudsRGBFromSensorData
std::vector< pcl::PointCloud< pcl::PointXYZRGB >::Ptr > RTABMAP_CORE_EXPORT cloudsRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< pcl::IndicesPtr > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1093
normal
Unit3_ normal(const OrientedPlane3_ &p)
rtabmap::LaserScan::getRGBOffset
int getRGBOffset() const
Definition: LaserScan.h:144
rtabmap::util3d::laserScanToPointNormal
pcl::PointNormal RTABMAP_CORE_EXPORT laserScanToPointNormal(const LaserScan &laserScan, int index)
Definition: util3d.cpp:2443
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::util3d::laserScanToPointRGB
pcl::PointXYZRGB RTABMAP_CORE_EXPORT laserScanToPointRGB(const LaserScan &laserScan, int index, unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2465
rtabmap::LaserScan::getIntensityOffset
int getIntensityOffset() const
Definition: LaserScan.h:143
offset
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate offset
path
path
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
rtabmap::util3d::cloudFromSensorData
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1040
util2d.h
uint8_t
::uint8_t uint8_t
rtabmap::util3d::laserScanToPointINormal
pcl::PointXYZINormal RTABMAP_CORE_EXPORT laserScanToPointINormal(const LaserScan &laserScan, int index, float intensity)
Definition: util3d.cpp:2579
W
Key W(std::uint64_t j)
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:210
iter
iterator iter(handle obj)
rtabmap::LaserScan::kXYINormal
@ kXYINormal
Definition: LaserScan.h:44
glm::abs
GLM_FUNC_DECL genType abs(genType const &x)
rtabmap::util3d::cloudFromDisparityRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudFromDisparityRGB(const cv::Mat &imageRgb, const cv::Mat &imageDisparity, const StereoCameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:713
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
dim
dim
c_str
const char * c_str(Args &&...args)
rtabmap::LaserScan::isOrganized
bool isOrganized() const
Definition: LaserScan.h:139
rtabmap::util3d::savePCDWords
void RTABMAP_CORE_EXPORT savePCDWords(const std::string &fileName, const std::multimap< int, pcl::PointXYZ > &words, const Transform &transform=Transform::getIdentity())
Definition: util3d.cpp:3384
rtabmap::LaserScan::empty
bool empty() const
Definition: LaserScan.h:129
v
Array< int, Dynamic, 1 > v
UDEBUG
#define UDEBUG(...)
rtabmap::util3d::removeNaNFromPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
Definition: util3d_filtering.cpp:1002
rtabmap::util3d::projectDisparityTo3D
cv::Point3f RTABMAP_CORE_EXPORT projectDisparityTo3D(const cv::Point2f &pt, float disparity, const StereoCameraModel &model)
Definition: util3d.cpp:2653
UTimer
Definition: UTimer.h:46
float
float
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2269
distance
Double_ distance(const OrientedPlane3_ &p)
rtabmap::util3d::cloudFromDepth
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(const cv::Mat &imageDepth, float cx, float cy, float fx, float fy, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:266
rtabmap::util3d::getMinMax3D
void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2613
rtabmap::util3d::projectCloudToCamerasImpl
std::vector< std::pair< std::pair< int, int >, pcl::PointXY > > projectCloudToCamerasImpl(const typename pcl::PointCloud< PointT > &cloud, const std::map< int, Transform > &cameraPoses, const std::map< int, std::vector< CameraModel > > &cameraModels, float maxDistance, float maxAngle, const std::vector< float > &roiRatios, const cv::Mat &projMask, bool distanceToCamPolicy, const ProgressState *state)
Definition: util3d.cpp:3028
rtabmap::util3d::loadBINScan
cv::Mat RTABMAP_CORE_EXPORT loadBINScan(const std::string &fileName)
Definition: util3d.cpp:3421
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:136
transform
EIGEN_DONT_INLINE void transform(const Quaternion< Scalar > &t, Data &data)
rtabmap::LaserScan::kXYZRGBNormal
@ kXYZRGBNormal
Definition: LaserScan.h:50
rtabmap::util3d::loadCloud
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT loadCloud(const std::string &path, const Transform &transform=Transform::getIdentity(), int downsampleStep=1, float voxelSize=0.0f)
Definition: util3d.cpp:3512
cx
const double cx
cols
int cols
rtabmap::LaserScan::formatName
std::string formatName() const
Definition: LaserScan.h:118
rtabmap::LaserScan::kXYNormal
@ kXYNormal
Definition: LaserScan.h:43
t
Point2 t(10, 10)
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2376
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::LaserScan::isCompressed
bool isCompressed() const
Definition: LaserScan.h:138
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2296
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
i
int i
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:53
rtabmap::util3d::laserScanFromDepthImages
pcl::PointCloud< pcl::PointXYZ > RTABMAP_CORE_EXPORT laserScanFromDepthImages(const cv::Mat &depthImages, const std::vector< CameraModel > &cameraModels, float maxDepth, float minDepth)
Definition: util3d.cpp:1357
rtabmap::util3d::laserScanFromDepthImage
pcl::PointCloud< pcl::PointXYZ > RTABMAP_CORE_EXPORT laserScanFromDepthImage(const cv::Mat &depthImage, float fx, float fy, float cx, float cy, float maxDepth=0, float minDepth=0, const Transform &localTransform=Transform::getIdentity())
Definition: util3d.cpp:1321
rtabmap::LaserScan::kXYZINormal
@ kXYZINormal
Definition: LaserScan.h:49
fy
const double fy
vx
StridedVectorType vx(make_vector(x, *n, std::abs(*incx)))
rtabmap::util3d::cloudsFromSensorData
std::vector< pcl::PointCloud< pcl::PointXYZ >::Ptr > RTABMAP_CORE_EXPORT cloudsFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< pcl::IndicesPtr > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:854
rtabmap::util3d::cloudFromDisparity
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT cloudFromDisparity(const cv::Mat &imageDisparity, const StereoCameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:612
rtabmap::LaserScan::kXYZNormal
@ kXYZNormal
Definition: LaserScan.h:48
msg
msg

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jul 1 2024 02:42:41