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)
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  pcl::uint32_t fieldOffsets[8] = {0};
1275  for(unsigned int i=0; i<cloud.fields.size(); ++i)
1276  {
1277  if(cloud.fields[i].name.compare("x") == 0)
1278  {
1279  fieldStates[0] = 1;
1280  fieldOffsets[0] = cloud.fields[i].offset;
1281  }
1282  else if(cloud.fields[i].name.compare("y") == 0)
1283  {
1284  fieldStates[1] = 1;
1285  fieldOffsets[1] = cloud.fields[i].offset;
1286  }
1287  else if(cloud.fields[i].name.compare("z") == 0)
1288  {
1289  fieldStates[2] = 1;
1290  fieldOffsets[2] = cloud.fields[i].offset;
1291  }
1292  else if(cloud.fields[i].name.compare("normal_x") == 0)
1293  {
1294  fieldStates[3] = 1;
1295  fieldOffsets[3] = cloud.fields[i].offset;
1296  }
1297  else if(cloud.fields[i].name.compare("normal_y") == 0)
1298  {
1299  fieldStates[4] = 1;
1300  fieldOffsets[4] = cloud.fields[i].offset;
1301  }
1302  else if(cloud.fields[i].name.compare("normal_z") == 0)
1303  {
1304  fieldStates[5] = 1;
1305  fieldOffsets[5] = cloud.fields[i].offset;
1306  }
1307  else if(cloud.fields[i].name.compare("rgb") == 0 || cloud.fields[i].name.compare("rgba") == 0)
1308  {
1309  fieldStates[6] = 1;
1310  fieldOffsets[6] = cloud.fields[i].offset;
1311  }
1312  else if(cloud.fields[i].name.compare("intensity") == 0)
1313  {
1314  fieldStates[7] = 1;
1315  fieldOffsets[7] = cloud.fields[i].offset;
1316  }
1317  else
1318  {
1319  UDEBUG("Ignoring \"%s\" field", cloud.fields[i].name.c_str());
1320  }
1321  }
1322  if(fieldStates[0]==0 || fieldStates[1]==0)
1323  {
1324  //should have at least x and y set
1325  UERROR("Cloud has not corresponding fields to laser scan!");
1326  return LaserScan();
1327  }
1328 
1329  bool hasNormals = fieldStates[3] || fieldStates[4] || fieldStates[5];
1330  bool hasIntensity = fieldStates[7];
1331  bool hasRGB = !hasIntensity&&fieldStates[6];
1332  bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2];
1333 
1334  LaserScan::Format format;
1335  if(is3D)
1336  {
1337  if(hasNormals && hasIntensity)
1338  {
1339  format = LaserScan::kXYZINormal;
1340  }
1341  else if(hasNormals && hasRGB)
1342  {
1343  format = LaserScan::kXYZRGBNormal;
1344  }
1345  else if(!hasNormals && hasIntensity)
1346  {
1347  format = LaserScan::kXYZI;
1348  }
1349  else if(!hasNormals && hasRGB)
1350  {
1351  format = LaserScan::kXYZRGB;
1352  }
1353  else
1354  {
1355  format = LaserScan::kXYZ;
1356  }
1357  }
1358  else
1359  {
1360  if(hasNormals && hasIntensity)
1361  {
1362  format = LaserScan::kXYINormal;
1363  }
1364  else if(!hasNormals && hasIntensity)
1365  {
1366  format = LaserScan::kXYI;
1367  }
1368  else
1369  {
1370  format = LaserScan::kXY;
1371  }
1372  }
1373 
1374  UASSERT(cloud.data.size()/cloud.point_step == cloud.height*cloud.width);
1375  cv::Mat laserScan = cv::Mat(1, (int)cloud.data.size()/cloud.point_step, CV_32FC(LaserScan::channels(format)));
1376 
1377  int oi=0;
1378  for (uint32_t row = 0; row < cloud.height; ++row)
1379  {
1380  const uint8_t* row_data = &cloud.data[row * cloud.row_step];
1381  for (uint32_t col = 0; col < cloud.width; ++col)
1382  {
1383  const uint8_t* msg_data = row_data + col * cloud.point_step;
1384 
1385  float * ptr = laserScan.ptr<float>(0, oi);
1386 
1387  bool valid = true;
1388  if(laserScan.channels() == 2)
1389  {
1390  ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
1391  ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
1392  valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]);
1393  }
1394  else if(laserScan.channels() == 3)
1395  {
1396  ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
1397  ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
1398  if(format == LaserScan::kXYI)
1399  {
1400  ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
1401  }
1402  else // XYZ
1403  {
1404  ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
1405  valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]);
1406  }
1407  }
1408  else if(laserScan.channels() == 4)
1409  {
1410  ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
1411  ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
1412  ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
1413  if(format == LaserScan::kXYZI)
1414  {
1415  ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
1416  }
1417  else // XYZRGB
1418  {
1419  pcl::uint8_t b=*(msg_data + fieldOffsets[6]);
1420  pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1);
1421  pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2);
1422  int * ptrInt = (int*)ptr;
1423  ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16);
1424  }
1425  valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]);
1426  }
1427  else if(laserScan.channels() == 5)
1428  {
1429  ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
1430  ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
1431  ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
1432  ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
1433  ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
1434  valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]);
1435  }
1436  else if(laserScan.channels() == 6)
1437  {
1438  ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
1439  ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
1440  if(format == LaserScan::kXYINormal)
1441  {
1442  ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
1443  }
1444  else // XYZNormal
1445  {
1446  ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
1447  }
1448  ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
1449  ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
1450  ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
1451  valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]);
1452  }
1453  else if(laserScan.channels() == 7)
1454  {
1455  ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
1456  ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
1457  ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
1458  if(format == LaserScan::kXYZINormal)
1459  {
1460  ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
1461  }
1462  else // XYZRGBNormal
1463  {
1464  pcl::uint8_t b=*(msg_data + fieldOffsets[6]);
1465  pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1);
1466  pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2);
1467  int * ptrInt = (int*)ptr;
1468  ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16);
1469  }
1470  ptr[4] = *(float*)(msg_data + fieldOffsets[3]);
1471  ptr[5] = *(float*)(msg_data + fieldOffsets[4]);
1472  ptr[6] = *(float*)(msg_data + fieldOffsets[5]);
1473  valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]) && uIsFinite(ptr[6]);
1474  }
1475  else
1476  {
1477  UFATAL("Cannot handle as many channels (%d)!", laserScan.channels());
1478  }
1479 
1480  if(!filterNaNs || valid)
1481  {
1482  ++oi;
1483  }
1484  }
1485  }
1486  if(oi == 0)
1487  {
1488  return LaserScan();
1489  }
1490  return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0, format);
1491 }
1492 
1493 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
1494 {
1495  return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
1496 }
1497 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
1498 {
1499  cv::Mat laserScan;
1500  bool nullTransform = transform.isNull() || transform.isIdentity();
1501  Eigen::Affine3f transform3f = transform.toEigen3f();
1502  int oi = 0;
1503  if(indices.get())
1504  {
1505  laserScan = cv::Mat(1, (int)indices->size(), CV_32FC3);
1506  for(unsigned int i=0; i<indices->size(); ++i)
1507  {
1508  int index = indices->at(i);
1509  if(!filterNaNs || pcl::isFinite(cloud.at(index)))
1510  {
1511  float * ptr = laserScan.ptr<float>(0, oi++);
1512  if(!nullTransform)
1513  {
1514  pcl::PointXYZ pt = pcl::transformPoint(cloud.at(index), transform3f);
1515  ptr[0] = pt.x;
1516  ptr[1] = pt.y;
1517  ptr[2] = pt.z;
1518  }
1519  else
1520  {
1521  ptr[0] = cloud.at(index).x;
1522  ptr[1] = cloud.at(index).y;
1523  ptr[2] = cloud.at(index).z;
1524  }
1525  }
1526  }
1527  }
1528  else
1529  {
1530  laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC3);
1531  for(unsigned int i=0; i<cloud.size(); ++i)
1532  {
1533  if(!filterNaNs || pcl::isFinite(cloud.at(i)))
1534  {
1535  float * ptr = laserScan.ptr<float>(0, oi++);
1536  if(!nullTransform)
1537  {
1538  pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
1539  ptr[0] = pt.x;
1540  ptr[1] = pt.y;
1541  ptr[2] = pt.z;
1542  }
1543  else
1544  {
1545  ptr[0] = cloud.at(i).x;
1546  ptr[1] = cloud.at(i).y;
1547  ptr[2] = cloud.at(i).z;
1548  }
1549  }
1550  }
1551  }
1552  if(oi == 0)
1553  {
1554  return cv::Mat();
1555  }
1556  return laserScan(cv::Range::all(), cv::Range(0,oi));
1557 }
1558 
1559 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
1560 {
1561  return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
1562 }
1563 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
1564 {
1565  cv::Mat laserScan;
1566  bool nullTransform = transform.isNull() || transform.isIdentity();
1567  int oi=0;
1568  if(indices.get())
1569  {
1570  laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(6));
1571  for(unsigned int i=0; i<indices->size(); ++i)
1572  {
1573  int index = indices->at(i);
1574  if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
1575  uIsFinite(cloud.at(index).normal_x) &&
1576  uIsFinite(cloud.at(index).normal_y) &&
1577  uIsFinite(cloud.at(index).normal_z)))
1578  {
1579  float * ptr = laserScan.ptr<float>(0, oi++);
1580  if(!nullTransform)
1581  {
1582  pcl::PointNormal pt = util3d::transformPoint(cloud.at(index), transform);
1583  ptr[0] = pt.x;
1584  ptr[1] = pt.y;
1585  ptr[2] = pt.z;
1586  ptr[3] = pt.normal_x;
1587  ptr[4] = pt.normal_y;
1588  ptr[5] = pt.normal_z;
1589  }
1590  else
1591  {
1592  ptr[0] = cloud.at(index).x;
1593  ptr[1] = cloud.at(index).y;
1594  ptr[2] = cloud.at(index).z;
1595  ptr[3] = cloud.at(index).normal_x;
1596  ptr[4] = cloud.at(index).normal_y;
1597  ptr[5] = cloud.at(index).normal_z;
1598  }
1599  }
1600  }
1601  }
1602  else
1603  {
1604  laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
1605  for(unsigned int i=0; i<cloud.size(); ++i)
1606  {
1607  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
1608  uIsFinite(cloud.at(i).normal_x) &&
1609  uIsFinite(cloud.at(i).normal_y) &&
1610  uIsFinite(cloud.at(i).normal_z)))
1611  {
1612  float * ptr = laserScan.ptr<float>(0, oi++);
1613  if(!nullTransform)
1614  {
1615  pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
1616  ptr[0] = pt.x;
1617  ptr[1] = pt.y;
1618  ptr[2] = pt.z;
1619  ptr[3] = pt.normal_x;
1620  ptr[4] = pt.normal_y;
1621  ptr[5] = pt.normal_z;
1622  }
1623  else
1624  {
1625  ptr[0] = cloud.at(i).x;
1626  ptr[1] = cloud.at(i).y;
1627  ptr[2] = cloud.at(i).z;
1628  ptr[3] = cloud.at(i).normal_x;
1629  ptr[4] = cloud.at(i).normal_y;
1630  ptr[5] = cloud.at(i).normal_z;
1631  }
1632  }
1633  }
1634  }
1635  if(oi == 0)
1636  {
1637  return cv::Mat();
1638  }
1639  return laserScan(cv::Range::all(), cv::Range(0,oi));
1640 }
1641 
1642 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
1643 {
1644  UASSERT(cloud.size() == normals.size());
1645  cv::Mat laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
1646  bool nullTransform = transform.isNull() || transform.isIdentity();
1647  int oi =0;
1648  for(unsigned int i=0; i<cloud.size(); ++i)
1649  {
1650  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
1651  {
1652  float * ptr = laserScan.ptr<float>(0, oi++);
1653  if(!nullTransform)
1654  {
1655  pcl::PointNormal pt;
1656  pt.x = cloud.at(i).x;
1657  pt.y = cloud.at(i).y;
1658  pt.z = cloud.at(i).z;
1659  pt.normal_x = normals.at(i).normal_x;
1660  pt.normal_y = normals.at(i).normal_y;
1661  pt.normal_z = normals.at(i).normal_z;
1662  pt = util3d::transformPoint(pt, transform);
1663  ptr[0] = pt.x;
1664  ptr[1] = pt.y;
1665  ptr[2] = pt.z;
1666  ptr[3] = pt.normal_x;
1667  ptr[4] = pt.normal_y;
1668  ptr[5] = pt.normal_z;
1669  }
1670  else
1671  {
1672  ptr[0] = cloud.at(i).x;
1673  ptr[1] = cloud.at(i).y;
1674  ptr[2] = cloud.at(i).z;
1675  ptr[3] = normals.at(i).normal_x;
1676  ptr[4] = normals.at(i).normal_y;
1677  ptr[5] = normals.at(i).normal_z;
1678  }
1679  }
1680  }
1681  if(oi == 0)
1682  {
1683  return cv::Mat();
1684  }
1685  return laserScan(cv::Range::all(), cv::Range(0,oi));
1686 }
1687 
1688 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform, bool filterNaNs)
1689 {
1690  return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
1691 }
1692 
1693 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
1694 {
1695  cv::Mat laserScan;
1696  bool nullTransform = transform.isNull() || transform.isIdentity();
1697  Eigen::Affine3f transform3f = transform.toEigen3f();
1698  int oi=0;
1699  if(indices.get())
1700  {
1701  laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4));
1702  for(unsigned int i=0; i<indices->size(); ++i)
1703  {
1704  int index = indices->at(i);
1705  if(!filterNaNs || pcl::isFinite(cloud.at(index)))
1706  {
1707  float * ptr = laserScan.ptr<float>(0, oi++);
1708  if(!nullTransform)
1709  {
1710  pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(index), transform3f);
1711  ptr[0] = pt.x;
1712  ptr[1] = pt.y;
1713  ptr[2] = pt.z;
1714  }
1715  else
1716  {
1717  ptr[0] = cloud.at(index).x;
1718  ptr[1] = cloud.at(index).y;
1719  ptr[2] = cloud.at(index).z;
1720  }
1721  int * ptrInt = (int*)ptr;
1722  ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16);
1723  }
1724  }
1725  }
1726  else
1727  {
1728  laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
1729  for(unsigned int i=0; i<cloud.size(); ++i)
1730  {
1731  if(!filterNaNs || pcl::isFinite(cloud.at(i)))
1732  {
1733  float * ptr = laserScan.ptr<float>(0, oi++);
1734  if(!nullTransform)
1735  {
1736  pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(i), transform3f);
1737  ptr[0] = pt.x;
1738  ptr[1] = pt.y;
1739  ptr[2] = pt.z;
1740  }
1741  else
1742  {
1743  ptr[0] = cloud.at(i).x;
1744  ptr[1] = cloud.at(i).y;
1745  ptr[2] = cloud.at(i).z;
1746  }
1747  int * ptrInt = (int*)ptr;
1748  ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
1749  }
1750  }
1751  }
1752  if(oi == 0)
1753  {
1754  return cv::Mat();
1755  }
1756  return laserScan(cv::Range::all(), cv::Range(0,oi));
1757 }
1758 
1759 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
1760 {
1761  return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
1762 }
1763 
1764 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
1765 {
1766  cv::Mat laserScan;
1767  bool nullTransform = transform.isNull() || transform.isIdentity();
1768  Eigen::Affine3f transform3f = transform.toEigen3f();
1769  int oi=0;
1770  if(indices.get())
1771  {
1772  laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4));
1773  for(unsigned int i=0; i<indices->size(); ++i)
1774  {
1775  int index = indices->at(i);
1776  if(!filterNaNs || pcl::isFinite(cloud.at(index)))
1777  {
1778  float * ptr = laserScan.ptr<float>(0, oi++);
1779  if(!nullTransform)
1780  {
1781  pcl::PointXYZI pt = pcl::transformPoint(cloud.at(index), transform3f);
1782  ptr[0] = pt.x;
1783  ptr[1] = pt.y;
1784  ptr[2] = pt.z;
1785  }
1786  else
1787  {
1788  ptr[0] = cloud.at(index).x;
1789  ptr[1] = cloud.at(index).y;
1790  ptr[2] = cloud.at(index).z;
1791  }
1792  ptr[3] = cloud.at(index).intensity;
1793  }
1794  }
1795  }
1796  else
1797  {
1798  laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
1799  for(unsigned int i=0; i<cloud.size(); ++i)
1800  {
1801  if(!filterNaNs || pcl::isFinite(cloud.at(i)))
1802  {
1803  float * ptr = laserScan.ptr<float>(0, oi++);
1804  if(!nullTransform)
1805  {
1806  pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f);
1807  ptr[0] = pt.x;
1808  ptr[1] = pt.y;
1809  ptr[2] = pt.z;
1810  }
1811  else
1812  {
1813  ptr[0] = cloud.at(i).x;
1814  ptr[1] = cloud.at(i).y;
1815  ptr[2] = cloud.at(i).z;
1816  }
1817  ptr[3] = cloud.at(i).intensity;
1818  }
1819  }
1820  }
1821  if(oi == 0)
1822  {
1823  return cv::Mat();
1824  }
1825  return laserScan(cv::Range::all(), cv::Range(0,oi));
1826 }
1827 
1828 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
1829 {
1830  UASSERT(cloud.size() == normals.size());
1831  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
1832  bool nullTransform = transform.isNull() || transform.isIdentity();
1833  int oi = 0;
1834  for(unsigned int i=0; i<cloud.size(); ++i)
1835  {
1836  if(!filterNaNs || pcl::isFinite(cloud.at(i)))
1837  {
1838  float * ptr = laserScan.ptr<float>(0, oi++);
1839  if(!nullTransform)
1840  {
1841  pcl::PointXYZRGBNormal pt;
1842  pt.x = cloud.at(i).x;
1843  pt.y = cloud.at(i).y;
1844  pt.z = cloud.at(i).z;
1845  pt.normal_x = normals.at(i).normal_x;
1846  pt.normal_y = normals.at(i).normal_y;
1847  pt.normal_z = normals.at(i).normal_z;
1848  pt = util3d::transformPoint(pt, transform);
1849  ptr[0] = pt.x;
1850  ptr[1] = pt.y;
1851  ptr[2] = pt.z;
1852  ptr[4] = pt.normal_x;
1853  ptr[5] = pt.normal_y;
1854  ptr[6] = pt.normal_z;
1855  }
1856  else
1857  {
1858  ptr[0] = cloud.at(i).x;
1859  ptr[1] = cloud.at(i).y;
1860  ptr[2] = cloud.at(i).z;
1861  ptr[4] = normals.at(i).normal_x;
1862  ptr[5] = normals.at(i).normal_y;
1863  ptr[6] = normals.at(i).normal_z;
1864  }
1865  int * ptrInt = (int*)ptr;
1866  ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
1867  }
1868  }
1869  if(oi == 0)
1870  {
1871  return cv::Mat();
1872  }
1873  return laserScan(cv::Range::all(), cv::Range(0,oi));
1874 }
1875 
1876 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform, bool filterNaNs)
1877 {
1878  return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
1879 }
1880 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
1881 {
1882  cv::Mat laserScan;
1883  bool nullTransform = transform.isNull() || transform.isIdentity();
1884  int oi = 0;
1885  if(indices.get())
1886  {
1887  laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(7));
1888  for(unsigned int i=0; i<indices->size(); ++i)
1889  {
1890  int index = indices->at(i);
1891  if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
1892  uIsFinite(cloud.at(index).normal_x) &&
1893  uIsFinite(cloud.at(index).normal_y) &&
1894  uIsFinite(cloud.at(index).normal_z)))
1895  {
1896  float * ptr = laserScan.ptr<float>(0, oi++);
1897  if(!nullTransform)
1898  {
1899  pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(index), transform);
1900  ptr[0] = pt.x;
1901  ptr[1] = pt.y;
1902  ptr[2] = pt.z;
1903  ptr[4] = pt.normal_x;
1904  ptr[5] = pt.normal_y;
1905  ptr[6] = pt.normal_z;
1906  }
1907  else
1908  {
1909  ptr[0] = cloud.at(index).x;
1910  ptr[1] = cloud.at(index).y;
1911  ptr[2] = cloud.at(index).z;
1912  ptr[4] = cloud.at(index).normal_x;
1913  ptr[5] = cloud.at(index).normal_y;
1914  ptr[6] = cloud.at(index).normal_z;
1915  }
1916  int * ptrInt = (int*)ptr;
1917  ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16);
1918  }
1919  }
1920  }
1921  else
1922  {
1923  laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(7));
1924  for(unsigned int i=0; i<cloud.size(); ++i)
1925  {
1926  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
1927  uIsFinite(cloud.at(i).normal_x) &&
1928  uIsFinite(cloud.at(i).normal_y) &&
1929  uIsFinite(cloud.at(i).normal_z)))
1930  {
1931  float * ptr = laserScan.ptr<float>(0, oi++);
1932  if(!nullTransform)
1933  {
1934  pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(i), transform);
1935  ptr[0] = pt.x;
1936  ptr[1] = pt.y;
1937  ptr[2] = pt.z;
1938  ptr[4] = pt.normal_x;
1939  ptr[5] = pt.normal_y;
1940  ptr[6] = pt.normal_z;
1941  }
1942  else
1943  {
1944  ptr[0] = cloud.at(i).x;
1945  ptr[1] = cloud.at(i).y;
1946  ptr[2] = cloud.at(i).z;
1947  ptr[4] = cloud.at(i).normal_x;
1948  ptr[5] = cloud.at(i).normal_y;
1949  ptr[6] = cloud.at(i).normal_z;
1950  }
1951  int * ptrInt = (int*)ptr;
1952  ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
1953  }
1954  }
1955  }
1956  if(oi == 0)
1957  {
1958  return cv::Mat();
1959  }
1960  return laserScan(cv::Range::all(), cv::Range(0,oi));
1961 }
1962 
1963 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
1964 {
1965  UASSERT(cloud.size() == normals.size());
1966  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
1967  bool nullTransform = transform.isNull() || transform.isIdentity();
1968  int oi=0;
1969  for(unsigned int i=0; i<cloud.size(); ++i)
1970  {
1971  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
1972  {
1973  float * ptr = laserScan.ptr<float>(0, oi++);
1974  if(!nullTransform)
1975  {
1976  pcl::PointXYZINormal pt;
1977  pt.x = cloud.at(i).x;
1978  pt.y = cloud.at(i).y;
1979  pt.z = cloud.at(i).z;
1980  pt.normal_x = normals.at(i).normal_x;
1981  pt.normal_y = normals.at(i).normal_y;
1982  pt.normal_z = normals.at(i).normal_z;
1983  pt = util3d::transformPoint(pt, transform);
1984  ptr[0] = pt.x;
1985  ptr[1] = pt.y;
1986  ptr[2] = pt.z;
1987  ptr[4] = pt.normal_x;
1988  ptr[5] = pt.normal_y;
1989  ptr[6] = pt.normal_z;
1990  }
1991  else
1992  {
1993  ptr[0] = cloud.at(i).x;
1994  ptr[1] = cloud.at(i).y;
1995  ptr[2] = cloud.at(i).z;
1996  ptr[4] = normals.at(i).normal_x;
1997  ptr[5] = normals.at(i).normal_y;
1998  ptr[6] = normals.at(i).normal_z;
1999  }
2000  ptr[3] = cloud.at(i).intensity;
2001  }
2002  }
2003  if(oi == 0)
2004  {
2005  return cv::Mat();
2006  }
2007  return laserScan(cv::Range::all(), cv::Range(0,oi));
2008 }
2009 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
2010 {
2011  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
2012  bool nullTransform = transform.isNull() || transform.isIdentity();
2013  int oi = 0;
2014  for(unsigned int i=0; i<cloud.size(); ++i)
2015  {
2016  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
2017  uIsFinite(cloud.at(i).normal_x) &&
2018  uIsFinite(cloud.at(i).normal_y) &&
2019  uIsFinite(cloud.at(i).normal_z)))
2020  {
2021  float * ptr = laserScan.ptr<float>(0, oi++);
2022  if(!nullTransform)
2023  {
2024  pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform);
2025  ptr[0] = pt.x;
2026  ptr[1] = pt.y;
2027  ptr[2] = pt.z;
2028  ptr[4] = pt.normal_x;
2029  ptr[5] = pt.normal_y;
2030  ptr[6] = pt.normal_z;
2031  }
2032  else
2033  {
2034  ptr[0] = cloud.at(i).x;
2035  ptr[1] = cloud.at(i).y;
2036  ptr[2] = cloud.at(i).z;
2037  ptr[4] = cloud.at(i).normal_x;
2038  ptr[5] = cloud.at(i).normal_y;
2039  ptr[6] = cloud.at(i).normal_z;
2040  }
2041  ptr[3] = cloud.at(i).intensity;
2042  }
2043  }
2044  if(oi == 0)
2045  {
2046  return cv::Mat();
2047  }
2048  return laserScan(cv::Range::all(), cv::Range(0,oi));
2049 }
2050 
2051 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
2052 {
2053  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
2054  bool nullTransform = transform.isNull();
2055  Eigen::Affine3f transform3f = transform.toEigen3f();
2056  int oi=0;
2057  for(unsigned int i=0; i<cloud.size(); ++i)
2058  {
2059  if(!filterNaNs || pcl::isFinite(cloud.at(i)))
2060  {
2061  float * ptr = laserScan.ptr<float>(0, oi++);
2062  if(!nullTransform)
2063  {
2064  pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
2065  ptr[0] = pt.x;
2066  ptr[1] = pt.y;
2067  }
2068  else
2069  {
2070  ptr[0] = cloud.at(i).x;
2071  ptr[1] = cloud.at(i).y;
2072  }
2073  }
2074 
2075  }
2076  if(oi == 0)
2077  {
2078  return cv::Mat();
2079  }
2080  return laserScan(cv::Range::all(), cv::Range(0,oi));
2081 }
2082 
2083 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
2084 {
2085  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
2086  bool nullTransform = transform.isNull();
2087  Eigen::Affine3f transform3f = transform.toEigen3f();
2088  int oi=0;
2089  for(unsigned int i=0; i<cloud.size(); ++i)
2090  {
2091  if(!filterNaNs || pcl::isFinite(cloud.at(i)))
2092  {
2093  float * ptr = laserScan.ptr<float>(0, oi++);
2094  if(!nullTransform)
2095  {
2096  pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f);
2097  ptr[0] = pt.x;
2098  ptr[1] = pt.y;
2099  ptr[2] = pt.intensity;
2100  }
2101  else
2102  {
2103  ptr[0] = cloud.at(i).x;
2104  ptr[1] = cloud.at(i).y;
2105  ptr[2] = cloud.at(i).intensity;
2106  }
2107  }
2108 
2109  }
2110  if(oi == 0)
2111  {
2112  return cv::Mat();
2113  }
2114  return laserScan(cv::Range::all(), cv::Range(0,oi));
2115 }
2116 
2117 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
2118 {
2119  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
2120  bool nullTransform = transform.isNull();
2121  int oi=0;
2122  for(unsigned int i=0; i<cloud.size(); ++i)
2123  {
2124  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
2125  uIsFinite(cloud.at(i).normal_x) &&
2126  uIsFinite(cloud.at(i).normal_y) &&
2127  uIsFinite(cloud.at(i).normal_z)))
2128  {
2129  float * ptr = laserScan.ptr<float>(0, oi++);
2130  if(!nullTransform)
2131  {
2132  pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
2133  ptr[0] = pt.x;
2134  ptr[1] = pt.y;
2135  ptr[2] = pt.normal_x;
2136  ptr[3] = pt.normal_y;
2137  ptr[4] = pt.normal_z;
2138  }
2139  else
2140  {
2141  const pcl::PointNormal & pt = cloud.at(i);
2142  ptr[0] = pt.x;
2143  ptr[1] = pt.y;
2144  ptr[2] = pt.normal_x;
2145  ptr[3] = pt.normal_y;
2146  ptr[4] = pt.normal_z;
2147  }
2148  }
2149  }
2150  if(oi == 0)
2151  {
2152  return cv::Mat();
2153  }
2154  return laserScan(cv::Range::all(), cv::Range(0,oi));
2155 }
2156 
2157 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
2158 {
2159  UASSERT(cloud.size() == normals.size());
2160  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
2161  bool nullTransform = transform.isNull() || transform.isIdentity();
2162  int oi=0;
2163  for(unsigned int i=0; i<cloud.size(); ++i)
2164  {
2165  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
2166  {
2167  float * ptr = laserScan.ptr<float>(0, oi++);
2168  if(!nullTransform)
2169  {
2170  pcl::PointNormal pt;
2171  pt.x = cloud.at(i).x;
2172  pt.y = cloud.at(i).y;
2173  pt.z = cloud.at(i).z;
2174  pt.normal_x = normals.at(i).normal_x;
2175  pt.normal_y = normals.at(i).normal_y;
2176  pt.normal_z = normals.at(i).normal_z;
2177  pt = util3d::transformPoint(pt, transform);
2178  ptr[0] = pt.x;
2179  ptr[1] = pt.y;
2180  ptr[2] = pt.normal_x;
2181  ptr[3] = pt.normal_y;
2182  ptr[4] = pt.normal_z;
2183  }
2184  else
2185  {
2186  ptr[0] = cloud.at(i).x;
2187  ptr[1] = cloud.at(i).y;
2188  ptr[2] = normals.at(i).normal_x;
2189  ptr[3] = normals.at(i).normal_y;
2190  ptr[4] = normals.at(i).normal_z;
2191  }
2192  }
2193  }
2194  if(oi == 0)
2195  {
2196  return cv::Mat();
2197  }
2198  return laserScan(cv::Range::all(), cv::Range(0,oi));
2199 }
2200 
2201 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
2202 {
2203  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
2204  bool nullTransform = transform.isNull();
2205  int oi=0;
2206  for(unsigned int i=0; i<cloud.size(); ++i)
2207  {
2208  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
2209  uIsFinite(cloud.at(i).normal_x) &&
2210  uIsFinite(cloud.at(i).normal_y) &&
2211  uIsFinite(cloud.at(i).normal_z)))
2212  {
2213  float * ptr = laserScan.ptr<float>(0, oi++);
2214  if(!nullTransform)
2215  {
2216  pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform);
2217  ptr[0] = pt.x;
2218  ptr[1] = pt.y;
2219  ptr[2] = pt.intensity;
2220  ptr[3] = pt.normal_x;
2221  ptr[4] = pt.normal_y;
2222  ptr[5] = pt.normal_z;
2223  }
2224  else
2225  {
2226  const pcl::PointXYZINormal & pt = cloud.at(i);
2227  ptr[0] = pt.x;
2228  ptr[1] = pt.y;
2229  ptr[2] = pt.intensity;
2230  ptr[3] = pt.normal_x;
2231  ptr[4] = pt.normal_y;
2232  ptr[5] = pt.normal_z;
2233  }
2234  }
2235  }
2236  if(oi == 0)
2237  {
2238  return cv::Mat();
2239  }
2240  return laserScan(cv::Range::all(), cv::Range(0,oi));
2241 }
2242 
2243 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
2244 {
2245  UASSERT(cloud.size() == normals.size());
2246  cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
2247  bool nullTransform = transform.isNull() || transform.isIdentity();
2248  int oi=0;
2249  for(unsigned int i=0; i<cloud.size(); ++i)
2250  {
2251  if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
2252  {
2253  float * ptr = laserScan.ptr<float>(0, oi++);
2254  if(!nullTransform)
2255  {
2256  pcl::PointXYZINormal pt;
2257  pt.x = cloud.at(i).x;
2258  pt.y = cloud.at(i).y;
2259  pt.z = cloud.at(i).z;
2260  pt.normal_x = normals.at(i).normal_x;
2261  pt.normal_y = normals.at(i).normal_y;
2262  pt.normal_z = normals.at(i).normal_z;
2263  pt = util3d::transformPoint(pt, transform);
2264  ptr[0] = pt.x;
2265  ptr[1] = pt.y;
2266  ptr[2] = pt.intensity;
2267  ptr[3] = pt.normal_x;
2268  ptr[4] = pt.normal_y;
2269  ptr[5] = pt.normal_z;
2270  }
2271  else
2272  {
2273  ptr[0] = cloud.at(i).x;
2274  ptr[1] = cloud.at(i).y;
2275  ptr[2] = cloud.at(i).intensity;
2276  ptr[3] = normals.at(i).normal_x;
2277  ptr[4] = normals.at(i).normal_y;
2278  ptr[5] = normals.at(i).normal_z;
2279  }
2280  }
2281  }
2282  if(oi == 0)
2283  {
2284  return cv::Mat();
2285  }
2286  return laserScan(cv::Range::all(), cv::Range(0,oi));
2287 }
2288 
2289 pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform)
2290 {
2291  pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
2292  if(laserScan.isEmpty())
2293  {
2294  return cloud;
2295  }
2296 
2297  if(laserScan.format() == LaserScan::kXY || laserScan.format() == LaserScan::kXYZ)
2298  {
2299  pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
2300  }
2301  else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
2302  {
2303  pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
2304  }
2305  else if(laserScan.format() == LaserScan::kXYNormal || laserScan.format() == LaserScan::kXYZNormal)
2306  {
2307  pcl::toPCLPointCloud2(*laserScanToPointCloudNormal(laserScan, transform), *cloud);
2308  }
2309  else if(laserScan.format() == LaserScan::kXYINormal || laserScan.format() == LaserScan::kXYZINormal)
2310  {
2311  pcl::toPCLPointCloud2(*laserScanToPointCloudINormal(laserScan, transform), *cloud);
2312  }
2313  else if(laserScan.format() == LaserScan::kXYZRGB)
2314  {
2315  pcl::toPCLPointCloud2(*laserScanToPointCloudRGB(laserScan, transform), *cloud);
2316  }
2317  else if(laserScan.format() == LaserScan::kXYZRGBNormal)
2318  {
2319  pcl::toPCLPointCloud2(*laserScanToPointCloudRGBNormal(laserScan, transform), *cloud);
2320  }
2321  else
2322  {
2323  UERROR("Unknown conversion from LaserScan format %d to PointCloud2.", laserScan.format());
2324  }
2325  return cloud;
2326 }
2327 
2328 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform)
2329 {
2330  pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
2331  output->resize(laserScan.size());
2332  output->is_dense = true;
2333  bool nullTransform = transform.isNull();
2334  Eigen::Affine3f transform3f = transform.toEigen3f();
2335  for(int i=0; i<laserScan.size(); ++i)
2336  {
2337  output->at(i) = util3d::laserScanToPoint(laserScan, i);
2338  if(!nullTransform)
2339  {
2340  output->at(i) = pcl::transformPoint(output->at(i), transform3f);
2341  }
2342  }
2343  return output;
2344 }
2345 
2346 pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform)
2347 {
2348  pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
2349  output->resize(laserScan.size());
2350  output->is_dense = true;
2351  bool nullTransform = transform.isNull();
2352  for(int i=0; i<laserScan.size(); ++i)
2353  {
2354  output->at(i) = laserScanToPointNormal(laserScan, i);
2355  if(!nullTransform)
2356  {
2357  output->at(i) = util3d::transformPoint(output->at(i), transform);
2358  }
2359  }
2360  return output;
2361 }
2362 
2363 pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
2364 {
2365  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
2366  output->resize(laserScan.size());
2367  output->is_dense = true;
2368  bool nullTransform = transform.isNull() || transform.isIdentity();
2369  Eigen::Affine3f transform3f = transform.toEigen3f();
2370  for(int i=0; i<laserScan.size(); ++i)
2371  {
2372  output->at(i) = util3d::laserScanToPointRGB(laserScan, i, r, g, b);
2373  if(!nullTransform)
2374  {
2375  output->at(i) = pcl::transformPoint(output->at(i), transform3f);
2376  }
2377  }
2378  return output;
2379 }
2380 
2381 pcl::PointCloud<pcl::PointXYZI>::Ptr laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform, float intensity)
2382 {
2383  pcl::PointCloud<pcl::PointXYZI>::Ptr output(new pcl::PointCloud<pcl::PointXYZI>);
2384  output->resize(laserScan.size());
2385  output->is_dense = true;
2386  bool nullTransform = transform.isNull() || transform.isIdentity();
2387  Eigen::Affine3f transform3f = transform.toEigen3f();
2388  for(int i=0; i<laserScan.size(); ++i)
2389  {
2390  output->at(i) = util3d::laserScanToPointI(laserScan, i, intensity);
2391  if(!nullTransform)
2392  {
2393  output->at(i) = pcl::transformPoint(output->at(i), transform3f);
2394  }
2395  }
2396  return output;
2397 }
2398 
2399 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
2400 {
2401  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
2402  output->resize(laserScan.size());
2403  output->is_dense = true;
2404  bool nullTransform = transform.isNull() || transform.isIdentity();
2405  for(int i=0; i<laserScan.size(); ++i)
2406  {
2407  output->at(i) = util3d::laserScanToPointRGBNormal(laserScan, i, r, g, b);
2408  if(!nullTransform)
2409  {
2410  output->at(i) = util3d::transformPoint(output->at(i), transform);
2411  }
2412  }
2413  return output;
2414 }
2415 
2416 pcl::PointCloud<pcl::PointXYZINormal>::Ptr laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform, float intensity)
2417 {
2418  pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZINormal>);
2419  output->resize(laserScan.size());
2420  output->is_dense = true;
2421  bool nullTransform = transform.isNull() || transform.isIdentity();
2422  for(int i=0; i<laserScan.size(); ++i)
2423  {
2424  output->at(i) = util3d::laserScanToPointINormal(laserScan, i, intensity);
2425  if(!nullTransform)
2426  {
2427  output->at(i) = util3d::transformPoint(output->at(i), transform);
2428  }
2429  }
2430  return output;
2431 }
2432 
2433 pcl::PointXYZ laserScanToPoint(const LaserScan & laserScan, int index)
2434 {
2435  UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
2436  pcl::PointXYZ output;
2437  const float * ptr = laserScan.data().ptr<float>(0, index);
2438  output.x = ptr[0];
2439  output.y = ptr[1];
2440  if(!laserScan.is2d())
2441  {
2442  output.z = ptr[2];
2443  }
2444  return output;
2445 }
2446 
2447 pcl::PointNormal laserScanToPointNormal(const LaserScan & laserScan, int index)
2448 {
2449  UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
2450  pcl::PointNormal output;
2451  const float * ptr = laserScan.data().ptr<float>(0, index);
2452  output.x = ptr[0];
2453  output.y = ptr[1];
2454  if(!laserScan.is2d())
2455  {
2456  output.z = ptr[2];
2457  }
2458  if(laserScan.hasNormals())
2459  {
2460  int offset = laserScan.getNormalsOffset();
2461  output.normal_x = ptr[offset];
2462  output.normal_y = ptr[offset+1];
2463  output.normal_z = ptr[offset+2];
2464  }
2465  return output;
2466 }
2467 
2468 pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
2469 {
2470  UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
2471  pcl::PointXYZRGB output;
2472  const float * ptr = laserScan.data().ptr<float>(0, index);
2473  output.x = ptr[0];
2474  output.y = ptr[1];
2475  if(!laserScan.is2d())
2476  {
2477  output.z = ptr[2];
2478  }
2479 
2480  if(laserScan.hasRGB())
2481  {
2482  int * ptrInt = (int*)ptr;
2483  int indexRGB = laserScan.getRGBOffset();
2484  output.b = (unsigned char)(ptrInt[indexRGB] & 0xFF);
2485  output.g = (unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF);
2486  output.r = (unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF);
2487  }
2488  else
2489  {
2490  output.r = r;
2491  output.g = g;
2492  output.b = b;
2493  }
2494  return output;
2495 }
2496 
2497 pcl::PointXYZI laserScanToPointI(const LaserScan & laserScan, int index, float intensity)
2498 {
2499  UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
2500  pcl::PointXYZI output;
2501  const float * ptr = laserScan.data().ptr<float>(0, index);
2502  output.x = ptr[0];
2503  output.y = ptr[1];
2504  if(!laserScan.is2d())
2505  {
2506  output.z = ptr[2];
2507  }
2508 
2509  if(laserScan.hasIntensity())
2510  {
2511  int offset = laserScan.getIntensityOffset();
2512  output.intensity = ptr[offset];
2513  }
2514  else
2515  {
2516  output.intensity = intensity;
2517  }
2518 
2519  return output;
2520 }
2521 
2522 pcl::PointXYZRGBNormal laserScanToPointRGBNormal(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::PointXYZRGBNormal 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
2543  {
2544  output.r = r;
2545  output.g = g;
2546  output.b = b;
2547  }
2548 
2549  if(laserScan.hasNormals())
2550  {
2551  int offset = laserScan.getNormalsOffset();
2552  output.normal_x = ptr[offset];
2553  output.normal_y = ptr[offset+1];
2554  output.normal_z = ptr[offset+2];
2555  }
2556 
2557  return output;
2558 }
2559 
2560 pcl::PointXYZINormal laserScanToPointINormal(const LaserScan & laserScan, int index, float intensity)
2561 {
2562  UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
2563  pcl::PointXYZINormal output;
2564  const float * ptr = laserScan.data().ptr<float>(0, index);
2565  output.x = ptr[0];
2566  output.y = ptr[1];
2567  if(!laserScan.is2d())
2568  {
2569  output.z = ptr[2];
2570  }
2571 
2572  if(laserScan.hasIntensity())
2573  {
2574  int offset = laserScan.getIntensityOffset();
2575  output.intensity = ptr[offset];
2576  }
2577  else
2578  {
2579  output.intensity = intensity;
2580  }
2581 
2582  if(laserScan.hasNormals())
2583  {
2584  int offset = laserScan.getNormalsOffset();
2585  output.normal_x = ptr[offset];
2586  output.normal_y = ptr[offset+1];
2587  output.normal_z = ptr[offset+2];
2588  }
2589 
2590  return output;
2591 }
2592 
2593 void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max)
2594 {
2595  UASSERT(!laserScan.empty());
2596  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));
2597 
2598  const float * ptr = laserScan.ptr<float>(0, 0);
2599  min.x = max.x = ptr[0];
2600  min.y = max.y = ptr[1];
2601  bool is3d = laserScan.channels() >= 3 && laserScan.channels() != 5;
2602  min.z = max.z = is3d?ptr[2]:0.0f;
2603  for(int i=1; i<laserScan.cols; ++i)
2604  {
2605  ptr = laserScan.ptr<float>(0, i);
2606 
2607  if(ptr[0] < min.x) min.x = ptr[0];
2608  else if(ptr[0] > max.x) max.x = ptr[0];
2609 
2610  if(ptr[1] < min.y) min.y = ptr[1];
2611  else if(ptr[1] > max.y) max.y = ptr[1];
2612 
2613  if(is3d)
2614  {
2615  if(ptr[2] < min.z) min.z = ptr[2];
2616  else if(ptr[2] > max.z) max.z = ptr[2];
2617  }
2618  }
2619 }
2620 void getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max)
2621 {
2622  cv::Point3f minCV, maxCV;
2623  getMinMax3D(laserScan, minCV, maxCV);
2624  min.x = minCV.x;
2625  min.y = minCV.y;
2626  min.z = minCV.z;
2627  max.x = maxCV.x;
2628  max.y = maxCV.y;
2629  max.z = maxCV.z;
2630 }
2631 
2632 // inspired from ROS image_geometry/src/stereo_camera_model.cpp
2633 cv::Point3f projectDisparityTo3D(
2634  const cv::Point2f & pt,
2635  float disparity,
2636  const StereoCameraModel & model)
2637 {
2638  if(disparity > 0.0f && model.baseline() > 0.0f && model.left().fx() > 0.0f)
2639  {
2640  //Z = baseline * f / (d + cx1-cx0);
2641  float c = 0.0f;
2642  if(model.right().cx()>0.0f && model.left().cx()>0.0f)
2643  {
2644  c = model.right().cx() - model.left().cx();
2645  }
2646  float W = model.baseline()/(disparity + c);
2647  return cv::Point3f((pt.x - model.left().cx())*W, (pt.y - model.left().cy())*W, model.left().fx()*W);
2648  }
2649  float bad_point = std::numeric_limits<float>::quiet_NaN ();
2650  return cv::Point3f(bad_point, bad_point, bad_point);
2651 }
2652 
2653 cv::Point3f projectDisparityTo3D(
2654  const cv::Point2f & pt,
2655  const cv::Mat & disparity,
2656  const StereoCameraModel & model)
2657 {
2658  UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
2659  int u = int(pt.x+0.5f);
2660  int v = int(pt.y+0.5f);
2661  float bad_point = std::numeric_limits<float>::quiet_NaN ();
2662  if(uIsInBounds(u, 0, disparity.cols) &&
2663  uIsInBounds(v, 0, disparity.rows))
2664  {
2665  float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
2666  return projectDisparityTo3D(pt, d, model);
2667  }
2668  return cv::Point3f(bad_point, bad_point, bad_point);
2669 }
2670 
2671 // Register point cloud to camera (return registered depth image)
2672 cv::Mat projectCloudToCamera(
2673  const cv::Size & imageSize,
2674  const cv::Mat & cameraMatrixK,
2675  const cv::Mat & laserScan, // assuming laser scan points are already in /base_link coordinate
2676  const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link
2677 {
2678  UASSERT(!cameraTransform.isNull());
2679  UASSERT(!laserScan.empty());
2680  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));
2681  UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
2682 
2683  float fx = cameraMatrixK.at<double>(0,0);
2684  float fy = cameraMatrixK.at<double>(1,1);
2685  float cx = cameraMatrixK.at<double>(0,2);
2686  float cy = cameraMatrixK.at<double>(1,2);
2687 
2688  cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
2689  Transform t = cameraTransform.inverse();
2690 
2691  int count = 0;
2692  for(int i=0; i<laserScan.cols; ++i)
2693  {
2694  const float* ptr = laserScan.ptr<float>(0, i);
2695 
2696  // Get 3D from laser scan
2697  cv::Point3f ptScan;
2698  if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC(5))
2699  {
2700  // 2D scans
2701  ptScan.x = ptr[0];
2702  ptScan.y = ptr[1];
2703  ptScan.z = 0;
2704  }
2705  else // 3D scans
2706  {
2707  ptScan.x = ptr[0];
2708  ptScan.y = ptr[1];
2709  ptScan.z = ptr[2];
2710  }
2711  ptScan = util3d::transformPoint(ptScan, t);
2712 
2713  // re-project in camera frame
2714  float z = ptScan.z;
2715 
2716  bool set = false;
2717  if(z > 0.0f)
2718  {
2719  float invZ = 1.0f/z;
2720  float dx = (fx*ptScan.x)*invZ + cx;
2721  float dy = (fy*ptScan.y)*invZ + cy;
2722  int dx_low = dx;
2723  int dy_low = dy;
2724  int dx_high = dx + 0.5f;
2725  int dy_high = dy + 0.5f;
2726 
2727  if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
2728  {
2729  float &zReg = registered.at<float>(dy_low, dx_low);
2730  if(zReg == 0 || z < zReg)
2731  {
2732  zReg = z;
2733  }
2734  set = true;
2735  }
2736  if((dx_low != dx_high || dy_low != dy_high) &&
2737  uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
2738  {
2739  float &zReg = registered.at<float>(dy_high, dx_high);
2740  if(zReg == 0 || z < zReg)
2741  {
2742  zReg = z;
2743  }
2744  set = true;
2745  }
2746  }
2747  if(set)
2748  {
2749  count++;
2750  }
2751  }
2752  UDEBUG("Points in camera=%d/%d", count, laserScan.cols);
2753 
2754  return registered;
2755 }
2756 
2757 cv::Mat projectCloudToCamera(
2758  const cv::Size & imageSize,
2759  const cv::Mat & cameraMatrixK,
2760  const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan, // assuming points are already in /base_link coordinate
2761  const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link
2762 {
2763  UASSERT(!cameraTransform.isNull());
2764  UASSERT(!laserScan->empty());
2765  UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
2766 
2767  float fx = cameraMatrixK.at<double>(0,0);
2768  float fy = cameraMatrixK.at<double>(1,1);
2769  float cx = cameraMatrixK.at<double>(0,2);
2770  float cy = cameraMatrixK.at<double>(1,2);
2771 
2772  cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
2773  Transform t = cameraTransform.inverse();
2774 
2775  int count = 0;
2776  for(int i=0; i<(int)laserScan->size(); ++i)
2777  {
2778  // Get 3D from laser scan
2779  pcl::PointXYZ ptScan = laserScan->at(i);
2780  ptScan = util3d::transformPoint(ptScan, t);
2781 
2782  // re-project in camera frame
2783  float z = ptScan.z;
2784  bool set = false;
2785  if(z > 0.0f)
2786  {
2787  float invZ = 1.0f/z;
2788  float dx = (fx*ptScan.x)*invZ + cx;
2789  float dy = (fy*ptScan.y)*invZ + cy;
2790  int dx_low = dx;
2791  int dy_low = dy;
2792  int dx_high = dx + 0.5f;
2793  int dy_high = dy + 0.5f;
2794  if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
2795  {
2796  set = true;
2797  float &zReg = registered.at<float>(dy_low, dx_low);
2798  if(zReg == 0 || z < zReg)
2799  {
2800  zReg = z;
2801  }
2802  }
2803  if((dx_low != dx_high || dy_low != dy_high) &&
2804  uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
2805  {
2806  set = true;
2807  float &zReg = registered.at<float>(dy_high, dx_high);
2808  if(zReg == 0 || z < zReg)
2809  {
2810  zReg = z;
2811  }
2812  }
2813  }
2814  if(set)
2815  {
2816  count++;
2817  }
2818  }
2819  UDEBUG("Points in camera=%d/%d", count, (int)laserScan->size());
2820 
2821  return registered;
2822 }
2823 
2824 cv::Mat projectCloudToCamera(
2825  const cv::Size & imageSize,
2826  const cv::Mat & cameraMatrixK,
2827  const pcl::PCLPointCloud2::Ptr laserScan, // assuming points are already in /base_link coordinate
2828  const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link
2829 {
2830  UASSERT(!cameraTransform.isNull());
2831  UASSERT(!laserScan->data.empty());
2832  UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
2833 
2834  float fx = cameraMatrixK.at<double>(0,0);
2835  float fy = cameraMatrixK.at<double>(1,1);
2836  float cx = cameraMatrixK.at<double>(0,2);
2837  float cy = cameraMatrixK.at<double>(1,2);
2838 
2839  cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
2840  Transform t = cameraTransform.inverse();
2841 
2842  pcl::MsgFieldMap field_map;
2843  pcl::createMapping<pcl::PointXYZ> (laserScan->fields, field_map);
2844 
2845  int count = 0;
2846  if(field_map.size() == 1)
2847  {
2848  for (uint32_t row = 0; row < laserScan->height; ++row)
2849  {
2850  const uint8_t* row_data = &laserScan->data[row * laserScan->row_step];
2851  for (uint32_t col = 0; col < laserScan->width; ++col)
2852  {
2853  const uint8_t* msg_data = row_data + col * laserScan->point_step;
2854  pcl::PointXYZ ptScan;
2855  memcpy (&ptScan, msg_data + field_map.front().serialized_offset, field_map.front().size);
2856  ptScan = util3d::transformPoint(ptScan, t);
2857 
2858  // re-project in camera frame
2859  float z = ptScan.z;
2860  bool set = false;
2861  if(z > 0.0f)
2862  {
2863  float invZ = 1.0f/z;
2864  float dx = (fx*ptScan.x)*invZ + cx;
2865  float dy = (fy*ptScan.y)*invZ + cy;
2866  int dx_low = dx;
2867  int dy_low = dy;
2868  int dx_high = dx + 0.5f;
2869  int dy_high = dy + 0.5f;
2870  if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
2871  {
2872  set = true;
2873  float &zReg = registered.at<float>(dy_low, dx_low);
2874  if(zReg == 0 || z < zReg)
2875  {
2876  zReg = z;
2877  }
2878  }
2879  if((dx_low != dx_high || dy_low != dy_high) &&
2880  uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
2881  {
2882  set = true;
2883  float &zReg = registered.at<float>(dy_high, dx_high);
2884  if(zReg == 0 || z < zReg)
2885  {
2886  zReg = z;
2887  }
2888  }
2889  }
2890  if(set)
2891  {
2892  count++;
2893  }
2894  }
2895  }
2896  }
2897  else
2898  {
2899  UERROR("field map pcl::pointXYZ not found!");
2900  }
2901 /*
2902  int count = 0;
2903  for(int i=0; i<(int)laserScan->size(); ++i)
2904  {
2905  // Get 3D from laser scan
2906  pcl::PointXYZ ptScan = laserScan->at(i);
2907  ptScan = util3d::transformPoint(ptScan, t);
2908 
2909  // re-project in camera frame
2910  float z = ptScan.z;
2911  bool set = false;
2912  if(z > 0.0f)
2913  {
2914  float invZ = 1.0f/z;
2915  float dx = (fx*ptScan.x)*invZ + cx;
2916  float dy = (fy*ptScan.y)*invZ + cy;
2917  int dx_low = dx;
2918  int dy_low = dy;
2919  int dx_high = dx + 0.5f;
2920  int dy_high = dy + 0.5f;
2921  if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
2922  {
2923  set = true;
2924  float &zReg = registered.at<float>(dy_low, dx_low);
2925  if(zReg == 0 || z < zReg)
2926  {
2927  zReg = z;
2928  }
2929  }
2930  if((dx_low != dx_high || dy_low != dy_high) &&
2931  uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
2932  {
2933  set = true;
2934  float &zReg = registered.at<float>(dy_high, dx_high);
2935  if(zReg == 0 || z < zReg)
2936  {
2937  zReg = z;
2938  }
2939  }
2940  }
2941  if(set)
2942  {
2943  count++;
2944  }
2945  }
2946  */
2947  UDEBUG("Points in camera=%d/%d", count, (int)laserScan->data.size());
2948 
2949  return registered;
2950 }
2951 
2952 void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection, bool fillToBorder)
2953 {
2954  UASSERT(registeredDepth.type() == CV_32FC1);
2955  if(verticalDirection)
2956  {
2957  // vertical, for each column
2958  for(int x=0; x<registeredDepth.cols; ++x)
2959  {
2960  float valueA = 0.0f;
2961  int indexA = -1;
2962  for(int y=0; y<registeredDepth.rows; ++y)
2963  {
2964  float v = registeredDepth.at<float>(y,x);
2965  if(fillToBorder && y == registeredDepth.rows-1 && v<=0.0f && indexA>=0)
2966  {
2967  v = valueA;
2968  }
2969  if(v > 0.0f)
2970  {
2971  if(fillToBorder && indexA < 0)
2972  {
2973  indexA = 0;
2974  valueA = v;
2975  }
2976  if(indexA >=0)
2977  {
2978  int range = y-indexA;
2979  if(range > 1)
2980  {
2981  float slope = (v-valueA)/(range);
2982  for(int k=1; k<range; ++k)
2983  {
2984  registeredDepth.at<float>(indexA+k,x) = valueA+slope*float(k);
2985  }
2986  }
2987  }
2988  valueA = v;
2989  indexA = y;
2990  }
2991  }
2992  }
2993  }
2994  else
2995  {
2996  // horizontal, for each row
2997  for(int y=0; y<registeredDepth.rows; ++y)
2998  {
2999  float valueA = 0.0f;
3000  int indexA = -1;
3001  for(int x=0; x<registeredDepth.cols; ++x)
3002  {
3003  float v = registeredDepth.at<float>(y,x);
3004  if(fillToBorder && x == registeredDepth.cols-1 && v<=0.0f && indexA>=0)
3005  {
3006  v = valueA;
3007  }
3008  if(v > 0.0f)
3009  {
3010  if(fillToBorder && indexA < 0)
3011  {
3012  indexA = 0;
3013  valueA = v;
3014  }
3015  if(indexA >=0)
3016  {
3017  int range = x-indexA;
3018  if(range > 1)
3019  {
3020  float slope = (v-valueA)/(range);
3021  for(int k=1; k<range; ++k)
3022  {
3023  registeredDepth.at<float>(y,indexA+k) = valueA+slope*float(k);
3024  }
3025  }
3026  }
3027  valueA = v;
3028  indexA = x;
3029  }
3030  }
3031  }
3032  }
3033 }
3034 
3035 bool isFinite(const cv::Point3f & pt)
3036 {
3037  return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z);
3038 }
3039 
3040 pcl::PointCloud<pcl::PointXYZ>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds)
3041 {
3042  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
3043  for(std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
3044  {
3045  *cloud += *(*iter);
3046  }
3047  return cloud;
3048 }
3049 
3050 pcl::PointCloud<pcl::PointXYZRGB>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds)
3051 {
3052  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
3053  for(std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
3054  {
3055  *cloud+=*(*iter);
3056  }
3057  return cloud;
3058 }
3059 
3060 pcl::IndicesPtr concatenate(const std::vector<pcl::IndicesPtr> & indices)
3061 {
3062  //compute total size
3063  unsigned int totalSize = 0;
3064  for(unsigned int i=0; i<indices.size(); ++i)
3065  {
3066  totalSize += (unsigned int)indices[i]->size();
3067  }
3068  pcl::IndicesPtr ind(new std::vector<int>(totalSize));
3069  unsigned int io = 0;
3070  for(unsigned int i=0; i<indices.size(); ++i)
3071  {
3072  for(unsigned int j=0; j<indices[i]->size(); ++j)
3073  {
3074  ind->at(io++) = indices[i]->at(j);
3075  }
3076  }
3077  return ind;
3078 }
3079 
3080 pcl::IndicesPtr concatenate(const pcl::IndicesPtr & indicesA, const pcl::IndicesPtr & indicesB)
3081 {
3082  pcl::IndicesPtr ind(new std::vector<int>(*indicesA));
3083  ind->resize(ind->size()+indicesB->size());
3084  unsigned int oi = (unsigned int)indicesA->size();
3085  for(unsigned int i=0; i<indicesB->size(); ++i)
3086  {
3087  ind->at(oi++) = indicesB->at(i);
3088  }
3089  return ind;
3090 }
3091 
3092 void savePCDWords(
3093  const std::string & fileName,
3094  const std::multimap<int, pcl::PointXYZ> & words,
3095  const Transform & transform)
3096 {
3097  if(words.size())
3098  {
3099  pcl::PointCloud<pcl::PointXYZ> cloud;
3100  cloud.resize(words.size());
3101  int i=0;
3102  for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
3103  {
3104  cloud[i++] = transformPoint(iter->second, transform);
3105  }
3106  pcl::io::savePCDFile(fileName, cloud);
3107  }
3108 }
3109 
3110 void savePCDWords(
3111  const std::string & fileName,
3112  const std::multimap<int, cv::Point3f> & words,
3113  const Transform & transform)
3114 {
3115  if(words.size())
3116  {
3117  pcl::PointCloud<pcl::PointXYZ> cloud;
3118  cloud.resize(words.size());
3119  int i=0;
3120  for(std::multimap<int, cv::Point3f>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
3121  {
3122  cv::Point3f pt = transformPoint(iter->second, transform);
3123  cloud[i++] = pcl::PointXYZ(pt.x, pt.y, pt.z);
3124  }
3125  pcl::io::savePCDFile(fileName, cloud);
3126  }
3127 }
3128 
3129 cv::Mat loadBINScan(const std::string & fileName)
3130 {
3131  cv::Mat output;
3132  long bytes = UFile::length(fileName);
3133  if(bytes)
3134  {
3135  int dim = 4;
3136  UASSERT(bytes % sizeof(float) == 0);
3137  size_t num = bytes/sizeof(float);
3138  UASSERT(num % dim == 0);
3139  output = cv::Mat(1, num/dim, CV_32FC(dim));
3140 
3141  // load point cloud
3142  FILE *stream;
3143  stream = fopen (fileName.c_str(),"rb");
3144  size_t actualReadNum = fread(output.data,sizeof(float),num,stream);
3145  UASSERT(num == actualReadNum);
3146  fclose(stream);
3147  }
3148 
3149  return output;
3150 }
3151 
3152 pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName)
3153 {
3154  return laserScanToPointCloud(loadScan(fileName));
3155 }
3156 
3157 pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, int dim)
3158 {
3159  return loadBINCloud(fileName);
3160 }
3161 
3162 LaserScan loadScan(const std::string & path)
3163 {
3164  std::string fileName = UFile::getName(path);
3165  if(UFile::getExtension(fileName).compare("bin") == 0)
3166  {
3167  return LaserScan(loadBINScan(path), 0, 0, LaserScan::kXYZI);
3168  }
3169  else if(UFile::getExtension(fileName).compare("pcd") == 0)
3170  {
3171  pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
3172  pcl::io::loadPCDFile(path, *cloud);
3173  return laserScanFromPointCloud(*cloud);
3174  }
3175  else
3176  {
3177  pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
3178  pcl::io::loadPLYFile(path, *cloud);
3179  return laserScanFromPointCloud(*cloud);
3180  }
3181  return LaserScan();
3182 }
3183 
3184 pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
3185  const std::string & path,
3186  const Transform & transform,
3187  int downsampleStep,
3188  float voxelSize)
3189 {
3190  UASSERT(!transform.isNull());
3191  UDEBUG("Loading cloud (step=%d, voxel=%f m) : %s", downsampleStep, voxelSize, path.c_str());
3192  std::string fileName = UFile::getName(path);
3193  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
3194  if(UFile::getExtension(fileName).compare("bin") == 0)
3195  {
3196  cloud = util3d::loadBINCloud(path); // Assume KITTI velodyne format
3197  }
3198  else if(UFile::getExtension(fileName).compare("pcd") == 0)
3199  {
3200  pcl::io::loadPCDFile(path, *cloud);
3201  }
3202  else
3203  {
3204  pcl::io::loadPLYFile(path, *cloud);
3205  }
3206  int previousSize = (int)cloud->size();
3207  if(downsampleStep > 1 && cloud->size())
3208  {
3209  cloud = util3d::downsample(cloud, downsampleStep);
3210  UDEBUG("Downsampling scan (step=%d): %d -> %d", downsampleStep, previousSize, (int)cloud->size());
3211  }
3212  previousSize = (int)cloud->size();
3213  if(voxelSize > 0.0f && cloud->size())
3214  {
3215  cloud = util3d::voxelize(cloud, voxelSize);
3216  UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", voxelSize, previousSize, (int)cloud->size());
3217  }
3218  if(transform.isIdentity())
3219  {
3220  return cloud;
3221  }
3222  return util3d::transformPointCloud(cloud, transform);
3223 }
3224 
3225 }
3226 
3227 }
rtabmap::util2d::disparityFromStereoImages
cv::Mat RTABMAP_EXP disparityFromStereoImages(const cv::Mat &leftImage, const cv::Mat &rightImage, const ParametersMap &parameters=ParametersMap())
Definition: util2d.cpp:734
d
d
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true)
Definition: util3d.cpp:1266
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3035
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:113
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:2447
rtabmap::util3d::projectDisparityTo3D
cv::Point3f RTABMAP_EXP projectDisparityTo3D(const cv::Point2f &pt, float disparity, const StereoCameraModel &model)
Definition: util3d.cpp:2633
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:112
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:193
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:63
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:66
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:73
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:89
rtabmap::util3d::getMinMax3D
void RTABMAP_EXP getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2593
f
f
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:934
rtabmap::LaserScan::channels
static int channels(Format format)
Definition: LaserScan.cpp:34
rtabmap::LaserScan::getRGBOffset
int getRGBOffset() const
Definition: LaserScan.h:80
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=255, unsigned char g=255, unsigned char b=255)
Definition: util3d.cpp:2399
rtabmap::util3d::laserScanToPointINormal
pcl::PointXYZINormal RTABMAP_EXP laserScanToPointINormal(const LaserScan &laserScan, int index, float intensity)
Definition: util3d.cpp:2560
rtabmap::util3d::removeNaNFromPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
Definition: util3d_filtering.cpp:745
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2289
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:69
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:74
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
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:2433
rtabmap::LaserScan::getNormalsOffset
int getNormalsOffset() const
Definition: LaserScan.h:81
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:72
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:3092
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:70
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:2522
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:161
UFATAL
#define UFATAL(...)
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2328
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
Definition: SensorData.h:173
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:1245
rtabmap::util3d::downsample
LaserScan RTABMAP_EXP downsample(const LaserScan &cloud, int step)
Definition: util3d_filtering.cpp:317
rtabmap::util3d::laserScan2dFromPointCloud
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:2051
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::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:119
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:97
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:96
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:2416
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::util3d::laserScanToPointRGB
pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const LaserScan &laserScan, int index, unsigned char r=255, unsigned char g=255, unsigned char b=255)
Definition: util3d.cpp:2468
rtabmap::util2d::computeRoi
cv::Rect RTABMAP_EXP computeRoi(const cv::Mat &image, const std::string &roiRatios)
Definition: util2d.cpp:1108
rtabmap::LaserScan::isCompressed
bool isCompressed() const
Definition: LaserScan.h:76
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:2497
rtabmap::CameraModel::roi
CameraModel roi(const cv::Rect &roi) const
Definition: CameraModel.cpp:489
util3d_transforms.h
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:98
rtabmap::LaserScan::getIntensityOffset
int getIntensityOffset() const
Definition: LaserScan.h:79
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:80
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:193
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:95
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:120
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2346
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
rtabmap::SensorData::stereoCameraModel
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:194
UWARN
#define UWARN(...)
rtabmap::util3d::fillProjectedCloudHoles
void RTABMAP_EXP fillProjectedCloudHoles(cv::Mat &depthRegistered, bool verticalDirection, bool fillToBorder)
Definition: util3d.cpp:2952
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:2672
rtabmap::util3d::loadBINCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP loadBINCloud(const std::string &fileName)
Definition: util3d.cpp:3152
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:169
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:2381
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:344
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:942
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:3060
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:407
rtabmap::util3d::loadScan
LaserScan RTABMAP_EXP loadScan(const std::string &path)
Definition: util3d.cpp:3162
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:3184
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:114
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::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=255, unsigned char g=255, unsigned char b=255)
Definition: util3d.cpp:2363
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:172
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:75
rtabmap::util3d::concatenateClouds
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP concatenateClouds(const std::list< pcl::PointCloud< pcl::PointXYZ >::Ptr > &clouds)
Definition: util3d.cpp:3040
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:109
util3d_surface.h
rtabmap::util3d::loadBINScan
cv::Mat RTABMAP_EXP loadBINScan(const std::string &fileName)
Definition: util3d.cpp:3129

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:43:40