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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:46:07