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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:37:06