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>
52 cv::Mat
bgrFromCloud(
const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
bool bgrOrder)
54 cv::Mat frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
56 for(
unsigned int h = 0;
h < cloud.height;
h++)
58 for(
unsigned int w = 0;
w < cloud.width;
w++)
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;
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;
79 const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
84 cv::Mat frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
87 for(
unsigned int h = 0;
h < cloud.height;
h++)
89 for(
unsigned int w = 0;
w < cloud.width;
w++)
91 float depth = cloud.at(
h*cloud.width +
w).z;
95 unsigned short depthMM = 0;
96 if(depth <= (
float)USHRT_MAX)
98 depthMM = (
unsigned short)depth;
100 frameDepth.at<
unsigned short>(
h,
w) = depthMM;
104 frameDepth.at<
float>(
h,
w) = depth;
111 w != cloud.width/2 &&
114 fx = cloud.at(
h*cloud.width +
w).x / ((
float(
w) -
float(cloud.width)/2.0f) * depth);
123 h != cloud.height/2 &&
126 fy = cloud.at(
h*cloud.width +
w).y / ((
float(
h) -
float(cloud.height)/2.0f) * depth);
140 cv::Mat & frameDepth,
146 frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
147 frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
151 for(
unsigned int h = 0;
h < cloud.height;
h++)
153 for(
unsigned int w = 0;
w < cloud.width;
w++)
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;
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;
170 float depth = cloud.at(
h*cloud.width +
w).z;
174 unsigned short depthMM = 0;
175 if(depth <= (
float)USHRT_MAX)
177 depthMM = (
unsigned short)depth;
179 frameDepth.at<
unsigned short>(
h,
w) = depthMM;
183 frameDepth.at<
float>(
h,
w) = depth;
190 w != cloud.width/2 &&
193 fx = 1.0f/(cloud.at(
h*cloud.width +
w).x / ((
float(
w) -
float(cloud.width)/2.0f) * depth));
202 h != cloud.height/2 &&
205 fy = 1.0f/(cloud.at(
h*cloud.width +
w).y / ((
float(
h) -
float(cloud.height)/2.0f) * depth));
216 const cv::Mat & depthImage,
221 float depthErrorRatio)
223 UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
231 cx =
cx > 0.0f ?
cx :
float(depthImage.cols/2) - 0.5f;
232 cy =
cy > 0.0f ?
cy :
float(depthImage.rows/2) - 0.5f;
235 pt.x = (
x -
cx) * depth /
fx;
236 pt.y = (
y -
cy) * depth /
fy;
241 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
247 const cv::Size & imageSize,
255 cx =
cx > 0.0f ?
cx :
float(imageSize.width/2) - 0.5f;
256 cy =
cy > 0.0f ?
cy :
float(imageSize.height/2) - 0.5f;
259 ray[0] = (
x -
cx) /
fx;
260 ray[1] = (
y -
cy) /
fy;
267 const cv::Mat & imageDepth,
273 std::vector<int> * validIndices)
280 const cv::Mat & imageDepthIn,
285 std::vector<int> * validIndices)
287 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
292 float rgbToDepthFactorX = 1.0f;
293 float rgbToDepthFactorY = 1.0f;
296 UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
298 cv::Mat imageDepth = imageDepthIn;
299 if(
model.imageHeight()>0 &&
model.imageWidth()>0)
301 UASSERT(
model.imageHeight() % imageDepthIn.rows == 0 &&
model.imageWidth() % imageDepthIn.cols == 0);
305 UDEBUG(
"Decimation from model (%d)", decimation);
306 if(
model.imageHeight() % decimation != 0)
308 UERROR(
"Decimation is not valid for current image size (model.imageHeight()=%d decimation=%d). The cloud is not created.",
model.imageHeight(), decimation);
311 if(
model.imageWidth() % decimation != 0)
313 UERROR(
"Decimation is not valid for current image size (model.imageWidth()=%d decimation=%d). The cloud is not created.",
model.imageWidth(), decimation);
318 decimation = -1*decimation;
320 int targetSize =
model.imageHeight() / decimation;
321 if(targetSize > imageDepthIn.rows)
323 UDEBUG(
"Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
327 else if(targetSize == imageDepthIn.rows)
333 UASSERT(imageDepthIn.rows % targetSize == 0);
334 decimation = imageDepthIn.rows / targetSize;
339 if(imageDepthIn.rows % decimation != 0)
341 UERROR(
"Decimation is not valid for current image size (imageDepth.rows=%d decimation=%d). The cloud is not created.", imageDepthIn.rows, decimation);
344 if(imageDepthIn.cols % decimation != 0)
346 UERROR(
"Decimation is not valid for current image size (imageDepth.cols=%d decimation=%d). The cloud is not created.", imageDepthIn.cols, decimation);
351 rgbToDepthFactorX = 1.0f/
float((
model.imageWidth() / imageDepth.cols));
352 rgbToDepthFactorY = 1.0f/
float((
model.imageHeight() / imageDepth.rows));
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());
362 cloud->height = imageDepth.rows/decimation;
363 cloud->width = imageDepth.cols/decimation;
364 cloud->is_dense =
false;
365 cloud->resize(cloud->height * cloud->width);
368 validIndices->resize(cloud->size());
371 float depthFx =
model.fx() * rgbToDepthFactorX;
372 float depthFy =
model.fy() * rgbToDepthFactorY;
373 float depthCx =
model.cx() * rgbToDepthFactorX;
374 float depthCy =
model.cy() * rgbToDepthFactorY;
376 UDEBUG(
"depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
377 imageDepth.cols, imageDepth.rows,
384 for(
int h = 0;
h < imageDepth.rows &&
h/decimation < (
int)cloud->height;
h+=decimation)
386 for(
int w = 0;
w < imageDepth.cols &&
w/decimation < (
int)cloud->width;
w+=decimation)
388 pcl::PointXYZ & pt = cloud->at((
h/decimation)*cloud->width + (
w/decimation));
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))
398 validIndices->at(oi++) = (
h/decimation)*cloud->width + (
w/decimation);
403 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
410 validIndices->resize(oi);
417 const cv::Mat & imageRgb,
418 const cv::Mat & imageDepth,
424 std::vector<int> * validIndices)
431 const cv::Mat & imageRgb,
432 const cv::Mat & imageDepthIn,
437 std::vector<int> * validIndices)
439 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
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());
451 UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
454 if(imageRgb.rows % decimation != 0 || imageRgb.cols % decimation != 0)
456 int oldDecimation = decimation;
457 while(decimation <= -1)
459 if(imageRgb.rows % decimation == 0 && imageRgb.cols % decimation == 0)
466 if(imageRgb.rows % oldDecimation != 0 || imageRgb.cols % oldDecimation != 0)
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);
474 if(imageDepthIn.rows % decimation != 0 || imageDepthIn.cols % decimation != 0)
476 int oldDecimation = decimation;
477 while(decimation >= 1)
479 if(imageDepthIn.rows % decimation == 0 && imageDepthIn.cols % decimation == 0)
486 if(imageDepthIn.rows % oldDecimation != 0 || imageDepthIn.cols % oldDecimation != 0)
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);
493 cv::Mat imageDepth = imageDepthIn;
496 UDEBUG(
"Decimation from RGB image (%d)", decimation);
498 decimation = -1*decimation;
500 int targetSize = imageRgb.rows / decimation;
501 if(targetSize > imageDepthIn.rows)
503 UDEBUG(
"Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
507 else if(targetSize == imageDepthIn.rows)
513 UASSERT(imageDepthIn.rows % targetSize == 0);
514 decimation = imageDepthIn.rows / targetSize;
519 if(imageRgb.channels() == 3)
523 else if(imageRgb.channels() == 1)
533 cloud->height = imageDepth.rows/decimation;
534 cloud->width = imageDepth.cols/decimation;
535 cloud->is_dense =
false;
536 cloud->resize(cloud->height * cloud->width);
539 validIndices->resize(cloud->size());
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;
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,
558 for(
int h = 0;
h < imageDepth.rows &&
h/decimation < (
int)cloud->height;
h+=decimation)
560 for(
int w = 0;
w < imageDepth.cols &&
w/decimation < (
int)cloud->width;
w+=decimation)
562 pcl::PointXYZRGB & pt = cloud->at((
h/decimation)*cloud->width + (
w/decimation));
564 int x =
int(
w*rgbToDepthFactorX);
565 int y =
int(
h*rgbToDepthFactorY);
566 UASSERT(
x >=0 && x<imageRgb.cols && y >=0 &&
y<imageRgb.rows);
569 const unsigned char * bgr = imageRgb.ptr<
unsigned char>(
y,
x);
576 unsigned char v = imageRgb.at<
unsigned char>(
y,
x);
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))
590 validIndices->at(oi) = (
h / decimation)*cloud->width + (
w / decimation);
596 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
602 validIndices->resize(oi);
606 UWARN(
"Cloud with only NaN values created!");
613 const cv::Mat & imageDisparity,
618 std::vector<int> * validIndices)
620 UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
623 if(imageDisparity.rows % decimation != 0 || imageDisparity.cols % decimation != 0)
625 int oldDecimation = decimation;
626 while(decimation >= 1)
628 if(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0)
635 if(imageDisparity.rows % oldDecimation != 0 || imageDisparity.cols % oldDecimation != 0)
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);
641 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
644 cloud->height = imageDisparity.rows/decimation;
645 cloud->width = imageDisparity.cols/decimation;
646 cloud->is_dense =
false;
647 cloud->resize(cloud->height * cloud->width);
650 validIndices->resize(cloud->size());
654 if(imageDisparity.type()==CV_16SC1)
656 for(
int h = 0;
h < imageDisparity.rows &&
h/decimation < (
int)cloud->height;
h+=decimation)
658 for(
int w = 0;
w < imageDisparity.cols &&
w/decimation < (
int)cloud->width;
w+=decimation)
660 float disp =
float(imageDisparity.at<
short>(
h,
w))/16.0f;
662 if(pt.z >= minDepth && (maxDepth <= 0.0f || pt.z <= maxDepth))
664 cloud->at((
h/decimation)*cloud->width + (
w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
667 validIndices->at(oi++) = (
h/decimation)*cloud->width + (
w/decimation);
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());
682 for(
int h = 0;
h < imageDisparity.rows &&
h/decimation < (
int)cloud->height;
h+=decimation)
684 for(
int w = 0;
w < imageDisparity.cols &&
w/decimation < (
int)cloud->width;
w+=decimation)
686 float disp = imageDisparity.at<
float>(
h,
w);
688 if(pt.z > minDepth && (maxDepth <= 0.0f || pt.z <= maxDepth))
690 cloud->at((
h/decimation)*cloud->width + (
w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
693 validIndices->at(oi++) = (
h/decimation)*cloud->width + (
w/decimation);
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());
708 validIndices->resize(oi);
714 const cv::Mat & imageRgb,
715 const cv::Mat & imageDisparity,
720 std::vector<int> * validIndices)
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);
729 if(imageDisparity.rows % decimation != 0 || imageDisparity.cols % decimation != 0)
731 int oldDecimation = decimation;
732 while(decimation >= 1)
734 if(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0)
741 if(imageDisparity.rows % oldDecimation != 0 || imageDisparity.cols % oldDecimation != 0)
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);
747 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
750 if(imageRgb.channels() == 3)
760 cloud->height = imageRgb.rows/decimation;
761 cloud->width = imageRgb.cols/decimation;
762 cloud->is_dense =
false;
763 cloud->resize(cloud->height * cloud->width);
766 validIndices->resize(cloud->size());
770 for(
int h = 0;
h < imageRgb.rows &&
h/decimation < (
int)cloud->height;
h+=decimation)
772 for(
int w = 0;
w < imageRgb.cols &&
w/decimation < (
int)cloud->width;
w+=decimation)
774 pcl::PointXYZRGB & pt = cloud->at((
h/decimation)*cloud->width + (
w/decimation));
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];
783 unsigned char v = imageRgb.at<
unsigned char>(
h,
w);
789 float disp = imageDisparity.type()==CV_16SC1?
float(imageDisparity.at<
short>(
h,
w))/16.0f:imageDisparity.at<
float>(
h,
w);
791 if(
util3d::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
798 validIndices->at(oi++) = (
h/decimation)*cloud->width + (
w/decimation);
803 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
809 validIndices->resize(oi);
815 const cv::Mat & imageLeft,
816 const cv::Mat & imageRight,
821 std::vector<int> * validIndices,
824 UASSERT(!imageLeft.empty() && !imageRight.empty());
825 UASSERT(imageRight.type() == CV_8UC1);
826 UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1);
827 UASSERT(imageLeft.rows == imageRight.rows &&
828 imageLeft.cols == imageRight.cols);
831 cv::Mat leftColor = imageLeft;
832 cv::Mat rightMono = imageRight;
835 if(leftColor.channels() == 3)
837 cv::cvtColor(leftColor, leftMono, CV_BGR2GRAY);
841 leftMono = leftColor;
859 std::vector<pcl::IndicesPtr> * validIndices,
861 const std::vector<float> & roiRatios)
868 std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
877 clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(
new pcl::PointCloud<pcl::PointXYZ>));
880 validIndices->push_back(pcl::IndicesPtr(
new std::vector<int>()));
884 cv::Mat depth = cv::Mat(sensorData.
depthRaw(), cv::Rect(subImageWidth*
i, 0, subImageWidth, sensorData.
depthRaw().rows));
886 if( roiRatios.size() == 4 &&
887 (roiRatios[0] > 0.0f ||
888 roiRatios[1] > 0.0f ||
889 roiRatios[2] > 0.0f ||
890 roiRatios[3] > 0.0f))
898 if( roiDepth.width%decimation==0 &&
899 roiDepth.height%decimation==0 &&
900 (roiRgb.width != 0 ||
901 (roiRgb.width%decimation==0 &&
902 roiRgb.height%decimation==0)))
904 depth = cv::Mat(depth, roiDepth);
905 if(
model.imageWidth() != 0 &&
model.imageHeight() != 0)
916 UERROR(
"Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
917 "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
918 "by decimation parameter (%d). Ignoring ROI ratios...",
937 validIndices?validIndices->back().get():0);
941 if(!
model.localTransform().isNull() && !
model.localTransform().isIdentity())
950 UERROR(
"Camera model %d is invalid",
i);
960 if(sensorData.
imageRaw().channels() == 3)
962 cv::cvtColor(sensorData.
imageRaw(), leftMono, CV_BGR2GRAY);
974 clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(
new pcl::PointCloud<pcl::PointXYZ>));
977 validIndices->push_back(pcl::IndicesPtr(
new std::vector<int>()));
981 cv::Mat left(leftMono, cv::Rect(subImageWidth*
i, 0, subImageWidth, leftMono.rows));
982 cv::Mat right(sensorData.
rightRaw(), cv::Rect(subImageWidth*
i, 0, subImageWidth, sensorData.
rightRaw().rows));
984 if( roiRatios.size() == 4 &&
985 ((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
986 (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
987 (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
988 (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
991 if( roi.width%decimation==0 &&
992 roi.height%decimation==0)
994 left = cv::Mat(left, roi);
995 right = cv::Mat(right, roi);
1000 UERROR(
"Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
1001 "dimension (left=%dx%d) cannot be divided exactly "
1002 "by decimation parameter (%d). Ignoring ROI ratios...",
1019 validIndices?validIndices->back().get():0);
1023 if(!
model.localTransform().isNull() && !
model.localTransform().isIdentity())
1027 clouds.back() = tmp;
1032 UERROR(
"Stereo camera model %d is invalid",
i);
1045 std::vector<int> * validIndices,
1047 const std::vector<float> & roiRatios)
1049 std::vector<pcl::IndicesPtr> validIndicesV;
1055 validIndices?&validIndicesV:0,
1061 UASSERT(validIndicesV.size() == clouds.size());
1064 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
1066 if(clouds.size() == 1)
1071 *validIndices = *validIndicesV[0];
1076 for(
size_t i=0;
i<clouds.size(); ++
i)
1083 validIndices->resize(cloud->size());
1084 for(
size_t i=0;
i<cloud->size(); ++
i)
1086 validIndices->at(
i) =
i;
1098 std::vector<pcl::IndicesPtr> * validIndices,
1100 const std::vector<float> & roiRatios)
1107 std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
1122 clouds.push_back(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(
new pcl::PointCloud<pcl::PointXYZRGB>));
1125 validIndices->push_back(pcl::IndicesPtr(
new std::vector<int>()));
1129 cv::Mat rgb(sensorData.
imageRaw(), cv::Rect(subRGBWidth*
i, 0, subRGBWidth, sensorData.
imageRaw().rows));
1130 cv::Mat depth(sensorData.
depthRaw(), cv::Rect(subDepthWidth*
i, 0, subDepthWidth, sensorData.
depthRaw().rows));
1132 if( roiRatios.size() == 4 &&
1133 ((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
1134 (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
1135 (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
1136 (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
1140 if( roiDepth.width%decimation==0 &&
1141 roiDepth.height%decimation==0 &&
1142 roiRgb.width%decimation==0 &&
1143 roiRgb.height%decimation==0)
1145 depth = cv::Mat(depth, roiDepth);
1146 rgb = cv::Mat(rgb, roiRgb);
1151 UERROR(
"Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
1152 "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
1153 "by decimation parameter (%d). Ignoring ROI ratios...",
1173 validIndices?validIndices->back().get():0);
1177 if(!
model.localTransform().isNull() && !
model.localTransform().isIdentity())
1181 clouds.back() = tmp;
1186 UERROR(
"Camera model %d is invalid",
i);
1200 clouds.push_back(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(
new pcl::PointCloud<pcl::PointXYZRGB>));
1203 validIndices->push_back(pcl::IndicesPtr(
new std::vector<int>()));
1207 cv::Mat left(sensorData.
imageRaw(), cv::Rect(subImageWidth*
i, 0, subImageWidth, sensorData.
imageRaw().rows));
1208 cv::Mat right(sensorData.
rightRaw(), cv::Rect(subImageWidth*
i, 0, subImageWidth, sensorData.
rightRaw().rows));
1210 if( roiRatios.size() == 4 &&
1211 ((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
1212 (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
1213 (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
1214 (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
1217 if( roi.width%decimation==0 &&
1218 roi.height%decimation==0)
1220 left = cv::Mat(left, roi);
1221 right = cv::Mat(right, roi);
1226 UERROR(
"Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
1227 "dimension (left=%dx%d) cannot be divided exactly "
1228 "by decimation parameter (%d). Ignoring ROI ratios...",
1246 validIndices?validIndices->back().get():0,
1251 if(!
model.localTransform().isNull() && !
model.localTransform().isIdentity())
1255 clouds.back() = tmp;
1260 UERROR(
"Stereo camera model %d is invalid",
i);
1273 std::vector<int> * validIndices,
1275 const std::vector<float> & roiRatios)
1277 std::vector<pcl::IndicesPtr> validIndicesV;
1283 validIndices?&validIndicesV:0,
1289 UASSERT(validIndicesV.size() == clouds.size());
1292 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
1294 if(clouds.size() == 1)
1299 *validIndices = *validIndicesV[0];
1304 for(
size_t i=0;
i<clouds.size(); ++
i)
1311 validIndices->resize(cloud->size());
1312 for(
size_t i=0;
i<cloud->size(); ++
i)
1314 validIndices->at(
i) =
i;
1322 const cv::Mat & depthImage,
1331 UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
1334 pcl::PointCloud<pcl::PointXYZ> scan;
1335 int middle = depthImage.rows/2;
1338 scan.resize(depthImage.cols);
1340 for(
int i=depthImage.cols-1;
i>=0; --
i)
1343 if(pcl::isFinite(pt) && pt.z >= minDepth && (maxDepth == 0 || pt.z < maxDepth))
1358 const cv::Mat & depthImages,
1359 const std::vector<CameraModel> & cameraModels,
1363 pcl::PointCloud<pcl::PointXYZ> scan;
1364 UASSERT(
int((depthImages.cols/cameraModels.size())*cameraModels.size()) == depthImages.cols);
1365 int subImageWidth = depthImages.cols/cameraModels.size();
1366 for(
int i=(
int)cameraModels.size()-1;
i>=0; --
i)
1368 if(cameraModels[
i].isValidForProjection())
1370 cv::Mat depth = cv::Mat(depthImages, cv::Rect(subImageWidth*
i, 0, subImageWidth, depthImages.rows));
1374 cameraModels[
i].
fx(),
1375 cameraModels[
i].
fy(),
1376 cameraModels[
i].
cx(),
1377 cameraModels[
i].
cy(),
1380 cameraModels[
i].localTransform());
1384 UERROR(
"Camera model %d is invalid",
i);
1402 laserScan = cv::Mat(1, (
int)
indices->size(), CV_32FC3);
1403 for(
unsigned int i=0;
i<
indices->size(); ++
i)
1406 if(!filterNaNs || pcl::isFinite(cloud.at(index)))
1408 float * ptr = laserScan.ptr<
float>(0, oi++);
1418 ptr[0] = cloud.at(index).x;
1419 ptr[1] = cloud.at(index).y;
1420 ptr[2] = cloud.at(index).z;
1427 laserScan = cv::Mat(1, (
int)cloud.size(), CV_32FC3);
1428 for(
unsigned int i=0;
i<cloud.size(); ++
i)
1430 if(!filterNaNs || pcl::isFinite(cloud.at(
i)))
1432 float * ptr = laserScan.ptr<
float>(0, oi++);
1442 ptr[0] = cloud.at(
i).x;
1443 ptr[1] = cloud.at(
i).y;
1444 ptr[2] = cloud.at(
i).z;
1467 laserScan = cv::Mat(1, (
int)
indices->size(), CV_32FC(6));
1468 for(
unsigned int i=0;
i<
indices->size(); ++
i)
1471 if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
1476 float * ptr = laserScan.ptr<
float>(0, oi++);
1483 ptr[3] = pt.normal_x;
1484 ptr[4] = pt.normal_y;
1485 ptr[5] = pt.normal_z;
1489 ptr[0] = cloud.at(index).x;
1490 ptr[1] = cloud.at(index).y;
1491 ptr[2] = cloud.at(index).z;
1492 ptr[3] = cloud.at(index).normal_x;
1493 ptr[4] = cloud.at(index).normal_y;
1494 ptr[5] = cloud.at(index).normal_z;
1501 laserScan = cv::Mat(1, (
int)cloud.size(), CV_32FC(6));
1502 for(
unsigned int i=0;
i<cloud.size(); ++
i)
1504 if(!filterNaNs || (pcl::isFinite(cloud.at(
i)) &&
1509 float * ptr = laserScan.ptr<
float>(0, oi++);
1516 ptr[3] = pt.normal_x;
1517 ptr[4] = pt.normal_y;
1518 ptr[5] = pt.normal_z;
1522 ptr[0] = cloud.at(
i).x;
1523 ptr[1] = cloud.at(
i).y;
1524 ptr[2] = cloud.at(
i).z;
1525 ptr[3] = cloud.at(
i).normal_x;
1526 ptr[4] = cloud.at(
i).normal_y;
1527 ptr[5] = cloud.at(
i).normal_z;
1541 UASSERT(cloud.size() == normals.size());
1542 cv::Mat laserScan = cv::Mat(1, (
int)cloud.size(), CV_32FC(6));
1545 for(
unsigned int i=0;
i<cloud.size(); ++
i)
1547 if(!filterNaNs || (pcl::isFinite(cloud.at(
i)) && pcl::isFinite(normals.at(
i))))
1549 float * ptr = laserScan.ptr<
float>(0, oi++);
1552 pcl::PointNormal pt;
1553 pt.x = cloud.at(
i).x;
1554 pt.y = cloud.at(
i).y;
1555 pt.z = cloud.at(
i).z;
1556 pt.normal_x = normals.at(
i).normal_x;
1557 pt.normal_y = normals.at(
i).normal_y;
1558 pt.normal_z = normals.at(
i).normal_z;
1563 ptr[3] = pt.normal_x;
1564 ptr[4] = pt.normal_y;
1565 ptr[5] = pt.normal_z;
1569 ptr[0] = cloud.at(
i).x;
1570 ptr[1] = cloud.at(
i).y;
1571 ptr[2] = cloud.at(
i).z;
1572 ptr[3] = normals.at(
i).normal_x;
1573 ptr[4] = normals.at(
i).normal_y;
1574 ptr[5] = normals.at(
i).normal_z;
1598 laserScan = cv::Mat(1, (
int)
indices->size(), CV_32FC(4));
1599 for(
unsigned int i=0;
i<
indices->size(); ++
i)
1602 if(!filterNaNs || pcl::isFinite(cloud.at(index)))
1604 float * ptr = laserScan.ptr<
float>(0, oi++);
1614 ptr[0] = cloud.at(index).x;
1615 ptr[1] = cloud.at(index).y;
1616 ptr[2] = cloud.at(index).z;
1618 int * ptrInt = (
int*)ptr;
1619 ptrInt[3] =
int(cloud.at(index).b) | (
int(cloud.at(index).g) << 8) | (
int(cloud.at(index).r) << 16);
1625 laserScan = cv::Mat(1, (
int)cloud.size(), CV_32FC(4));
1626 for(
unsigned int i=0;
i<cloud.size(); ++
i)
1628 if(!filterNaNs || pcl::isFinite(cloud.at(
i)))
1630 float * ptr = laserScan.ptr<
float>(0, oi++);
1640 ptr[0] = cloud.at(
i).x;
1641 ptr[1] = cloud.at(
i).y;
1642 ptr[2] = cloud.at(
i).z;
1644 int * ptrInt = (
int*)ptr;
1645 ptrInt[3] =
int(cloud.at(
i).b) | (
int(cloud.at(
i).g) << 8) | (
int(cloud.at(
i).r) << 16);
1669 laserScan = cv::Mat(1, (
int)
indices->size(), CV_32FC(4));
1670 for(
unsigned int i=0;
i<
indices->size(); ++
i)
1673 if(!filterNaNs || pcl::isFinite(cloud.at(index)))
1675 float * ptr = laserScan.ptr<
float>(0, oi++);
1685 ptr[0] = cloud.at(index).x;
1686 ptr[1] = cloud.at(index).y;
1687 ptr[2] = cloud.at(index).z;
1689 ptr[3] = cloud.at(index).intensity;
1695 laserScan = cv::Mat(1, (
int)cloud.size(), CV_32FC(4));
1696 for(
unsigned int i=0;
i<cloud.size(); ++
i)
1698 if(!filterNaNs || pcl::isFinite(cloud.at(
i)))
1700 float * ptr = laserScan.ptr<
float>(0, oi++);
1710 ptr[0] = cloud.at(
i).x;
1711 ptr[1] = cloud.at(
i).y;
1712 ptr[2] = cloud.at(
i).z;
1714 ptr[3] = cloud.at(
i).intensity;
1727 UASSERT(cloud.size() == normals.size());
1728 cv::Mat laserScan(1, (
int)cloud.size(), CV_32FC(7));
1731 for(
unsigned int i=0;
i<cloud.size(); ++
i)
1733 if(!filterNaNs || pcl::isFinite(cloud.at(
i)))
1735 float * ptr = laserScan.ptr<
float>(0, oi++);
1738 pcl::PointXYZRGBNormal pt;
1739 pt.x = cloud.at(
i).x;
1740 pt.y = cloud.at(
i).y;
1741 pt.z = cloud.at(
i).z;
1742 pt.normal_x = normals.at(
i).normal_x;
1743 pt.normal_y = normals.at(
i).normal_y;
1744 pt.normal_z = normals.at(
i).normal_z;
1749 ptr[4] = pt.normal_x;
1750 ptr[5] = pt.normal_y;
1751 ptr[6] = pt.normal_z;
1755 ptr[0] = cloud.at(
i).x;
1756 ptr[1] = cloud.at(
i).y;
1757 ptr[2] = cloud.at(
i).z;
1758 ptr[4] = normals.at(
i).normal_x;
1759 ptr[5] = normals.at(
i).normal_y;
1760 ptr[6] = normals.at(
i).normal_z;
1762 int * ptrInt = (
int*)ptr;
1763 ptrInt[3] =
int(cloud.at(
i).b) | (
int(cloud.at(
i).g) << 8) | (
int(cloud.at(
i).r) << 16);
1784 laserScan = cv::Mat(1, (
int)
indices->size(), CV_32FC(7));
1785 for(
unsigned int i=0;
i<
indices->size(); ++
i)
1788 if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
1793 float * ptr = laserScan.ptr<
float>(0, oi++);
1800 ptr[4] = pt.normal_x;
1801 ptr[5] = pt.normal_y;
1802 ptr[6] = pt.normal_z;
1806 ptr[0] = cloud.at(index).x;
1807 ptr[1] = cloud.at(index).y;
1808 ptr[2] = cloud.at(index).z;
1809 ptr[4] = cloud.at(index).normal_x;
1810 ptr[5] = cloud.at(index).normal_y;
1811 ptr[6] = cloud.at(index).normal_z;
1813 int * ptrInt = (
int*)ptr;
1814 ptrInt[3] =
int(cloud.at(index).b) | (
int(cloud.at(index).g) << 8) | (
int(cloud.at(index).r) << 16);
1820 laserScan = cv::Mat(1, (
int)cloud.size(), CV_32FC(7));
1821 for(
unsigned int i=0;
i<cloud.size(); ++
i)
1823 if(!filterNaNs || (pcl::isFinite(cloud.at(
i)) &&
1828 float * ptr = laserScan.ptr<
float>(0, oi++);
1835 ptr[4] = pt.normal_x;
1836 ptr[5] = pt.normal_y;
1837 ptr[6] = pt.normal_z;
1841 ptr[0] = cloud.at(
i).x;
1842 ptr[1] = cloud.at(
i).y;
1843 ptr[2] = cloud.at(
i).z;
1844 ptr[4] = cloud.at(
i).normal_x;
1845 ptr[5] = cloud.at(
i).normal_y;
1846 ptr[6] = cloud.at(
i).normal_z;
1848 int * ptrInt = (
int*)ptr;
1849 ptrInt[3] =
int(cloud.at(
i).b) | (
int(cloud.at(
i).g) << 8) | (
int(cloud.at(
i).r) << 16);
1862 UASSERT(cloud.size() == normals.size());
1863 cv::Mat laserScan(1, (
int)cloud.size(), CV_32FC(7));
1866 for(
unsigned int i=0;
i<cloud.size(); ++
i)
1868 if(!filterNaNs || (pcl::isFinite(cloud.at(
i)) && pcl::isFinite(normals.at(
i))))
1870 float * ptr = laserScan.ptr<
float>(0, oi++);
1873 pcl::PointXYZINormal pt;
1874 pt.x = cloud.at(
i).x;
1875 pt.y = cloud.at(
i).y;
1876 pt.z = cloud.at(
i).z;
1877 pt.normal_x = normals.at(
i).normal_x;
1878 pt.normal_y = normals.at(
i).normal_y;
1879 pt.normal_z = normals.at(
i).normal_z;
1884 ptr[4] = pt.normal_x;
1885 ptr[5] = pt.normal_y;
1886 ptr[6] = pt.normal_z;
1890 ptr[0] = cloud.at(
i).x;
1891 ptr[1] = cloud.at(
i).y;
1892 ptr[2] = cloud.at(
i).z;
1893 ptr[4] = normals.at(
i).normal_x;
1894 ptr[5] = normals.at(
i).normal_y;
1895 ptr[6] = normals.at(
i).normal_z;
1897 ptr[3] = cloud.at(
i).intensity;
1918 laserScan = cv::Mat(1, (
int)
indices->size(), CV_32FC(7));
1919 for(
unsigned int i=0;
i<
indices->size(); ++
i)
1922 if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
1927 float * ptr = laserScan.ptr<
float>(0, oi++);
1934 ptr[4] = pt.normal_x;
1935 ptr[5] = pt.normal_y;
1936 ptr[6] = pt.normal_z;
1940 ptr[0] = cloud.at(index).x;
1941 ptr[1] = cloud.at(index).y;
1942 ptr[2] = cloud.at(index).z;
1943 ptr[4] = cloud.at(index).normal_x;
1944 ptr[5] = cloud.at(index).normal_y;
1945 ptr[6] = cloud.at(index).normal_z;
1947 ptr[3] = cloud.at(
i).intensity;
1953 laserScan = cv::Mat(1, (
int)cloud.size(), CV_32FC(7));
1954 for(
unsigned int i=0;
i<cloud.size(); ++
i)
1956 if(!filterNaNs || (pcl::isFinite(cloud.at(
i)) &&
1961 float * ptr = laserScan.ptr<
float>(0, oi++);
1968 ptr[4] = pt.normal_x;
1969 ptr[5] = pt.normal_y;
1970 ptr[6] = pt.normal_z;
1974 ptr[0] = cloud.at(
i).x;
1975 ptr[1] = cloud.at(
i).y;
1976 ptr[2] = cloud.at(
i).z;
1977 ptr[4] = cloud.at(
i).normal_x;
1978 ptr[5] = cloud.at(
i).normal_y;
1979 ptr[6] = cloud.at(
i).normal_z;
1981 ptr[3] = cloud.at(
i).intensity;
1994 cv::Mat laserScan(1, (
int)cloud.size(), CV_32FC2);
1995 bool nullTransform =
transform.isNull();
1998 for(
unsigned int i=0;
i<cloud.size(); ++
i)
2000 if(!filterNaNs || pcl::isFinite(cloud.at(
i)))
2002 float * ptr = laserScan.ptr<
float>(0, oi++);
2011 ptr[0] = cloud.at(
i).x;
2012 ptr[1] = cloud.at(
i).y;
2026 cv::Mat laserScan(1, (
int)cloud.size(), CV_32FC3);
2027 bool nullTransform =
transform.isNull();
2030 for(
unsigned int i=0;
i<cloud.size(); ++
i)
2032 if(!filterNaNs || pcl::isFinite(cloud.at(
i)))
2034 float * ptr = laserScan.ptr<
float>(0, oi++);
2040 ptr[2] = pt.intensity;
2044 ptr[0] = cloud.at(
i).x;
2045 ptr[1] = cloud.at(
i).y;
2046 ptr[2] = cloud.at(
i).intensity;
2060 cv::Mat laserScan(1, (
int)cloud.size(), CV_32FC(5));
2061 bool nullTransform =
transform.isNull();
2063 for(
unsigned int i=0;
i<cloud.size(); ++
i)
2065 if(!filterNaNs || (pcl::isFinite(cloud.at(
i)) &&
2070 float * ptr = laserScan.ptr<
float>(0, oi++);
2076 ptr[2] = pt.normal_x;
2077 ptr[3] = pt.normal_y;
2078 ptr[4] = pt.normal_z;
2082 const pcl::PointNormal & pt = cloud.at(
i);
2085 ptr[2] = pt.normal_x;
2086 ptr[3] = pt.normal_y;
2087 ptr[4] = pt.normal_z;
2100 UASSERT(cloud.size() == normals.size());
2101 cv::Mat laserScan(1, (
int)cloud.size(), CV_32FC(5));
2104 for(
unsigned int i=0;
i<cloud.size(); ++
i)
2106 if(!filterNaNs || (pcl::isFinite(cloud.at(
i)) && pcl::isFinite(normals.at(
i))))
2108 float * ptr = laserScan.ptr<
float>(0, oi++);
2111 pcl::PointNormal pt;
2112 pt.x = cloud.at(
i).x;
2113 pt.y = cloud.at(
i).y;
2114 pt.z = cloud.at(
i).z;
2115 pt.normal_x = normals.at(
i).normal_x;
2116 pt.normal_y = normals.at(
i).normal_y;
2117 pt.normal_z = normals.at(
i).normal_z;
2121 ptr[2] = pt.normal_x;
2122 ptr[3] = pt.normal_y;
2123 ptr[4] = pt.normal_z;
2127 ptr[0] = cloud.at(
i).x;
2128 ptr[1] = cloud.at(
i).y;
2129 ptr[2] = normals.at(
i).normal_x;
2130 ptr[3] = normals.at(
i).normal_y;
2131 ptr[4] = normals.at(
i).normal_z;
2144 cv::Mat laserScan(1, (
int)cloud.size(), CV_32FC(6));
2145 bool nullTransform =
transform.isNull();
2147 for(
unsigned int i=0;
i<cloud.size(); ++
i)
2149 if(!filterNaNs || (pcl::isFinite(cloud.at(
i)) &&
2154 float * ptr = laserScan.ptr<
float>(0, oi++);
2160 ptr[2] = pt.intensity;
2161 ptr[3] = pt.normal_x;
2162 ptr[4] = pt.normal_y;
2163 ptr[5] = pt.normal_z;
2167 const pcl::PointXYZINormal & pt = cloud.at(
i);
2170 ptr[2] = pt.intensity;
2171 ptr[3] = pt.normal_x;
2172 ptr[4] = pt.normal_y;
2173 ptr[5] = pt.normal_z;
2186 UASSERT(cloud.size() == normals.size());
2187 cv::Mat laserScan(1, (
int)cloud.size(), CV_32FC(6));
2190 for(
unsigned int i=0;
i<cloud.size(); ++
i)
2192 if(!filterNaNs || (pcl::isFinite(cloud.at(
i)) && pcl::isFinite(normals.at(
i))))
2194 float * ptr = laserScan.ptr<
float>(0, oi++);
2197 pcl::PointXYZINormal pt;
2198 pt.x = cloud.at(
i).x;
2199 pt.y = cloud.at(
i).y;
2200 pt.z = cloud.at(
i).z;
2201 pt.normal_x = normals.at(
i).normal_x;
2202 pt.normal_y = normals.at(
i).normal_y;
2203 pt.normal_z = normals.at(
i).normal_z;
2207 ptr[2] = pt.intensity;
2208 ptr[3] = pt.normal_x;
2209 ptr[4] = pt.normal_y;
2210 ptr[5] = pt.normal_z;
2214 ptr[0] = cloud.at(
i).x;
2215 ptr[1] = cloud.at(
i).y;
2216 ptr[2] = cloud.at(
i).intensity;
2217 ptr[3] = normals.at(
i).normal_x;
2218 ptr[4] = normals.at(
i).normal_y;
2219 ptr[5] = normals.at(
i).normal_z;
2232 pcl::PCLPointCloud2::Ptr cloud(
new pcl::PCLPointCloud2);
2264 UERROR(
"Unknown conversion from LaserScan format %d to PointCloud2.", laserScan.
format());
2271 pcl::PointCloud<pcl::PointXYZ>::Ptr output(
new pcl::PointCloud<pcl::PointXYZ>);
2274 output->width = laserScan.
data().cols;
2275 output->height = laserScan.
data().rows;
2276 output->is_dense =
false;
2280 output->is_dense =
true;
2282 output->resize(laserScan.
size());
2283 bool nullTransform =
transform.isNull();
2285 for(
int i=0;
i<laserScan.
size(); ++
i)
2298 pcl::PointCloud<pcl::PointNormal>::Ptr output(
new pcl::PointCloud<pcl::PointNormal>);
2301 output->width = laserScan.
data().cols;
2302 output->height = laserScan.
data().rows;
2303 output->is_dense =
false;
2307 output->is_dense =
true;
2309 output->resize(laserScan.
size());
2310 bool nullTransform =
transform.isNull();
2311 for(
int i=0;
i<laserScan.
size(); ++
i)
2324 pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(
new pcl::PointCloud<pcl::PointXYZRGB>);
2327 output->width = laserScan.
data().cols;
2328 output->height = laserScan.
data().rows;
2329 output->is_dense =
false;
2333 output->is_dense =
true;
2335 output->resize(laserScan.
size());
2338 for(
int i=0;
i<laserScan.
size(); ++
i)
2351 pcl::PointCloud<pcl::PointXYZI>::Ptr output(
new pcl::PointCloud<pcl::PointXYZI>);
2354 output->width = laserScan.
data().cols;
2355 output->height = laserScan.
data().rows;
2356 output->is_dense =
false;
2360 output->is_dense =
true;
2362 output->resize(laserScan.
size());
2365 for(
int i=0;
i<laserScan.
size(); ++
i)
2378 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(
new pcl::PointCloud<pcl::PointXYZRGBNormal>);
2381 output->width = laserScan.
data().cols;
2382 output->height = laserScan.
data().rows;
2383 output->is_dense =
false;
2387 output->is_dense =
true;
2389 output->resize(laserScan.
size());
2391 for(
int i=0;
i<laserScan.
size(); ++
i)
2404 pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(
new pcl::PointCloud<pcl::PointXYZINormal>);
2407 output->width = laserScan.
data().cols;
2408 output->height = laserScan.
data().rows;
2409 output->is_dense =
false;
2413 output->is_dense =
true;
2415 output->resize(laserScan.
size());
2417 for(
int i=0;
i<laserScan.
size(); ++
i)
2431 pcl::PointXYZ output;
2432 int row = index / laserScan.
data().cols;
2433 const float * ptr = laserScan.
data().ptr<
float>(
row, index -
row*laserScan.
data().cols);
2436 if(!laserScan.
is2d())
2446 pcl::PointNormal output;
2447 int row = index / laserScan.
data().cols;
2448 const float * ptr = laserScan.
data().ptr<
float>(
row, index -
row*laserScan.
data().cols);
2451 if(!laserScan.
is2d())
2458 output.normal_x = ptr[
offset];
2459 output.normal_y = ptr[
offset+1];
2460 output.normal_z = ptr[
offset+2];
2468 pcl::PointXYZRGB output;
2469 int row = index / laserScan.
data().cols;
2470 const float * ptr = laserScan.
data().ptr<
float>(
row, index -
row*laserScan.
data().cols);
2473 if(!laserScan.
is2d())
2480 int * ptrInt = (
int*)ptr;
2482 output.b = (
unsigned char)(ptrInt[indexRGB] & 0xFF);
2483 output.g = (
unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF);
2484 output.r = (
unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF);
2489 int * ptrInt = (
int*)ptr;
2491 output.r = (
unsigned char)(ptrInt[indexIntensity] & 0xFF);
2492 output.g = (
unsigned char)((ptrInt[indexIntensity] >> 8) & 0xFF);
2493 output.b = (
unsigned char)((ptrInt[indexIntensity] >> 16) & 0xFF);
2494 output.a = (
unsigned char)((ptrInt[indexIntensity] >> 24) & 0xFF);
2508 pcl::PointXYZI output;
2509 int row = index / laserScan.
data().cols;
2510 const float * ptr = laserScan.
data().ptr<
float>(
row, index -
row*laserScan.
data().cols);
2513 if(!laserScan.
is2d())
2521 output.intensity = ptr[
offset];
2525 output.intensity = intensity;
2534 pcl::PointXYZRGBNormal output;
2535 int row = index / laserScan.
data().cols;
2536 const float * ptr = laserScan.
data().ptr<
float>(
row, index -
row*laserScan.
data().cols);
2539 if(!laserScan.
is2d())
2546 int * ptrInt = (
int*)ptr;
2548 output.b = (
unsigned char)(ptrInt[indexRGB] & 0xFF);
2549 output.g = (
unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF);
2550 output.r = (
unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF);
2554 int * ptrInt = (
int*)ptr;
2556 output.r = (
unsigned char)(ptrInt[indexIntensity] & 0xFF);
2557 output.g = (
unsigned char)((ptrInt[indexIntensity] >> 8) & 0xFF);
2558 output.b = (
unsigned char)((ptrInt[indexIntensity] >> 16) & 0xFF);
2559 output.a = (
unsigned char)((ptrInt[indexIntensity] >> 24) & 0xFF);
2571 output.normal_x = ptr[
offset];
2572 output.normal_y = ptr[
offset+1];
2573 output.normal_z = ptr[
offset+2];
2582 pcl::PointXYZINormal output;
2583 int row = index / laserScan.
data().cols;
2584 const float * ptr = laserScan.
data().ptr<
float>(
row, index -
row*laserScan.
data().cols);
2587 if(!laserScan.
is2d())
2595 output.intensity = ptr[
offset];
2599 output.intensity = intensity;
2605 output.normal_x = ptr[
offset];
2606 output.normal_y = ptr[
offset+1];
2607 output.normal_z = ptr[
offset+2];
2616 UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
2618 const float * ptr = laserScan.ptr<
float>(0, 0);
2621 bool is3d = laserScan.channels() >= 3 && laserScan.channels() != 5;
2622 min.z =
max.z = is3d?ptr[2]:0.0f;
2623 for(
int i=1;
i<laserScan.cols; ++
i)
2625 ptr = laserScan.ptr<
float>(0,
i);
2627 if(ptr[0] <
min.x)
min.x = ptr[0];
2628 else if(ptr[0] >
max.x)
max.x = ptr[0];
2630 if(ptr[1] <
min.y)
min.y = ptr[1];
2631 else if(ptr[1] >
max.y)
max.y = ptr[1];
2635 if(ptr[2] <
min.z)
min.z = ptr[2];
2636 else if(ptr[2] >
max.z)
max.z = ptr[2];
2642 cv::Point3f minCV, maxCV;
2654 const cv::Point2f & pt,
2658 if(disparity > 0.0
f &&
model.baseline() > 0.0f &&
model.left().fx() > 0.0f)
2662 if(
model.right().cx()>0.0f &&
model.left().cx()>0.0f)
2666 float W =
model.baseline()/(disparity +
c);
2667 return cv::Point3f((pt.x -
model.left().cx())*
W,
2670 float bad_point = std::numeric_limits<float>::quiet_NaN ();
2671 return cv::Point3f(bad_point, bad_point, bad_point);
2675 const cv::Point2f & pt,
2676 const cv::Mat & disparity,
2679 UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
2680 int u =
int(pt.x+0.5f);
2681 int v =
int(pt.y+0.5f);
2682 float bad_point = std::numeric_limits<float>::quiet_NaN ();
2686 float d = disparity.type() == CV_16SC1?
float(disparity.at<
short>(
v,u))/16.0f:disparity.at<
float>(
v,u);
2689 return cv::Point3f(bad_point, bad_point, bad_point);
2694 const cv::Size & imageSize,
2695 const cv::Mat & cameraMatrixK,
2696 const cv::Mat & laserScan,
2701 UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
2702 UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
2704 float fx = cameraMatrixK.at<
double>(0,0);
2705 float fy = cameraMatrixK.at<
double>(1,1);
2706 float cx = cameraMatrixK.at<
double>(0,2);
2707 float cy = cameraMatrixK.at<
double>(1,2);
2709 cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
2713 for(
int i=0;
i<laserScan.cols; ++
i)
2715 const float* ptr = laserScan.ptr<
float>(0,
i);
2719 if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC(5))
2740 float invZ = 1.0f/
z;
2741 float dx = (
fx*ptScan.x)*invZ +
cx;
2742 float dy = (
fy*ptScan.y)*invZ +
cy;
2745 int dx_high = dx + 0.5f;
2746 int dy_high = dy + 0.5f;
2750 float &zReg = registered.at<
float>(dy_low, dx_low);
2751 if(zReg == 0 ||
z < zReg)
2757 if((dx_low != dx_high || dy_low != dy_high) &&
2760 float &zReg = registered.at<
float>(dy_high, dx_high);
2761 if(zReg == 0 ||
z < zReg)
2773 UDEBUG(
"Points in camera=%d/%d",
count, laserScan.cols);
2779 const cv::Size & imageSize,
2780 const cv::Mat & cameraMatrixK,
2781 const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan,
2786 UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
2788 float fx = cameraMatrixK.at<
double>(0,0);
2789 float fy = cameraMatrixK.at<
double>(1,1);
2790 float cx = cameraMatrixK.at<
double>(0,2);
2791 float cy = cameraMatrixK.at<
double>(1,2);
2793 cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
2797 for(
int i=0;
i<(
int)laserScan->size(); ++
i)
2800 pcl::PointXYZ ptScan = laserScan->at(
i);
2808 float invZ = 1.0f/
z;
2809 float dx = (
fx*ptScan.x)*invZ +
cx;
2810 float dy = (
fy*ptScan.y)*invZ +
cy;
2813 int dx_high = dx + 0.5f;
2814 int dy_high = dy + 0.5f;
2818 float &zReg = registered.at<
float>(dy_low, dx_low);
2819 if(zReg == 0 ||
z < zReg)
2824 if((dx_low != dx_high || dy_low != dy_high) &&
2828 float &zReg = registered.at<
float>(dy_high, dx_high);
2829 if(zReg == 0 ||
z < zReg)
2840 UDEBUG(
"Points in camera=%d/%d",
count, (
int)laserScan->size());
2846 const cv::Size & imageSize,
2847 const cv::Mat & cameraMatrixK,
2848 const pcl::PCLPointCloud2::Ptr laserScan,
2852 UASSERT(!laserScan->data.empty());
2853 UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
2855 float fx = cameraMatrixK.at<
double>(0,0);
2856 float fy = cameraMatrixK.at<
double>(1,1);
2857 float cx = cameraMatrixK.at<
double>(0,2);
2858 float cy = cameraMatrixK.at<
double>(1,2);
2860 cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
2863 pcl::MsgFieldMap field_map;
2864 pcl::createMapping<pcl::PointXYZ> (laserScan->fields, field_map);
2867 if(field_map.size() == 1)
2871 const uint8_t* row_data = &laserScan->data[
row * laserScan->row_step];
2874 const uint8_t* msg_data = row_data +
col * laserScan->point_step;
2875 pcl::PointXYZ ptScan;
2876 memcpy (&ptScan, msg_data + field_map.front().serialized_offset, field_map.front().size);
2884 float invZ = 1.0f/
z;
2885 float dx = (
fx*ptScan.x)*invZ +
cx;
2886 float dy = (
fy*ptScan.y)*invZ +
cy;
2889 int dx_high = dx + 0.5f;
2890 int dy_high = dy + 0.5f;
2894 float &zReg = registered.at<
float>(dy_low, dx_low);
2895 if(zReg == 0 ||
z < zReg)
2900 if((dx_low != dx_high || dy_low != dy_high) &&
2904 float &zReg = registered.at<
float>(dy_high, dx_high);
2905 if(zReg == 0 ||
z < zReg)
2920 UERROR(
"field map pcl::pointXYZ not found!");
2922 UDEBUG(
"Points in camera=%d/%d",
count, (
int)laserScan->data.size());
2929 UASSERT(registeredDepth.type() == CV_32FC1);
2930 if(verticalDirection)
2933 for(
int x=0;
x<registeredDepth.cols; ++
x)
2935 float valueA = 0.0f;
2937 for(
int y=0;
y<registeredDepth.rows; ++
y)
2939 float v = registeredDepth.at<
float>(
y,
x);
2940 if(fillToBorder &&
y == registeredDepth.rows-1 && v<=0.0f && indexA>=0)
2946 if(fillToBorder && indexA < 0)
2956 float slope = (
v-valueA)/(
range);
2957 for(
int k=1; k<
range; ++k)
2959 registeredDepth.at<
float>(indexA+k,
x) = valueA+slope*
float(k);
2972 for(
int y=0;
y<registeredDepth.rows; ++
y)
2974 float valueA = 0.0f;
2976 for(
int x=0;
x<registeredDepth.cols; ++
x)
2978 float v = registeredDepth.at<
float>(
y,
x);
2979 if(fillToBorder &&
x == registeredDepth.cols-1 && v<=0.0f && indexA>=0)
2985 if(fillToBorder && indexA < 0)
2995 float slope = (
v-valueA)/(
range);
2996 for(
int k=1; k<
range; ++k)
2998 registeredDepth.at<
float>(
y,indexA+k) = valueA+slope*
float(k);
3027 template<
class Po
intT>
3029 const typename pcl::PointCloud<PointT> & cloud,
3030 const std::map<int, Transform> & cameraPoses,
3031 const std::map<
int, std::vector<CameraModel> > & cameraModels,
3034 const std::vector<float> & roiRatios,
3035 const cv::Mat & projMask,
3036 bool distanceToCamPolicy,
3039 UINFO(
"cloud=%d points", (
int)cloud.size());
3040 UINFO(
"cameraPoses=%d", (
int)cameraPoses.size());
3041 UINFO(
"cameraModels=%d", (
int)cameraModels.size());
3042 UINFO(
"maxDistance=%f", maxDistance);
3043 UINFO(
"maxAngle=%f", maxAngle);
3044 UINFO(
"distanceToCamPolicy=%s", distanceToCamPolicy?
"true":
"false");
3045 UINFO(
"roiRatios=%s", roiRatios.size() == 4?
uFormat(
"%f %f %f %f", roiRatios[0], roiRatios[1], roiRatios[2], roiRatios[3]).
c_str():
"");
3046 UINFO(
"projMask=%dx%d", projMask.cols, projMask.rows);
3047 std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
3049 if (cloud.empty() || cameraPoses.empty() || cameraModels.empty())
3050 return pointToPixel;
3052 std::string
msg =
uFormat(
"Computing visible points per cam (%d points, %d cams)", (
int)cloud.size(), (
int)cameraPoses.size());
3057 UWARN(
"Projecting to cameras cancelled!");
3058 return pointToPixel;
3061 std::vector<ProjectionInfo> invertedIndex(cloud.size());
3062 int cameraProcessed = 0;
3063 bool wrongMaskFormatWarned =
false;
3064 for(std::map<int, Transform>::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter)
3066 std::map<int, std::vector<CameraModel> >::const_iterator
iter=cameraModels.find(pter->first);
3067 if(
iter!=cameraModels.end() && !
iter->second.empty())
3069 cv::Mat validProjMask;
3070 if(!projMask.empty())
3072 if(projMask.type() != CV_8UC1)
3074 if(!wrongMaskFormatWarned)
3075 UERROR(
"Wrong camera projection mask type %d, should be CV_8UC1", projMask.type());
3076 wrongMaskFormatWarned =
true;
3078 else if(projMask.cols ==
iter->second[0].imageWidth() * (
int)
iter->second.size() &&
3079 projMask.rows ==
iter->second[0].imageHeight())
3081 validProjMask = projMask;
3085 UWARN(
"Camera projection mask (%dx%d) is not valid for current "
3086 "camera model(s) (count=%ld, image size=%dx%d). It will be "
3087 "ignored for node %d",
3088 projMask.cols, projMask.rows,
3089 iter->second.size(),
3090 iter->second[0].imageWidth(),
3091 iter->second[0].imageHeight(),
3096 for(
size_t camIndex=0; camIndex<
iter->second.size(); ++camIndex)
3098 Transform cameraTransform = (pter->second *
iter->second[camIndex].localTransform());
3100 cv::Mat cameraMatrixK =
iter->second[camIndex].K();
3101 UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
3102 const cv::Size & imageSize =
iter->second[camIndex].imageSize();
3104 float fx = cameraMatrixK.at<
double>(0,0);
3105 float fy = cameraMatrixK.at<
double>(1,1);
3106 float cx = cameraMatrixK.at<
double>(0,2);
3107 float cy = cameraMatrixK.at<
double>(1,2);
3110 cv::Mat registered = cv::Mat::zeros(imageSize, CV_32SC2);
3113 cv::Rect roi(0,0,imageSize.width, imageSize.height);
3114 if(roiRatios.size()==4)
3120 for(
size_t i=0;
i<cloud.size(); ++
i)
3123 PointT ptScan = cloud.at(
i);
3129 if(
z > 0.0
f && (maxDistance<=0 ||
z<maxDistance))
3131 float invZ = 1.0f/
z;
3132 float dx = (
fx*ptScan.x)*invZ +
cx;
3133 float dy = (
fy*ptScan.y)*invZ +
cy;
3136 int dx_high = dx + 0.5f;
3137 int dy_high = dy + 0.5f;
3140 (validProjMask.empty() || validProjMask.at<
unsigned char>(dy_low, imageSize.width*camIndex+dx_low) > 0))
3143 cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_low, dx_low);
3144 if(zReg[0] == 0 || zMM < zReg[0])
3150 if((dx_low != dx_high || dy_low != dy_high) &&
3152 (validProjMask.empty() || validProjMask.at<
unsigned char>(dy_high, imageSize.width*camIndex+dx_high) > 0))
3155 cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_high, dx_high);
3156 if(zReg[0] == 0 || zMM < zReg[0])
3170 registered = cv::Mat();
3171 UINFO(
"No points projected in camera %d/%d", pter->first, camIndex);
3175 UDEBUG(
"%d points projected in camera %d/%d",
count, pter->first, camIndex);
3177 for(
int u=0; u<registered.cols; ++u)
3179 for(
int v=0;
v<registered.rows; ++
v)
3181 cv::Vec2i &zReg = registered.at<cv::Vec2i>(
v, u);
3185 info.nodeID = pter->first;
3186 info.cameraIndex = camIndex;
3190 const PointT & pt = cloud.at(zReg[1]);
3191 Eigen::Vector4f camDir(
cam.x()-pt.x,
cam.y()-pt.y,
cam.z()-pt.z, 0);
3192 Eigen::Vector4f
normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
3193 float angleToCam = maxAngle<=0?0:pcl::getAngle3D(
normal, camDir);
3194 float distanceToCam = zReg[0]/1000.0f;
3195 if( (maxAngle<=0 || (camDir.dot(
normal) > 0 && angleToCam < maxAngle)) &&
3196 (maxDistance<=0 || distanceToCam<maxDistance))
3198 float vx =
info.uv.x-0.5f;
3199 float vy =
info.uv.y-0.5f;
3201 float distanceToCenter =
vx*
vx+
vy*
vy;
3203 if(distanceToCamPolicy)
3210 if(invertedIndex[zReg[1]].
distance != -1.0
f)
3214 invertedIndex[zReg[1]] =
info;
3219 invertedIndex[zReg[1]] =
info;
3228 msg =
uFormat(
"Processed camera %d/%d", (
int)cameraProcessed+1, (
int)cameraPoses.size());
3233 UWARN(
"Projecting to cameras cancelled!");
3234 return pointToPixel;
3239 msg =
uFormat(
"Select best camera for %d points...", (
int)cloud.size());
3244 UWARN(
"Projecting to cameras cancelled!");
3245 return pointToPixel;
3248 pointToPixel.resize(invertedIndex.size());
3252 for(
size_t i=0;
i<invertedIndex.size(); ++
i)
3255 int cameraIndex = -1;
3256 pcl::PointXY uv_coords;
3259 nodeID = invertedIndex[
i].nodeID;
3260 cameraIndex = invertedIndex[
i].cameraIndex;
3261 uv_coords = invertedIndex[
i].uv;
3264 if(nodeID>-1 && cameraIndex> -1)
3266 pointToPixel[
i].first.first = nodeID;
3267 pointToPixel[
i].first.second = cameraIndex;
3268 pointToPixel[
i].second = uv_coords;
3273 msg =
uFormat(
"Process %d points...done! (%d [%d%%] projected in cameras)", (
int)cloud.size(), colorized, colorized*100/cloud.size());
3280 return pointToPixel;
3284 const typename pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
3285 const std::map<int, Transform> & cameraPoses,
3286 const std::map<
int, std::vector<CameraModel> > & cameraModels,
3289 const std::vector<float> & roiRatios,
3290 const cv::Mat & projMask,
3291 bool distanceToCamPolicy,
3301 distanceToCamPolicy,
3306 const typename pcl::PointCloud<pcl::PointXYZINormal> & cloud,
3307 const std::map<int, Transform> & cameraPoses,
3308 const std::map<
int, std::vector<CameraModel> > & cameraModels,
3311 const std::vector<float> & roiRatios,
3312 const cv::Mat & projMask,
3313 bool distanceToCamPolicy,
3323 distanceToCamPolicy,
3332 pcl::PointCloud<pcl::PointXYZ>::Ptr
concatenateClouds(
const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds)
3334 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
3335 for(std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr>::const_iterator
iter = clouds.begin();
iter!=clouds.end(); ++
iter)
3342 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
concatenateClouds(
const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds)
3344 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
3345 for(std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator
iter = clouds.begin();
iter!=clouds.end(); ++
iter)
3352 pcl::IndicesPtr
concatenate(
const std::vector<pcl::IndicesPtr> & indices)
3355 unsigned int totalSize = 0;
3360 pcl::IndicesPtr
ind(
new std::vector<int>(totalSize));
3361 unsigned int io = 0;
3372 pcl::IndicesPtr
concatenate(
const pcl::IndicesPtr & indicesA,
const pcl::IndicesPtr & indicesB)
3374 pcl::IndicesPtr
ind(
new std::vector<int>(*indicesA));
3375 ind->resize(
ind->size()+indicesB->size());
3376 unsigned int oi = (
unsigned int)indicesA->size();
3377 for(
unsigned int i=0;
i<indicesB->size(); ++
i)
3379 ind->at(oi++) = indicesB->at(
i);
3385 const std::string & fileName,
3386 const std::multimap<int, pcl::PointXYZ> & words,
3391 pcl::PointCloud<pcl::PointXYZ> cloud;
3392 cloud.resize(words.size());
3394 for(std::multimap<int, pcl::PointXYZ>::const_iterator
iter=words.begin();
iter!=words.end(); ++
iter)
3398 pcl::io::savePCDFile(fileName, cloud);
3403 const std::string & fileName,
3404 const std::multimap<int, cv::Point3f> & words,
3409 pcl::PointCloud<pcl::PointXYZ> cloud;
3410 cloud.resize(words.size());
3412 for(std::multimap<int, cv::Point3f>::const_iterator
iter=words.begin();
iter!=words.end(); ++
iter)
3415 cloud[
i++] = pcl::PointXYZ(pt.x, pt.y, pt.z);
3417 pcl::io::savePCDFile(fileName, cloud);
3431 output = cv::Mat(1, num/
dim, CV_32FC(
dim));
3435 stream = fopen (fileName.c_str(),
"rb");
3436 size_t actualReadNum = fread(output.data,
sizeof(
float),num,
stream);
3437 UASSERT(num == actualReadNum);
3444 pcl::PointCloud<pcl::PointXYZ>::Ptr
loadBINCloud(
const std::string & fileName)
3449 pcl::PointCloud<pcl::PointXYZ>::Ptr
loadBINCloud(
const std::string & fileName,
int dim)
3463 pcl::PCLPointCloud2::Ptr cloud(
new pcl::PCLPointCloud2);
3467 pcl::io::loadPCDFile(
path, *cloud);
3471 pcl::io::loadPLYFile(
path, *cloud);
3473 if(cloud->height > 1)
3475 cloud->is_dense =
false;
3479 if(!cloud->data.empty())
3483 for(
unsigned int i=0;
i<cloud->fields.size(); ++
i)
3485 if(cloud->fields[
i].name.compare(
"z") == 0)
3487 zOffset = cloud->fields[
i].offset;
3496 const uint8_t* row_data = &cloud->data[
row * cloud->row_step];
3499 const uint8_t* msg_data = row_data +
col * cloud->point_step;
3500 float z = *(
float*)(msg_data + zOffset);
3513 const std::string & path,
3519 UDEBUG(
"Loading cloud (step=%d, voxel=%f m) : %s", downsampleStep, voxelSize,
path.c_str());
3521 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
3528 pcl::io::loadPCDFile(
path, *cloud);
3532 pcl::io::loadPLYFile(
path, *cloud);
3534 int previousSize = (
int)cloud->size();
3535 if(downsampleStep > 1 && cloud->size())
3538 UDEBUG(
"Downsampling scan (step=%d): %d -> %d", downsampleStep, previousSize, (
int)cloud->size());
3540 previousSize = (
int)cloud->size();
3541 if(voxelSize > 0.0
f && cloud->size())
3544 UDEBUG(
"Voxel filtering scan (voxel=%f m): %d -> %d", voxelSize, previousSize, (
int)cloud->size());
3560 UERROR(
"velocity should be valid!");
3572 UERROR(
"input scan is empty!");
3581 firstStamp = inputStamp + input.
data().ptr<
float>(0, 0)[offsetTime];
3582 lastStamp = inputStamp + input.
data().ptr<
float>(0, input.
size()-1)[offsetTime];
3584 if(lastStamp <= firstStamp)
3586 UERROR(
"First and last stamps in the scan are the same!");
3593 float vx,
vy,vz, vroll,vpitch,vyaw;
3594 velocity.getTranslationAndEulerAngles(
vx,
vy,vz, vroll,vpitch,vyaw);
3598 double dt1 = firstStamp - inputStamp;
3599 double dt2 = lastStamp - inputStamp;
3606 UERROR(
"Could not get transform between stamps %f and %f!",
3613 UERROR(
"Could not get transform between stamps %f and %f!",
3621 double scanTime = lastStamp - firstStamp;
3622 cv::Mat output(1, input.
size(), CV_32FC4);
3627 bool timeOnColumns = input.
data().cols > input.
data().rows;
3636 for(
int u=0; u<input.
data().
cols; ++u)
3638 const float * inputPtr = input.
data().ptr<
float>(0, u);
3639 stamp = inputStamp + inputPtr[offsetTime];
3642 for(
int v=0;
v<input.
data().rows; ++
v)
3644 inputPtr = input.
data().ptr<
float>(
v, u);
3645 pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]);
3646 if(pcl::isFinite(pt))
3648 if(!isLocalTransformIdentity)
3653 if(!isLocalTransformIdentity)
3657 float * dataPtr = output.ptr<
float>(0, oi++);
3661 dataPtr[3] = input.
data().ptr<
float>(
v, u)[offsetIntensity];
3673 for(
int v=0;
v<input.
data().rows; ++
v)
3675 const float * inputPtr = input.
data().ptr<
float>(
v, 0);
3676 stamp = inputStamp + inputPtr[offsetTime];
3679 for(
int u=0; u<input.
data().cols; ++u)
3681 inputPtr = input.
data().ptr<
float>(
v, u);
3682 pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]);
3683 if(pcl::isFinite(pt))
3685 if(!isLocalTransformIdentity)
3690 if(!isLocalTransformIdentity)
3694 float * dataPtr = output.ptr<
float>(0, oi++);
3698 dataPtr[3] = input.
data().ptr<
float>(
v, u)[offsetIntensity];
3704 UDEBUG(
"Lidar deskewing time=%fs", processingTime.
elapsed());