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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:38:58