00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <rtabmap/core/util3d.h>
00029 #include <rtabmap/core/util3d_transforms.h>
00030 #include <rtabmap/core/util3d_filtering.h>
00031 #include <rtabmap/core/util3d_surface.h>
00032 #include <rtabmap/core/util2d.h>
00033 #include <rtabmap/core/util2d.h>
00034 #include <rtabmap/utilite/ULogger.h>
00035 #include <rtabmap/utilite/UMath.h>
00036 #include <rtabmap/utilite/UConversion.h>
00037 #include <rtabmap/utilite/UFile.h>
00038 #include <pcl/io/pcd_io.h>
00039 #include <pcl/io/ply_io.h>
00040 #include <pcl/common/transforms.h>
00041 #include <opencv2/imgproc/imgproc.hpp>
00042 #include <opencv2/imgproc/types_c.h>
00043
00044 namespace rtabmap
00045 {
00046
00047 namespace util3d
00048 {
00049
00050 cv::Mat bgrFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder)
00051 {
00052 cv::Mat frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
00053
00054 for(unsigned int h = 0; h < cloud.height; h++)
00055 {
00056 for(unsigned int w = 0; w < cloud.width; w++)
00057 {
00058 if(bgrOrder)
00059 {
00060 frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).b;
00061 frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
00062 frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).r;
00063 }
00064 else
00065 {
00066 frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).r;
00067 frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
00068 frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).b;
00069 }
00070 }
00071 }
00072 return frameBGR;
00073 }
00074
00075
00076 cv::Mat depthFromCloud(
00077 const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
00078 float & fx,
00079 float & fy,
00080 bool depth16U)
00081 {
00082 cv::Mat frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
00083 fx = 0.0f;
00084 fy = 0.0f;
00085 for(unsigned int h = 0; h < cloud.height; h++)
00086 {
00087 for(unsigned int w = 0; w < cloud.width; w++)
00088 {
00089 float depth = cloud.at(h*cloud.width + w).z;
00090 if(depth16U)
00091 {
00092 depth *= 1000.0f;
00093 unsigned short depthMM = 0;
00094 if(depth <= (float)USHRT_MAX)
00095 {
00096 depthMM = (unsigned short)depth;
00097 }
00098 frameDepth.at<unsigned short>(h,w) = depthMM;
00099 }
00100 else
00101 {
00102 frameDepth.at<float>(h,w) = depth;
00103 }
00104
00105
00106 if(fx == 0.0f &&
00107 uIsFinite(cloud.at(h*cloud.width + w).x) &&
00108 uIsFinite(depth) &&
00109 w != cloud.width/2 &&
00110 depth > 0)
00111 {
00112 fx = cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth);
00113 if(depth16U)
00114 {
00115 fx*=1000.0f;
00116 }
00117 }
00118 if(fy == 0.0f &&
00119 uIsFinite(cloud.at(h*cloud.width + w).y) &&
00120 uIsFinite(depth) &&
00121 h != cloud.height/2 &&
00122 depth > 0)
00123 {
00124 fy = cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth);
00125 if(depth16U)
00126 {
00127 fy*=1000.0f;
00128 }
00129 }
00130 }
00131 }
00132 return frameDepth;
00133 }
00134
00135
00136 void rgbdFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
00137 cv::Mat & frameBGR,
00138 cv::Mat & frameDepth,
00139 float & fx,
00140 float & fy,
00141 bool bgrOrder,
00142 bool depth16U)
00143 {
00144 frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
00145 frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
00146
00147 fx = 0.0f;
00148 fy = 0.0f;
00149 for(unsigned int h = 0; h < cloud.height; h++)
00150 {
00151 for(unsigned int w = 0; w < cloud.width; w++)
00152 {
00153
00154 if(bgrOrder)
00155 {
00156 frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).b;
00157 frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
00158 frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).r;
00159 }
00160 else
00161 {
00162 frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).r;
00163 frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
00164 frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).b;
00165 }
00166
00167
00168 float depth = cloud.at(h*cloud.width + w).z;
00169 if(depth16U)
00170 {
00171 depth *= 1000.0f;
00172 unsigned short depthMM = 0;
00173 if(depth <= (float)USHRT_MAX)
00174 {
00175 depthMM = (unsigned short)depth;
00176 }
00177 frameDepth.at<unsigned short>(h,w) = depthMM;
00178 }
00179 else
00180 {
00181 frameDepth.at<float>(h,w) = depth;
00182 }
00183
00184
00185 if(fx == 0.0f &&
00186 uIsFinite(cloud.at(h*cloud.width + w).x) &&
00187 uIsFinite(depth) &&
00188 w != cloud.width/2 &&
00189 depth > 0)
00190 {
00191 fx = 1.0f/(cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth));
00192 if(depth16U)
00193 {
00194 fx/=1000.0f;
00195 }
00196 }
00197 if(fy == 0.0f &&
00198 uIsFinite(cloud.at(h*cloud.width + w).y) &&
00199 uIsFinite(depth) &&
00200 h != cloud.height/2 &&
00201 depth > 0)
00202 {
00203 fy = 1.0f/(cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth));
00204 if(depth16U)
00205 {
00206 fy/=1000.0f;
00207 }
00208 }
00209 }
00210 }
00211 }
00212
00213 pcl::PointXYZ projectDepthTo3D(
00214 const cv::Mat & depthImage,
00215 float x, float y,
00216 float cx, float cy,
00217 float fx, float fy,
00218 bool smoothing,
00219 float depthErrorRatio)
00220 {
00221 UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
00222
00223 pcl::PointXYZ pt;
00224
00225 float depth = util2d::getDepth(depthImage, x, y, smoothing, depthErrorRatio);
00226 if(depth > 0.0f)
00227 {
00228
00229 cx = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f;
00230 cy = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f;
00231
00232
00233 pt.x = (x - cx) * depth / fx;
00234 pt.y = (y - cy) * depth / fy;
00235 pt.z = depth;
00236 }
00237 else
00238 {
00239 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
00240 }
00241 return pt;
00242 }
00243
00244 Eigen::Vector3f projectDepthTo3DRay(
00245 const cv::Size & imageSize,
00246 float x, float y,
00247 float cx, float cy,
00248 float fx, float fy)
00249 {
00250 Eigen::Vector3f ray;
00251
00252
00253 cx = cx > 0.0f ? cx : float(imageSize.width/2) - 0.5f;
00254 cy = cy > 0.0f ? cy : float(imageSize.height/2) - 0.5f;
00255
00256
00257 ray[0] = (x - cx) / fx;
00258 ray[1] = (y - cy) / fy;
00259 ray[2] = 1.0f;
00260
00261 return ray;
00262 }
00263
00264 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
00265 const cv::Mat & imageDepth,
00266 float cx, float cy,
00267 float fx, float fy,
00268 int decimation,
00269 float maxDepth,
00270 float minDepth,
00271 std::vector<int> * validIndices)
00272 {
00273 CameraModel model(fx, fy, cx, cy);
00274 return cloudFromDepth(imageDepth, model, decimation, maxDepth, minDepth, validIndices);
00275 }
00276
00277 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
00278 const cv::Mat & imageDepthIn,
00279 const CameraModel & model,
00280 int decimation,
00281 float maxDepth,
00282 float minDepth,
00283 std::vector<int> * validIndices)
00284 {
00285 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00286 if(decimation == 0)
00287 {
00288 decimation = 1;
00289 }
00290 float rgbToDepthFactorX = 1.0f;
00291 float rgbToDepthFactorY = 1.0f;
00292
00293 UASSERT(model.isValidForProjection());
00294 UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
00295
00296 cv::Mat imageDepth = imageDepthIn;
00297 if(model.imageHeight()>0 && model.imageWidth()>0)
00298 {
00299 UASSERT(model.imageHeight() % imageDepthIn.rows == 0 && model.imageWidth() % imageDepthIn.cols == 0);
00300
00301 if(decimation < 0)
00302 {
00303 UDEBUG("Decimation from model (%d)", decimation);
00304 if(model.imageHeight() % decimation != 0)
00305 {
00306 UERROR("Decimation is not valid for current image size (model.imageHeight()=%d decimation=%d). The cloud is not created.", model.imageHeight(), decimation);
00307 return cloud;
00308 }
00309 if(model.imageWidth() % decimation != 0)
00310 {
00311 UERROR("Decimation is not valid for current image size (model.imageWidth()=%d decimation=%d). The cloud is not created.", model.imageWidth(), decimation);
00312 return cloud;
00313 }
00314
00315
00316 decimation = -1*decimation;
00317
00318 int targetSize = model.imageHeight() / decimation;
00319 if(targetSize > imageDepthIn.rows)
00320 {
00321 UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
00322 imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows);
00323 decimation = 1;
00324 }
00325 else if(targetSize == imageDepthIn.rows)
00326 {
00327 decimation = 1;
00328 }
00329 else
00330 {
00331 UASSERT(imageDepthIn.rows % targetSize == 0);
00332 decimation = imageDepthIn.rows / targetSize;
00333 }
00334 }
00335 else
00336 {
00337 if(imageDepthIn.rows % decimation != 0)
00338 {
00339 UERROR("Decimation is not valid for current image size (imageDepth.rows=%d decimation=%d). The cloud is not created.", imageDepthIn.rows, decimation);
00340 return cloud;
00341 }
00342 if(imageDepthIn.cols % decimation != 0)
00343 {
00344 UERROR("Decimation is not valid for current image size (imageDepth.cols=%d decimation=%d). The cloud is not created.", imageDepthIn.cols, decimation);
00345 return cloud;
00346 }
00347 }
00348
00349 rgbToDepthFactorX = 1.0f/float((model.imageWidth() / imageDepth.cols));
00350 rgbToDepthFactorY = 1.0f/float((model.imageHeight() / imageDepth.rows));
00351 }
00352 else
00353 {
00354 decimation = abs(decimation);
00355 UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
00356 UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("cols=%d decimation=%d", imageDepth.cols, decimation).c_str());
00357 }
00358
00359
00360 cloud->height = imageDepth.rows/decimation;
00361 cloud->width = imageDepth.cols/decimation;
00362 cloud->is_dense = false;
00363 cloud->resize(cloud->height * cloud->width);
00364 if(validIndices)
00365 {
00366 validIndices->resize(cloud->size());
00367 }
00368
00369 float depthFx = model.fx() * rgbToDepthFactorX;
00370 float depthFy = model.fy() * rgbToDepthFactorY;
00371 float depthCx = model.cx() * rgbToDepthFactorX;
00372 float depthCy = model.cy() * rgbToDepthFactorY;
00373
00374 UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
00375 imageDepth.cols, imageDepth.rows,
00376 model.fx(), model.fy(), model.cx(), model.cy(),
00377 rgbToDepthFactorX,
00378 rgbToDepthFactorY,
00379 decimation);
00380
00381 int oi = 0;
00382 for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
00383 {
00384 for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
00385 {
00386 pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
00387
00388 pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
00389 if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
00390 {
00391 pt.x = ptXYZ.x;
00392 pt.y = ptXYZ.y;
00393 pt.z = ptXYZ.z;
00394 if(validIndices)
00395 {
00396 validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
00397 }
00398 }
00399 else
00400 {
00401 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
00402 }
00403 }
00404 }
00405
00406 if(validIndices)
00407 {
00408 validIndices->resize(oi);
00409 }
00410
00411 return cloud;
00412 }
00413
00414 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
00415 const cv::Mat & imageRgb,
00416 const cv::Mat & imageDepth,
00417 float cx, float cy,
00418 float fx, float fy,
00419 int decimation,
00420 float maxDepth,
00421 float minDepth,
00422 std::vector<int> * validIndices)
00423 {
00424 CameraModel model(fx, fy, cx, cy);
00425 return cloudFromDepthRGB(imageRgb, imageDepth, model, decimation, maxDepth, minDepth, validIndices);
00426 }
00427
00428 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
00429 const cv::Mat & imageRgb,
00430 const cv::Mat & imageDepthIn,
00431 const CameraModel & model,
00432 int decimation,
00433 float maxDepth,
00434 float minDepth,
00435 std::vector<int> * validIndices)
00436 {
00437 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00438 if(decimation == 0)
00439 {
00440 decimation = 1;
00441 }
00442 UDEBUG("");
00443 UASSERT(model.isValidForProjection());
00444 UASSERT_MSG((model.imageHeight() == 0 && model.imageWidth() == 0) ||
00445 (model.imageHeight() == imageRgb.rows && model.imageWidth() == imageRgb.cols),
00446 uFormat("model=%dx%d rgb=%dx%d", model.imageWidth(), model.imageHeight(), imageRgb.cols, imageRgb.rows).c_str());
00447 UASSERT_MSG(imageRgb.rows % imageDepthIn.rows == 0 && imageRgb.cols % imageDepthIn.cols == 0,
00448 uFormat("rgb=%dx%d depth=%dx%d", imageRgb.cols, imageRgb.rows, imageDepthIn.cols, imageDepthIn.rows).c_str());
00449 UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
00450 if(decimation < 0)
00451 {
00452 if(imageRgb.rows % decimation != 0 || imageRgb.cols % decimation != 0)
00453 {
00454 int oldDecimation = decimation;
00455 while(decimation <= -1)
00456 {
00457 if(imageRgb.rows % decimation == 0 && imageRgb.cols % decimation == 0)
00458 {
00459 break;
00460 }
00461 ++decimation;
00462 }
00463
00464 if(imageRgb.rows % oldDecimation != 0 || imageRgb.cols % oldDecimation != 0)
00465 {
00466 UWARN("Decimation (%d) is not valid for current image size (rgb=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageRgb.cols, imageRgb.rows, decimation);
00467 }
00468 }
00469 }
00470 else
00471 {
00472 if(imageDepthIn.rows % decimation != 0 || imageDepthIn.cols % decimation != 0)
00473 {
00474 int oldDecimation = decimation;
00475 while(decimation >= 1)
00476 {
00477 if(imageDepthIn.rows % decimation == 0 && imageDepthIn.cols % decimation == 0)
00478 {
00479 break;
00480 }
00481 --decimation;
00482 }
00483
00484 if(imageDepthIn.rows % oldDecimation != 0 || imageDepthIn.cols % oldDecimation != 0)
00485 {
00486 UWARN("Decimation (%d) is not valid for current image size (depth=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageDepthIn.cols, imageDepthIn.rows, decimation);
00487 }
00488 }
00489 }
00490
00491 cv::Mat imageDepth = imageDepthIn;
00492 if(decimation < 0)
00493 {
00494 UDEBUG("Decimation from RGB image (%d)", decimation);
00495
00496 decimation = -1*decimation;
00497
00498 int targetSize = imageRgb.rows / decimation;
00499 if(targetSize > imageDepthIn.rows)
00500 {
00501 UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
00502 imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows);
00503 decimation = 1;
00504 }
00505 else if(targetSize == imageDepthIn.rows)
00506 {
00507 decimation = 1;
00508 }
00509 else
00510 {
00511 UASSERT(imageDepthIn.rows % targetSize == 0);
00512 decimation = imageDepthIn.rows / targetSize;
00513 }
00514 }
00515
00516 bool mono;
00517 if(imageRgb.channels() == 3)
00518 {
00519 mono = false;
00520 }
00521 else if(imageRgb.channels() == 1)
00522 {
00523 mono = true;
00524 }
00525 else
00526 {
00527 return cloud;
00528 }
00529
00530
00531 cloud->height = imageDepth.rows/decimation;
00532 cloud->width = imageDepth.cols/decimation;
00533 cloud->is_dense = false;
00534 cloud->resize(cloud->height * cloud->width);
00535 if(validIndices)
00536 {
00537 validIndices->resize(cloud->size());
00538 }
00539
00540 float rgbToDepthFactorX = float(imageRgb.cols) / float(imageDepth.cols);
00541 float rgbToDepthFactorY = float(imageRgb.rows) / float(imageDepth.rows);
00542 float depthFx = model.fx() / rgbToDepthFactorX;
00543 float depthFy = model.fy() / rgbToDepthFactorY;
00544 float depthCx = model.cx() / rgbToDepthFactorX;
00545 float depthCy = model.cy() / rgbToDepthFactorY;
00546
00547 UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
00548 imageRgb.cols, imageRgb.rows,
00549 imageDepth.cols, imageDepth.rows,
00550 model.fx(), model.fy(), model.cx(), model.cy(),
00551 rgbToDepthFactorX,
00552 rgbToDepthFactorY,
00553 decimation);
00554
00555 int oi = 0;
00556 for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
00557 {
00558 for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
00559 {
00560 pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
00561
00562 int x = int(w*rgbToDepthFactorX);
00563 int y = int(h*rgbToDepthFactorY);
00564 UASSERT(x >=0 && x<imageRgb.cols && y >=0 && y<imageRgb.rows);
00565 if(!mono)
00566 {
00567 const unsigned char * bgr = imageRgb.ptr<unsigned char>(y,x);
00568 pt.b = bgr[0];
00569 pt.g = bgr[1];
00570 pt.r = bgr[2];
00571 }
00572 else
00573 {
00574 unsigned char v = imageRgb.at<unsigned char>(y,x);
00575 pt.b = v;
00576 pt.g = v;
00577 pt.r = v;
00578 }
00579
00580 pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
00581 if (pcl::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
00582 {
00583 pt.x = ptXYZ.x;
00584 pt.y = ptXYZ.y;
00585 pt.z = ptXYZ.z;
00586 if (validIndices)
00587 {
00588 validIndices->at(oi) = (h / decimation)*cloud->width + (w / decimation);
00589 }
00590 ++oi;
00591 }
00592 else
00593 {
00594 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
00595 }
00596 }
00597 }
00598 if(validIndices)
00599 {
00600 validIndices->resize(oi);
00601 }
00602 if(oi == 0)
00603 {
00604 UWARN("Cloud with only NaN values created!");
00605 }
00606 UDEBUG("");
00607 return cloud;
00608 }
00609
00610 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
00611 const cv::Mat & imageDisparity,
00612 const StereoCameraModel & model,
00613 int decimation,
00614 float maxDepth,
00615 float minDepth,
00616 std::vector<int> * validIndices)
00617 {
00618 UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
00619 UASSERT(decimation >= 1);
00620
00621 if(imageDisparity.rows % decimation != 0 || imageDisparity.cols % decimation != 0)
00622 {
00623 int oldDecimation = decimation;
00624 while(decimation >= 1)
00625 {
00626 if(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0)
00627 {
00628 break;
00629 }
00630 --decimation;
00631 }
00632
00633 if(imageDisparity.rows % oldDecimation != 0 || imageDisparity.cols % oldDecimation != 0)
00634 {
00635 UWARN("Decimation (%d) is not valid for current image size (depth=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageDisparity.cols, imageDisparity.rows, decimation);
00636 }
00637 }
00638
00639 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00640
00641
00642 cloud->height = imageDisparity.rows/decimation;
00643 cloud->width = imageDisparity.cols/decimation;
00644 cloud->is_dense = false;
00645 cloud->resize(cloud->height * cloud->width);
00646 if(validIndices)
00647 {
00648 validIndices->resize(cloud->size());
00649 }
00650
00651 int oi = 0;
00652 if(imageDisparity.type()==CV_16SC1)
00653 {
00654 for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
00655 {
00656 for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
00657 {
00658 float disp = float(imageDisparity.at<short>(h,w))/16.0f;
00659 cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
00660 if(pt.z >= minDepth && (maxDepth <= 0.0f || pt.z <= maxDepth))
00661 {
00662 cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
00663 if(validIndices)
00664 {
00665 validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
00666 }
00667 }
00668 else
00669 {
00670 cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(
00671 std::numeric_limits<float>::quiet_NaN(),
00672 std::numeric_limits<float>::quiet_NaN(),
00673 std::numeric_limits<float>::quiet_NaN());
00674 }
00675 }
00676 }
00677 }
00678 else
00679 {
00680 for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
00681 {
00682 for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
00683 {
00684 float disp = imageDisparity.at<float>(h,w);
00685 cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
00686 if(pt.z > minDepth && (maxDepth <= 0.0f || pt.z <= maxDepth))
00687 {
00688 cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
00689 if(validIndices)
00690 {
00691 validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
00692 }
00693 }
00694 else
00695 {
00696 cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(
00697 std::numeric_limits<float>::quiet_NaN(),
00698 std::numeric_limits<float>::quiet_NaN(),
00699 std::numeric_limits<float>::quiet_NaN());
00700 }
00701 }
00702 }
00703 }
00704 if(validIndices)
00705 {
00706 validIndices->resize(oi);
00707 }
00708 return cloud;
00709 }
00710
00711 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
00712 const cv::Mat & imageRgb,
00713 const cv::Mat & imageDisparity,
00714 const StereoCameraModel & model,
00715 int decimation,
00716 float maxDepth,
00717 float minDepth,
00718 std::vector<int> * validIndices)
00719 {
00720 UASSERT(!imageRgb.empty() && !imageDisparity.empty());
00721 UASSERT(imageRgb.rows == imageDisparity.rows &&
00722 imageRgb.cols == imageDisparity.cols &&
00723 (imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1));
00724 UASSERT(imageRgb.channels() == 3 || imageRgb.channels() == 1);
00725 UASSERT(decimation >= 1);
00726
00727 if(imageDisparity.rows % decimation != 0 || imageDisparity.cols % decimation != 0)
00728 {
00729 int oldDecimation = decimation;
00730 while(decimation >= 1)
00731 {
00732 if(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0)
00733 {
00734 break;
00735 }
00736 --decimation;
00737 }
00738
00739 if(imageDisparity.rows % oldDecimation != 0 || imageDisparity.cols % oldDecimation != 0)
00740 {
00741 UWARN("Decimation (%d) is not valid for current image size (depth=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageDisparity.cols, imageDisparity.rows, decimation);
00742 }
00743 }
00744
00745 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00746
00747 bool mono;
00748 if(imageRgb.channels() == 3)
00749 {
00750 mono = false;
00751 }
00752 else
00753 {
00754 mono = true;
00755 }
00756
00757
00758 cloud->height = imageRgb.rows/decimation;
00759 cloud->width = imageRgb.cols/decimation;
00760 cloud->is_dense = false;
00761 cloud->resize(cloud->height * cloud->width);
00762 if(validIndices)
00763 {
00764 validIndices->resize(cloud->size());
00765 }
00766
00767 int oi=0;
00768 for(int h = 0; h < imageRgb.rows && h/decimation < (int)cloud->height; h+=decimation)
00769 {
00770 for(int w = 0; w < imageRgb.cols && w/decimation < (int)cloud->width; w+=decimation)
00771 {
00772 pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
00773 if(!mono)
00774 {
00775 pt.b = imageRgb.at<cv::Vec3b>(h,w)[0];
00776 pt.g = imageRgb.at<cv::Vec3b>(h,w)[1];
00777 pt.r = imageRgb.at<cv::Vec3b>(h,w)[2];
00778 }
00779 else
00780 {
00781 unsigned char v = imageRgb.at<unsigned char>(h,w);
00782 pt.b = v;
00783 pt.g = v;
00784 pt.r = v;
00785 }
00786
00787 float disp = imageDisparity.type()==CV_16SC1?float(imageDisparity.at<short>(h,w))/16.0f:imageDisparity.at<float>(h,w);
00788 cv::Point3f ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
00789 if(util3d::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
00790 {
00791 pt.x = ptXYZ.x;
00792 pt.y = ptXYZ.y;
00793 pt.z = ptXYZ.z;
00794 if(validIndices)
00795 {
00796 validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
00797 }
00798 }
00799 else
00800 {
00801 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
00802 }
00803 }
00804 }
00805 if(validIndices)
00806 {
00807 validIndices->resize(oi);
00808 }
00809 return cloud;
00810 }
00811
00812 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
00813 const cv::Mat & imageLeft,
00814 const cv::Mat & imageRight,
00815 const StereoCameraModel & model,
00816 int decimation,
00817 float maxDepth,
00818 float minDepth,
00819 std::vector<int> * validIndices,
00820 const ParametersMap & parameters)
00821 {
00822 UASSERT(!imageLeft.empty() && !imageRight.empty());
00823 UASSERT(imageRight.type() == CV_8UC1);
00824 UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1);
00825 UASSERT(imageLeft.rows == imageRight.rows &&
00826 imageLeft.cols == imageRight.cols);
00827 UASSERT(decimation >= 1.0f);
00828
00829 cv::Mat leftColor = imageLeft;
00830 cv::Mat rightMono = imageRight;
00831
00832 cv::Mat leftMono;
00833 if(leftColor.channels() == 3)
00834 {
00835 cv::cvtColor(leftColor, leftMono, CV_BGR2GRAY);
00836 }
00837 else
00838 {
00839 leftMono = leftColor;
00840 }
00841
00842 return cloudFromDisparityRGB(
00843 leftColor,
00844 util2d::disparityFromStereoImages(leftMono, rightMono, parameters),
00845 model,
00846 decimation,
00847 maxDepth,
00848 minDepth,
00849 validIndices);
00850 }
00851
00852 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
00853 const SensorData & sensorData,
00854 int decimation,
00855 float maxDepth,
00856 float minDepth,
00857 std::vector<int> * validIndices,
00858 const ParametersMap & stereoParameters,
00859 const std::vector<float> & roiRatios)
00860 {
00861 if(decimation == 0)
00862 {
00863 decimation = 1;
00864 }
00865
00866 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00867
00868 if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
00869 {
00870
00871 UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
00872 int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
00873 for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
00874 {
00875 if(sensorData.cameraModels()[i].isValidForProjection())
00876 {
00877 cv::Mat depth = cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows));
00878 CameraModel model = sensorData.cameraModels()[i];
00879 if( roiRatios.size() == 4 &&
00880 (roiRatios[0] > 0.0f ||
00881 roiRatios[1] > 0.0f ||
00882 roiRatios[2] > 0.0f ||
00883 roiRatios[3] > 0.0f))
00884 {
00885 cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios);
00886 cv::Rect roiRgb;
00887 if(model.imageWidth() && model.imageHeight())
00888 {
00889 roiRgb = util2d::computeRoi(model.imageSize(), roiRatios);
00890 }
00891 if( roiDepth.width%decimation==0 &&
00892 roiDepth.height%decimation==0 &&
00893 (roiRgb.width != 0 ||
00894 (roiRgb.width%decimation==0 &&
00895 roiRgb.height%decimation==0)))
00896 {
00897 depth = cv::Mat(depth, roiDepth);
00898 if(model.imageWidth() != 0 && model.imageHeight() != 0)
00899 {
00900 model = model.roi(util2d::computeRoi(model.imageSize(), roiRatios));
00901 }
00902 else
00903 {
00904 model = model.roi(roiDepth);
00905 }
00906 }
00907 else
00908 {
00909 UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
00910 "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
00911 "by decimation parameter (%d). Ignoring ROI ratios...",
00912 roiRatios[0],
00913 roiRatios[1],
00914 roiRatios[2],
00915 roiRatios[3],
00916 roiDepth.width,
00917 roiDepth.height,
00918 roiRgb.width,
00919 roiRgb.height,
00920 decimation);
00921 }
00922 }
00923
00924 pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
00925 depth,
00926 model,
00927 decimation,
00928 maxDepth,
00929 minDepth,
00930 sensorData.cameraModels().size()==1?validIndices:0);
00931
00932 if(tmp->size())
00933 {
00934 if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
00935 {
00936 tmp = util3d::transformPointCloud(tmp, model.localTransform());
00937 }
00938
00939 if(sensorData.cameraModels().size() > 1)
00940 {
00941 tmp = util3d::removeNaNFromPointCloud(tmp);
00942 *cloud += *tmp;
00943 }
00944 else
00945 {
00946 cloud = tmp;
00947 }
00948 }
00949 }
00950 else
00951 {
00952 UERROR("Camera model %d is invalid", i);
00953 }
00954 }
00955
00956 if(cloud->is_dense && validIndices)
00957 {
00958
00959 validIndices->resize(cloud->size());
00960 for(unsigned int i=0; i<cloud->size(); ++i)
00961 {
00962 validIndices->at(i) = i;
00963 }
00964 }
00965 }
00966 else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection())
00967 {
00968
00969 UASSERT(sensorData.rightRaw().type() == CV_8UC1);
00970
00971 cv::Mat leftMono;
00972 if(sensorData.imageRaw().channels() == 3)
00973 {
00974 cv::cvtColor(sensorData.imageRaw(), leftMono, CV_BGR2GRAY);
00975 }
00976 else
00977 {
00978 leftMono = sensorData.imageRaw();
00979 }
00980
00981 cv::Mat right(sensorData.rightRaw());
00982 StereoCameraModel model = sensorData.stereoCameraModel();
00983 if( roiRatios.size() == 4 &&
00984 ((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
00985 (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
00986 (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
00987 (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
00988 {
00989 cv::Rect roi = util2d::computeRoi(leftMono, roiRatios);
00990 if( roi.width%decimation==0 &&
00991 roi.height%decimation==0)
00992 {
00993 leftMono = cv::Mat(leftMono, roi);
00994 right = cv::Mat(right, roi);
00995 model.roi(roi);
00996 }
00997 else
00998 {
00999 UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
01000 "dimension (left=%dx%d) cannot be divided exactly "
01001 "by decimation parameter (%d). Ignoring ROI ratios...",
01002 roiRatios[0],
01003 roiRatios[1],
01004 roiRatios[2],
01005 roiRatios[3],
01006 roi.width,
01007 roi.height,
01008 decimation);
01009 }
01010 }
01011
01012 cloud = cloudFromDisparity(
01013 util2d::disparityFromStereoImages(leftMono, right, stereoParameters),
01014 model,
01015 decimation,
01016 maxDepth,
01017 minDepth,
01018 validIndices);
01019
01020 if(cloud->size())
01021 {
01022 if(cloud->size() && !sensorData.stereoCameraModel().left().localTransform().isNull() && !sensorData.stereoCameraModel().left().localTransform().isIdentity())
01023 {
01024 cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
01025 }
01026 }
01027 }
01028 return cloud;
01029 }
01030
01031 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
01032 const SensorData & sensorData,
01033 int decimation,
01034 float maxDepth,
01035 float minDepth,
01036 std::vector<int> * validIndices,
01037 const ParametersMap & stereoParameters,
01038 const std::vector<float> & roiRatios)
01039 {
01040 if(decimation == 0)
01041 {
01042 decimation = 1;
01043 }
01044
01045 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
01046
01047 if(!sensorData.imageRaw().empty() && !sensorData.depthRaw().empty() && sensorData.cameraModels().size())
01048 {
01049
01050 UDEBUG("");
01051 UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
01052 UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
01053 UASSERT_MSG(sensorData.imageRaw().cols % sensorData.depthRaw().cols == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().cols, sensorData.depthRaw().cols).c_str());
01054 UASSERT_MSG(sensorData.imageRaw().rows % sensorData.depthRaw().rows == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().rows, sensorData.depthRaw().rows).c_str());
01055 int subRGBWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
01056 int subDepthWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
01057
01058 for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
01059 {
01060 if(sensorData.cameraModels()[i].isValidForProjection())
01061 {
01062 cv::Mat rgb(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows));
01063 cv::Mat depth(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows));
01064 CameraModel model = sensorData.cameraModels()[i];
01065 if( roiRatios.size() == 4 &&
01066 ((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
01067 (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
01068 (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
01069 (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
01070 {
01071 cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios);
01072 cv::Rect roiRgb = util2d::computeRoi(rgb, roiRatios);
01073 if( roiDepth.width%decimation==0 &&
01074 roiDepth.height%decimation==0 &&
01075 roiRgb.width%decimation==0 &&
01076 roiRgb.height%decimation==0)
01077 {
01078 depth = cv::Mat(depth, roiDepth);
01079 rgb = cv::Mat(rgb, roiRgb);
01080 model = model.roi(roiRgb);
01081 }
01082 else
01083 {
01084 UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
01085 "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
01086 "by decimation parameter (%d). Ignoring ROI ratios...",
01087 roiRatios[0],
01088 roiRatios[1],
01089 roiRatios[2],
01090 roiRatios[3],
01091 roiDepth.width,
01092 roiDepth.height,
01093 roiRgb.width,
01094 roiRgb.height,
01095 decimation);
01096 }
01097 }
01098
01099 pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
01100 rgb,
01101 depth,
01102 model,
01103 decimation,
01104 maxDepth,
01105 minDepth,
01106 sensorData.cameraModels().size() == 1?validIndices:0);
01107
01108 if(tmp->size())
01109 {
01110 if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
01111 {
01112 tmp = util3d::transformPointCloud(tmp, model.localTransform());
01113 }
01114
01115 if(sensorData.cameraModels().size() > 1)
01116 {
01117 tmp = util3d::removeNaNFromPointCloud(tmp);
01118 *cloud += *tmp;
01119 }
01120 else
01121 {
01122 cloud = tmp;
01123 }
01124 }
01125 }
01126 else
01127 {
01128 UERROR("Camera model %d is invalid", i);
01129 }
01130 }
01131
01132 if(cloud->is_dense && validIndices)
01133 {
01134
01135 validIndices->resize(cloud->size());
01136 for(unsigned int i=0; i<cloud->size(); ++i)
01137 {
01138 validIndices->at(i) = i;
01139 }
01140 }
01141 }
01142 else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection())
01143 {
01144
01145 UDEBUG("");
01146
01147 cv::Mat left(sensorData.imageRaw());
01148 cv::Mat right(sensorData.rightRaw());
01149 StereoCameraModel model = sensorData.stereoCameraModel();
01150 if( roiRatios.size() == 4 &&
01151 ((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
01152 (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
01153 (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
01154 (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
01155 {
01156 cv::Rect roi = util2d::computeRoi(left, roiRatios);
01157 if( roi.width%decimation==0 &&
01158 roi.height%decimation==0)
01159 {
01160 left = cv::Mat(left, roi);
01161 right = cv::Mat(right, roi);
01162 model.roi(roi);
01163 }
01164 else
01165 {
01166 UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
01167 "dimension (left=%dx%d) cannot be divided exactly "
01168 "by decimation parameter (%d). Ignoring ROI ratios...",
01169 roiRatios[0],
01170 roiRatios[1],
01171 roiRatios[2],
01172 roiRatios[3],
01173 roi.width,
01174 roi.height,
01175 decimation);
01176 }
01177 }
01178
01179 cloud = cloudFromStereoImages(
01180 left,
01181 right,
01182 model,
01183 decimation,
01184 maxDepth,
01185 minDepth,
01186 validIndices,
01187 stereoParameters);
01188
01189 if(cloud->size() && !sensorData.stereoCameraModel().left().localTransform().isNull() && !sensorData.stereoCameraModel().left().localTransform().isIdentity())
01190 {
01191 cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
01192 }
01193 }
01194 return cloud;
01195 }
01196
01197 pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
01198 const cv::Mat & depthImage,
01199 float fx,
01200 float fy,
01201 float cx,
01202 float cy,
01203 float maxDepth,
01204 float minDepth,
01205 const Transform & localTransform)
01206 {
01207 UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
01208 UASSERT(!localTransform.isNull());
01209
01210 pcl::PointCloud<pcl::PointXYZ> scan;
01211 int middle = depthImage.rows/2;
01212 if(middle)
01213 {
01214 scan.resize(depthImage.cols);
01215 int oi = 0;
01216 for(int i=depthImage.cols-1; i>=0; --i)
01217 {
01218 pcl::PointXYZ pt = util3d::projectDepthTo3D(depthImage, i, middle, cx, cy, fx, fy, false);
01219 if(pcl::isFinite(pt) && pt.z >= minDepth && (maxDepth == 0 || pt.z < maxDepth))
01220 {
01221 if(!localTransform.isIdentity())
01222 {
01223 pt = util3d::transformPoint(pt, localTransform);
01224 }
01225 scan[oi++] = pt;
01226 }
01227 }
01228 scan.resize(oi);
01229 }
01230 return scan;
01231 }
01232
01233 pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImages(
01234 const cv::Mat & depthImages,
01235 const std::vector<CameraModel> & cameraModels,
01236 float maxDepth,
01237 float minDepth)
01238 {
01239 pcl::PointCloud<pcl::PointXYZ> scan;
01240 UASSERT(int((depthImages.cols/cameraModels.size())*cameraModels.size()) == depthImages.cols);
01241 int subImageWidth = depthImages.cols/cameraModels.size();
01242 for(int i=(int)cameraModels.size()-1; i>=0; --i)
01243 {
01244 if(cameraModels[i].isValidForProjection())
01245 {
01246 cv::Mat depth = cv::Mat(depthImages, cv::Rect(subImageWidth*i, 0, subImageWidth, depthImages.rows));
01247
01248 scan += laserScanFromDepthImage(
01249 depth,
01250 cameraModels[i].fx(),
01251 cameraModels[i].fy(),
01252 cameraModels[i].cx(),
01253 cameraModels[i].cy(),
01254 maxDepth,
01255 minDepth,
01256 cameraModels[i].localTransform());
01257 }
01258 else
01259 {
01260 UERROR("Camera model %d is invalid", i);
01261 }
01262 }
01263 return scan;
01264 }
01265
01266 LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filterNaNs)
01267 {
01268 if(cloud.data.empty())
01269 {
01270 return LaserScan();
01271 }
01272
01273 int fieldStates[8] = {0};
01274 pcl::uint32_t fieldOffsets[8] = {0};
01275 for(unsigned int i=0; i<cloud.fields.size(); ++i)
01276 {
01277 if(cloud.fields[i].name.compare("x") == 0)
01278 {
01279 fieldStates[0] = 1;
01280 fieldOffsets[0] = cloud.fields[i].offset;
01281 }
01282 else if(cloud.fields[i].name.compare("y") == 0)
01283 {
01284 fieldStates[1] = 1;
01285 fieldOffsets[1] = cloud.fields[i].offset;
01286 }
01287 else if(cloud.fields[i].name.compare("z") == 0)
01288 {
01289 fieldStates[2] = 1;
01290 fieldOffsets[2] = cloud.fields[i].offset;
01291 }
01292 else if(cloud.fields[i].name.compare("normal_x") == 0)
01293 {
01294 fieldStates[3] = 1;
01295 fieldOffsets[3] = cloud.fields[i].offset;
01296 }
01297 else if(cloud.fields[i].name.compare("normal_y") == 0)
01298 {
01299 fieldStates[4] = 1;
01300 fieldOffsets[4] = cloud.fields[i].offset;
01301 }
01302 else if(cloud.fields[i].name.compare("normal_z") == 0)
01303 {
01304 fieldStates[5] = 1;
01305 fieldOffsets[5] = cloud.fields[i].offset;
01306 }
01307 else if(cloud.fields[i].name.compare("rgb") == 0 || cloud.fields[i].name.compare("rgba") == 0)
01308 {
01309 fieldStates[6] = 1;
01310 fieldOffsets[6] = cloud.fields[i].offset;
01311 }
01312 else if(cloud.fields[i].name.compare("intensity") == 0)
01313 {
01314 fieldStates[7] = 1;
01315 fieldOffsets[7] = cloud.fields[i].offset;
01316 }
01317 else
01318 {
01319 UDEBUG("Ignoring \"%s\" field", cloud.fields[i].name.c_str());
01320 }
01321 }
01322 if(fieldStates[0]==0 || fieldStates[1]==0)
01323 {
01324
01325 UERROR("Cloud has not corresponding fields to laser scan!");
01326 return LaserScan();
01327 }
01328
01329 bool hasNormals = fieldStates[3] || fieldStates[4] || fieldStates[5];
01330 bool hasIntensity = fieldStates[7];
01331 bool hasRGB = !hasIntensity&&fieldStates[6];
01332 bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2];
01333
01334 LaserScan::Format format;
01335 if(is3D)
01336 {
01337 if(hasNormals && hasIntensity)
01338 {
01339 format = LaserScan::kXYZINormal;
01340 }
01341 else if(hasNormals && hasRGB)
01342 {
01343 format = LaserScan::kXYZRGBNormal;
01344 }
01345 else if(!hasNormals && hasIntensity)
01346 {
01347 format = LaserScan::kXYZI;
01348 }
01349 else if(!hasNormals && hasRGB)
01350 {
01351 format = LaserScan::kXYZRGB;
01352 }
01353 else
01354 {
01355 format = LaserScan::kXYZ;
01356 }
01357 }
01358 else
01359 {
01360 if(hasNormals && hasIntensity)
01361 {
01362 format = LaserScan::kXYINormal;
01363 }
01364 else if(!hasNormals && hasIntensity)
01365 {
01366 format = LaserScan::kXYI;
01367 }
01368 else
01369 {
01370 format = LaserScan::kXY;
01371 }
01372 }
01373
01374 UASSERT(cloud.data.size()/cloud.point_step == cloud.height*cloud.width);
01375 cv::Mat laserScan = cv::Mat(1, (int)cloud.data.size()/cloud.point_step, CV_32FC(LaserScan::channels(format)));
01376
01377 int oi=0;
01378 for (uint32_t row = 0; row < cloud.height; ++row)
01379 {
01380 const uint8_t* row_data = &cloud.data[row * cloud.row_step];
01381 for (uint32_t col = 0; col < cloud.width; ++col)
01382 {
01383 const uint8_t* msg_data = row_data + col * cloud.point_step;
01384
01385 float * ptr = laserScan.ptr<float>(0, oi);
01386
01387 bool valid = true;
01388 if(laserScan.channels() == 2)
01389 {
01390 ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
01391 ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
01392 valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]);
01393 }
01394 else if(laserScan.channels() == 3)
01395 {
01396 ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
01397 ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
01398 if(format == LaserScan::kXYI)
01399 {
01400 ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
01401 }
01402 else
01403 {
01404 ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
01405 valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]);
01406 }
01407 }
01408 else if(laserScan.channels() == 4)
01409 {
01410 ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
01411 ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
01412 ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
01413 if(format == LaserScan::kXYZI)
01414 {
01415 ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
01416 }
01417 else
01418 {
01419 pcl::uint8_t b=*(msg_data + fieldOffsets[6]);
01420 pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1);
01421 pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2);
01422 int * ptrInt = (int*)ptr;
01423 ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16);
01424 }
01425 valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]);
01426 }
01427 else if(laserScan.channels() == 5)
01428 {
01429 ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
01430 ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
01431 ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
01432 ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
01433 ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
01434 valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]);
01435 }
01436 else if(laserScan.channels() == 6)
01437 {
01438 ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
01439 ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
01440 if(format == LaserScan::kXYINormal)
01441 {
01442 ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
01443 }
01444 else
01445 {
01446 ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
01447 }
01448 ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
01449 ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
01450 ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
01451 valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]);
01452 }
01453 else if(laserScan.channels() == 7)
01454 {
01455 ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
01456 ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
01457 ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
01458 if(format == LaserScan::kXYZINormal)
01459 {
01460 ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
01461 }
01462 else
01463 {
01464 pcl::uint8_t b=*(msg_data + fieldOffsets[6]);
01465 pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1);
01466 pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2);
01467 int * ptrInt = (int*)ptr;
01468 ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16);
01469 }
01470 ptr[4] = *(float*)(msg_data + fieldOffsets[3]);
01471 ptr[5] = *(float*)(msg_data + fieldOffsets[4]);
01472 ptr[6] = *(float*)(msg_data + fieldOffsets[5]);
01473 valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]) && uIsFinite(ptr[6]);
01474 }
01475 else
01476 {
01477 UFATAL("Cannot handle as many channels (%d)!", laserScan.channels());
01478 }
01479
01480 if(!filterNaNs || valid)
01481 {
01482 ++oi;
01483 }
01484 }
01485 }
01486 if(oi == 0)
01487 {
01488 return LaserScan();
01489 }
01490 return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0, format);
01491 }
01492
01493 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
01494 {
01495 return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
01496 }
01497 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
01498 {
01499 cv::Mat laserScan;
01500 bool nullTransform = transform.isNull() || transform.isIdentity();
01501 Eigen::Affine3f transform3f = transform.toEigen3f();
01502 int oi = 0;
01503 if(indices.get())
01504 {
01505 laserScan = cv::Mat(1, (int)indices->size(), CV_32FC3);
01506 for(unsigned int i=0; i<indices->size(); ++i)
01507 {
01508 int index = indices->at(i);
01509 if(!filterNaNs || pcl::isFinite(cloud.at(index)))
01510 {
01511 float * ptr = laserScan.ptr<float>(0, oi++);
01512 if(!nullTransform)
01513 {
01514 pcl::PointXYZ pt = pcl::transformPoint(cloud.at(index), transform3f);
01515 ptr[0] = pt.x;
01516 ptr[1] = pt.y;
01517 ptr[2] = pt.z;
01518 }
01519 else
01520 {
01521 ptr[0] = cloud.at(index).x;
01522 ptr[1] = cloud.at(index).y;
01523 ptr[2] = cloud.at(index).z;
01524 }
01525 }
01526 }
01527 }
01528 else
01529 {
01530 laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC3);
01531 for(unsigned int i=0; i<cloud.size(); ++i)
01532 {
01533 if(!filterNaNs || pcl::isFinite(cloud.at(i)))
01534 {
01535 float * ptr = laserScan.ptr<float>(0, oi++);
01536 if(!nullTransform)
01537 {
01538 pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
01539 ptr[0] = pt.x;
01540 ptr[1] = pt.y;
01541 ptr[2] = pt.z;
01542 }
01543 else
01544 {
01545 ptr[0] = cloud.at(i).x;
01546 ptr[1] = cloud.at(i).y;
01547 ptr[2] = cloud.at(i).z;
01548 }
01549 }
01550 }
01551 }
01552 if(oi == 0)
01553 {
01554 return cv::Mat();
01555 }
01556 return laserScan(cv::Range::all(), cv::Range(0,oi));
01557 }
01558
01559 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
01560 {
01561 return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
01562 }
01563 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
01564 {
01565 cv::Mat laserScan;
01566 bool nullTransform = transform.isNull() || transform.isIdentity();
01567 int oi=0;
01568 if(indices.get())
01569 {
01570 laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(6));
01571 for(unsigned int i=0; i<indices->size(); ++i)
01572 {
01573 int index = indices->at(i);
01574 if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
01575 uIsFinite(cloud.at(index).normal_x) &&
01576 uIsFinite(cloud.at(index).normal_y) &&
01577 uIsFinite(cloud.at(index).normal_z)))
01578 {
01579 float * ptr = laserScan.ptr<float>(0, oi++);
01580 if(!nullTransform)
01581 {
01582 pcl::PointNormal pt = util3d::transformPoint(cloud.at(index), transform);
01583 ptr[0] = pt.x;
01584 ptr[1] = pt.y;
01585 ptr[2] = pt.z;
01586 ptr[3] = pt.normal_x;
01587 ptr[4] = pt.normal_y;
01588 ptr[5] = pt.normal_z;
01589 }
01590 else
01591 {
01592 ptr[0] = cloud.at(index).x;
01593 ptr[1] = cloud.at(index).y;
01594 ptr[2] = cloud.at(index).z;
01595 ptr[3] = cloud.at(index).normal_x;
01596 ptr[4] = cloud.at(index).normal_y;
01597 ptr[5] = cloud.at(index).normal_z;
01598 }
01599 }
01600 }
01601 }
01602 else
01603 {
01604 laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
01605 for(unsigned int i=0; i<cloud.size(); ++i)
01606 {
01607 if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
01608 uIsFinite(cloud.at(i).normal_x) &&
01609 uIsFinite(cloud.at(i).normal_y) &&
01610 uIsFinite(cloud.at(i).normal_z)))
01611 {
01612 float * ptr = laserScan.ptr<float>(0, oi++);
01613 if(!nullTransform)
01614 {
01615 pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
01616 ptr[0] = pt.x;
01617 ptr[1] = pt.y;
01618 ptr[2] = pt.z;
01619 ptr[3] = pt.normal_x;
01620 ptr[4] = pt.normal_y;
01621 ptr[5] = pt.normal_z;
01622 }
01623 else
01624 {
01625 ptr[0] = cloud.at(i).x;
01626 ptr[1] = cloud.at(i).y;
01627 ptr[2] = cloud.at(i).z;
01628 ptr[3] = cloud.at(i).normal_x;
01629 ptr[4] = cloud.at(i).normal_y;
01630 ptr[5] = cloud.at(i).normal_z;
01631 }
01632 }
01633 }
01634 }
01635 if(oi == 0)
01636 {
01637 return cv::Mat();
01638 }
01639 return laserScan(cv::Range::all(), cv::Range(0,oi));
01640 }
01641
01642 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
01643 {
01644 UASSERT(cloud.size() == normals.size());
01645 cv::Mat laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
01646 bool nullTransform = transform.isNull() || transform.isIdentity();
01647 int oi =0;
01648 for(unsigned int i=0; i<cloud.size(); ++i)
01649 {
01650 if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
01651 {
01652 float * ptr = laserScan.ptr<float>(0, oi++);
01653 if(!nullTransform)
01654 {
01655 pcl::PointNormal pt;
01656 pt.x = cloud.at(i).x;
01657 pt.y = cloud.at(i).y;
01658 pt.z = cloud.at(i).z;
01659 pt.normal_x = normals.at(i).normal_x;
01660 pt.normal_y = normals.at(i).normal_y;
01661 pt.normal_z = normals.at(i).normal_z;
01662 pt = util3d::transformPoint(pt, transform);
01663 ptr[0] = pt.x;
01664 ptr[1] = pt.y;
01665 ptr[2] = pt.z;
01666 ptr[3] = pt.normal_x;
01667 ptr[4] = pt.normal_y;
01668 ptr[5] = pt.normal_z;
01669 }
01670 else
01671 {
01672 ptr[0] = cloud.at(i).x;
01673 ptr[1] = cloud.at(i).y;
01674 ptr[2] = cloud.at(i).z;
01675 ptr[3] = normals.at(i).normal_x;
01676 ptr[4] = normals.at(i).normal_y;
01677 ptr[5] = normals.at(i).normal_z;
01678 }
01679 }
01680 }
01681 if(oi == 0)
01682 {
01683 return cv::Mat();
01684 }
01685 return laserScan(cv::Range::all(), cv::Range(0,oi));
01686 }
01687
01688 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform, bool filterNaNs)
01689 {
01690 return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
01691 }
01692
01693 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
01694 {
01695 cv::Mat laserScan;
01696 bool nullTransform = transform.isNull() || transform.isIdentity();
01697 Eigen::Affine3f transform3f = transform.toEigen3f();
01698 int oi=0;
01699 if(indices.get())
01700 {
01701 laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4));
01702 for(unsigned int i=0; i<indices->size(); ++i)
01703 {
01704 int index = indices->at(i);
01705 if(!filterNaNs || pcl::isFinite(cloud.at(index)))
01706 {
01707 float * ptr = laserScan.ptr<float>(0, oi++);
01708 if(!nullTransform)
01709 {
01710 pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(index), transform3f);
01711 ptr[0] = pt.x;
01712 ptr[1] = pt.y;
01713 ptr[2] = pt.z;
01714 }
01715 else
01716 {
01717 ptr[0] = cloud.at(index).x;
01718 ptr[1] = cloud.at(index).y;
01719 ptr[2] = cloud.at(index).z;
01720 }
01721 int * ptrInt = (int*)ptr;
01722 ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16);
01723 }
01724 }
01725 }
01726 else
01727 {
01728 laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
01729 for(unsigned int i=0; i<cloud.size(); ++i)
01730 {
01731 if(!filterNaNs || pcl::isFinite(cloud.at(i)))
01732 {
01733 float * ptr = laserScan.ptr<float>(0, oi++);
01734 if(!nullTransform)
01735 {
01736 pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(i), transform3f);
01737 ptr[0] = pt.x;
01738 ptr[1] = pt.y;
01739 ptr[2] = pt.z;
01740 }
01741 else
01742 {
01743 ptr[0] = cloud.at(i).x;
01744 ptr[1] = cloud.at(i).y;
01745 ptr[2] = cloud.at(i).z;
01746 }
01747 int * ptrInt = (int*)ptr;
01748 ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
01749 }
01750 }
01751 }
01752 if(oi == 0)
01753 {
01754 return cv::Mat();
01755 }
01756 return laserScan(cv::Range::all(), cv::Range(0,oi));
01757 }
01758
01759 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
01760 {
01761 return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
01762 }
01763
01764 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
01765 {
01766 cv::Mat laserScan;
01767 bool nullTransform = transform.isNull() || transform.isIdentity();
01768 Eigen::Affine3f transform3f = transform.toEigen3f();
01769 int oi=0;
01770 if(indices.get())
01771 {
01772 laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4));
01773 for(unsigned int i=0; i<indices->size(); ++i)
01774 {
01775 int index = indices->at(i);
01776 if(!filterNaNs || pcl::isFinite(cloud.at(index)))
01777 {
01778 float * ptr = laserScan.ptr<float>(0, oi++);
01779 if(!nullTransform)
01780 {
01781 pcl::PointXYZI pt = pcl::transformPoint(cloud.at(index), transform3f);
01782 ptr[0] = pt.x;
01783 ptr[1] = pt.y;
01784 ptr[2] = pt.z;
01785 }
01786 else
01787 {
01788 ptr[0] = cloud.at(index).x;
01789 ptr[1] = cloud.at(index).y;
01790 ptr[2] = cloud.at(index).z;
01791 }
01792 ptr[3] = cloud.at(index).intensity;
01793 }
01794 }
01795 }
01796 else
01797 {
01798 laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
01799 for(unsigned int i=0; i<cloud.size(); ++i)
01800 {
01801 if(!filterNaNs || pcl::isFinite(cloud.at(i)))
01802 {
01803 float * ptr = laserScan.ptr<float>(0, oi++);
01804 if(!nullTransform)
01805 {
01806 pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f);
01807 ptr[0] = pt.x;
01808 ptr[1] = pt.y;
01809 ptr[2] = pt.z;
01810 }
01811 else
01812 {
01813 ptr[0] = cloud.at(i).x;
01814 ptr[1] = cloud.at(i).y;
01815 ptr[2] = cloud.at(i).z;
01816 }
01817 ptr[3] = cloud.at(i).intensity;
01818 }
01819 }
01820 }
01821 if(oi == 0)
01822 {
01823 return cv::Mat();
01824 }
01825 return laserScan(cv::Range::all(), cv::Range(0,oi));
01826 }
01827
01828 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
01829 {
01830 UASSERT(cloud.size() == normals.size());
01831 cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
01832 bool nullTransform = transform.isNull() || transform.isIdentity();
01833 int oi = 0;
01834 for(unsigned int i=0; i<cloud.size(); ++i)
01835 {
01836 if(!filterNaNs || pcl::isFinite(cloud.at(i)))
01837 {
01838 float * ptr = laserScan.ptr<float>(0, oi++);
01839 if(!nullTransform)
01840 {
01841 pcl::PointXYZRGBNormal pt;
01842 pt.x = cloud.at(i).x;
01843 pt.y = cloud.at(i).y;
01844 pt.z = cloud.at(i).z;
01845 pt.normal_x = normals.at(i).normal_x;
01846 pt.normal_y = normals.at(i).normal_y;
01847 pt.normal_z = normals.at(i).normal_z;
01848 pt = util3d::transformPoint(pt, transform);
01849 ptr[0] = pt.x;
01850 ptr[1] = pt.y;
01851 ptr[2] = pt.z;
01852 ptr[4] = pt.normal_x;
01853 ptr[5] = pt.normal_y;
01854 ptr[6] = pt.normal_z;
01855 }
01856 else
01857 {
01858 ptr[0] = cloud.at(i).x;
01859 ptr[1] = cloud.at(i).y;
01860 ptr[2] = cloud.at(i).z;
01861 ptr[4] = normals.at(i).normal_x;
01862 ptr[5] = normals.at(i).normal_y;
01863 ptr[6] = normals.at(i).normal_z;
01864 }
01865 int * ptrInt = (int*)ptr;
01866 ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
01867 }
01868 }
01869 if(oi == 0)
01870 {
01871 return cv::Mat();
01872 }
01873 return laserScan(cv::Range::all(), cv::Range(0,oi));
01874 }
01875
01876 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform, bool filterNaNs)
01877 {
01878 return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
01879 }
01880 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
01881 {
01882 cv::Mat laserScan;
01883 bool nullTransform = transform.isNull() || transform.isIdentity();
01884 int oi = 0;
01885 if(indices.get())
01886 {
01887 laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(7));
01888 for(unsigned int i=0; i<indices->size(); ++i)
01889 {
01890 int index = indices->at(i);
01891 if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
01892 uIsFinite(cloud.at(index).normal_x) &&
01893 uIsFinite(cloud.at(index).normal_y) &&
01894 uIsFinite(cloud.at(index).normal_z)))
01895 {
01896 float * ptr = laserScan.ptr<float>(0, oi++);
01897 if(!nullTransform)
01898 {
01899 pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(index), transform);
01900 ptr[0] = pt.x;
01901 ptr[1] = pt.y;
01902 ptr[2] = pt.z;
01903 ptr[4] = pt.normal_x;
01904 ptr[5] = pt.normal_y;
01905 ptr[6] = pt.normal_z;
01906 }
01907 else
01908 {
01909 ptr[0] = cloud.at(index).x;
01910 ptr[1] = cloud.at(index).y;
01911 ptr[2] = cloud.at(index).z;
01912 ptr[4] = cloud.at(index).normal_x;
01913 ptr[5] = cloud.at(index).normal_y;
01914 ptr[6] = cloud.at(index).normal_z;
01915 }
01916 int * ptrInt = (int*)ptr;
01917 ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16);
01918 }
01919 }
01920 }
01921 else
01922 {
01923 laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(7));
01924 for(unsigned int i=0; i<cloud.size(); ++i)
01925 {
01926 if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
01927 uIsFinite(cloud.at(i).normal_x) &&
01928 uIsFinite(cloud.at(i).normal_y) &&
01929 uIsFinite(cloud.at(i).normal_z)))
01930 {
01931 float * ptr = laserScan.ptr<float>(0, oi++);
01932 if(!nullTransform)
01933 {
01934 pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(i), transform);
01935 ptr[0] = pt.x;
01936 ptr[1] = pt.y;
01937 ptr[2] = pt.z;
01938 ptr[4] = pt.normal_x;
01939 ptr[5] = pt.normal_y;
01940 ptr[6] = pt.normal_z;
01941 }
01942 else
01943 {
01944 ptr[0] = cloud.at(i).x;
01945 ptr[1] = cloud.at(i).y;
01946 ptr[2] = cloud.at(i).z;
01947 ptr[4] = cloud.at(i).normal_x;
01948 ptr[5] = cloud.at(i).normal_y;
01949 ptr[6] = cloud.at(i).normal_z;
01950 }
01951 int * ptrInt = (int*)ptr;
01952 ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
01953 }
01954 }
01955 }
01956 if(oi == 0)
01957 {
01958 return cv::Mat();
01959 }
01960 return laserScan(cv::Range::all(), cv::Range(0,oi));
01961 }
01962
01963 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
01964 {
01965 UASSERT(cloud.size() == normals.size());
01966 cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
01967 bool nullTransform = transform.isNull() || transform.isIdentity();
01968 int oi=0;
01969 for(unsigned int i=0; i<cloud.size(); ++i)
01970 {
01971 if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
01972 {
01973 float * ptr = laserScan.ptr<float>(0, oi++);
01974 if(!nullTransform)
01975 {
01976 pcl::PointXYZINormal pt;
01977 pt.x = cloud.at(i).x;
01978 pt.y = cloud.at(i).y;
01979 pt.z = cloud.at(i).z;
01980 pt.normal_x = normals.at(i).normal_x;
01981 pt.normal_y = normals.at(i).normal_y;
01982 pt.normal_z = normals.at(i).normal_z;
01983 pt = util3d::transformPoint(pt, transform);
01984 ptr[0] = pt.x;
01985 ptr[1] = pt.y;
01986 ptr[2] = pt.z;
01987 ptr[4] = pt.normal_x;
01988 ptr[5] = pt.normal_y;
01989 ptr[6] = pt.normal_z;
01990 }
01991 else
01992 {
01993 ptr[0] = cloud.at(i).x;
01994 ptr[1] = cloud.at(i).y;
01995 ptr[2] = cloud.at(i).z;
01996 ptr[4] = normals.at(i).normal_x;
01997 ptr[5] = normals.at(i).normal_y;
01998 ptr[6] = normals.at(i).normal_z;
01999 }
02000 ptr[3] = cloud.at(i).intensity;
02001 }
02002 }
02003 if(oi == 0)
02004 {
02005 return cv::Mat();
02006 }
02007 return laserScan(cv::Range::all(), cv::Range(0,oi));
02008 }
02009 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
02010 {
02011 cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
02012 bool nullTransform = transform.isNull() || transform.isIdentity();
02013 int oi = 0;
02014 for(unsigned int i=0; i<cloud.size(); ++i)
02015 {
02016 if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
02017 uIsFinite(cloud.at(i).normal_x) &&
02018 uIsFinite(cloud.at(i).normal_y) &&
02019 uIsFinite(cloud.at(i).normal_z)))
02020 {
02021 float * ptr = laserScan.ptr<float>(0, oi++);
02022 if(!nullTransform)
02023 {
02024 pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform);
02025 ptr[0] = pt.x;
02026 ptr[1] = pt.y;
02027 ptr[2] = pt.z;
02028 ptr[4] = pt.normal_x;
02029 ptr[5] = pt.normal_y;
02030 ptr[6] = pt.normal_z;
02031 }
02032 else
02033 {
02034 ptr[0] = cloud.at(i).x;
02035 ptr[1] = cloud.at(i).y;
02036 ptr[2] = cloud.at(i).z;
02037 ptr[4] = cloud.at(i).normal_x;
02038 ptr[5] = cloud.at(i).normal_y;
02039 ptr[6] = cloud.at(i).normal_z;
02040 }
02041 ptr[3] = cloud.at(i).intensity;
02042 }
02043 }
02044 if(oi == 0)
02045 {
02046 return cv::Mat();
02047 }
02048 return laserScan(cv::Range::all(), cv::Range(0,oi));
02049 }
02050
02051 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
02052 {
02053 cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
02054 bool nullTransform = transform.isNull();
02055 Eigen::Affine3f transform3f = transform.toEigen3f();
02056 int oi=0;
02057 for(unsigned int i=0; i<cloud.size(); ++i)
02058 {
02059 if(!filterNaNs || pcl::isFinite(cloud.at(i)))
02060 {
02061 float * ptr = laserScan.ptr<float>(0, oi++);
02062 if(!nullTransform)
02063 {
02064 pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
02065 ptr[0] = pt.x;
02066 ptr[1] = pt.y;
02067 }
02068 else
02069 {
02070 ptr[0] = cloud.at(i).x;
02071 ptr[1] = cloud.at(i).y;
02072 }
02073 }
02074
02075 }
02076 if(oi == 0)
02077 {
02078 return cv::Mat();
02079 }
02080 return laserScan(cv::Range::all(), cv::Range(0,oi));
02081 }
02082
02083 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
02084 {
02085 cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
02086 bool nullTransform = transform.isNull();
02087 Eigen::Affine3f transform3f = transform.toEigen3f();
02088 int oi=0;
02089 for(unsigned int i=0; i<cloud.size(); ++i)
02090 {
02091 if(!filterNaNs || pcl::isFinite(cloud.at(i)))
02092 {
02093 float * ptr = laserScan.ptr<float>(0, oi++);
02094 if(!nullTransform)
02095 {
02096 pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f);
02097 ptr[0] = pt.x;
02098 ptr[1] = pt.y;
02099 ptr[2] = pt.intensity;
02100 }
02101 else
02102 {
02103 ptr[0] = cloud.at(i).x;
02104 ptr[1] = cloud.at(i).y;
02105 ptr[2] = cloud.at(i).intensity;
02106 }
02107 }
02108
02109 }
02110 if(oi == 0)
02111 {
02112 return cv::Mat();
02113 }
02114 return laserScan(cv::Range::all(), cv::Range(0,oi));
02115 }
02116
02117 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
02118 {
02119 cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
02120 bool nullTransform = transform.isNull();
02121 int oi=0;
02122 for(unsigned int i=0; i<cloud.size(); ++i)
02123 {
02124 if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
02125 uIsFinite(cloud.at(i).normal_x) &&
02126 uIsFinite(cloud.at(i).normal_y) &&
02127 uIsFinite(cloud.at(i).normal_z)))
02128 {
02129 float * ptr = laserScan.ptr<float>(0, oi++);
02130 if(!nullTransform)
02131 {
02132 pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
02133 ptr[0] = pt.x;
02134 ptr[1] = pt.y;
02135 ptr[2] = pt.normal_x;
02136 ptr[3] = pt.normal_y;
02137 ptr[4] = pt.normal_z;
02138 }
02139 else
02140 {
02141 const pcl::PointNormal & pt = cloud.at(i);
02142 ptr[0] = pt.x;
02143 ptr[1] = pt.y;
02144 ptr[2] = pt.normal_x;
02145 ptr[3] = pt.normal_y;
02146 ptr[4] = pt.normal_z;
02147 }
02148 }
02149 }
02150 if(oi == 0)
02151 {
02152 return cv::Mat();
02153 }
02154 return laserScan(cv::Range::all(), cv::Range(0,oi));
02155 }
02156
02157 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
02158 {
02159 UASSERT(cloud.size() == normals.size());
02160 cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
02161 bool nullTransform = transform.isNull() || transform.isIdentity();
02162 int oi=0;
02163 for(unsigned int i=0; i<cloud.size(); ++i)
02164 {
02165 if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
02166 {
02167 float * ptr = laserScan.ptr<float>(0, oi++);
02168 if(!nullTransform)
02169 {
02170 pcl::PointNormal pt;
02171 pt.x = cloud.at(i).x;
02172 pt.y = cloud.at(i).y;
02173 pt.z = cloud.at(i).z;
02174 pt.normal_x = normals.at(i).normal_x;
02175 pt.normal_y = normals.at(i).normal_y;
02176 pt.normal_z = normals.at(i).normal_z;
02177 pt = util3d::transformPoint(pt, transform);
02178 ptr[0] = pt.x;
02179 ptr[1] = pt.y;
02180 ptr[2] = pt.normal_x;
02181 ptr[3] = pt.normal_y;
02182 ptr[4] = pt.normal_z;
02183 }
02184 else
02185 {
02186 ptr[0] = cloud.at(i).x;
02187 ptr[1] = cloud.at(i).y;
02188 ptr[2] = normals.at(i).normal_x;
02189 ptr[3] = normals.at(i).normal_y;
02190 ptr[4] = normals.at(i).normal_z;
02191 }
02192 }
02193 }
02194 if(oi == 0)
02195 {
02196 return cv::Mat();
02197 }
02198 return laserScan(cv::Range::all(), cv::Range(0,oi));
02199 }
02200
02201 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
02202 {
02203 cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
02204 bool nullTransform = transform.isNull();
02205 int oi=0;
02206 for(unsigned int i=0; i<cloud.size(); ++i)
02207 {
02208 if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
02209 uIsFinite(cloud.at(i).normal_x) &&
02210 uIsFinite(cloud.at(i).normal_y) &&
02211 uIsFinite(cloud.at(i).normal_z)))
02212 {
02213 float * ptr = laserScan.ptr<float>(0, oi++);
02214 if(!nullTransform)
02215 {
02216 pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform);
02217 ptr[0] = pt.x;
02218 ptr[1] = pt.y;
02219 ptr[2] = pt.intensity;
02220 ptr[3] = pt.normal_x;
02221 ptr[4] = pt.normal_y;
02222 ptr[5] = pt.normal_z;
02223 }
02224 else
02225 {
02226 const pcl::PointXYZINormal & pt = cloud.at(i);
02227 ptr[0] = pt.x;
02228 ptr[1] = pt.y;
02229 ptr[2] = pt.intensity;
02230 ptr[3] = pt.normal_x;
02231 ptr[4] = pt.normal_y;
02232 ptr[5] = pt.normal_z;
02233 }
02234 }
02235 }
02236 if(oi == 0)
02237 {
02238 return cv::Mat();
02239 }
02240 return laserScan(cv::Range::all(), cv::Range(0,oi));
02241 }
02242
02243 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
02244 {
02245 UASSERT(cloud.size() == normals.size());
02246 cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
02247 bool nullTransform = transform.isNull() || transform.isIdentity();
02248 int oi=0;
02249 for(unsigned int i=0; i<cloud.size(); ++i)
02250 {
02251 if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
02252 {
02253 float * ptr = laserScan.ptr<float>(0, oi++);
02254 if(!nullTransform)
02255 {
02256 pcl::PointXYZINormal pt;
02257 pt.x = cloud.at(i).x;
02258 pt.y = cloud.at(i).y;
02259 pt.z = cloud.at(i).z;
02260 pt.normal_x = normals.at(i).normal_x;
02261 pt.normal_y = normals.at(i).normal_y;
02262 pt.normal_z = normals.at(i).normal_z;
02263 pt = util3d::transformPoint(pt, transform);
02264 ptr[0] = pt.x;
02265 ptr[1] = pt.y;
02266 ptr[2] = pt.intensity;
02267 ptr[3] = pt.normal_x;
02268 ptr[4] = pt.normal_y;
02269 ptr[5] = pt.normal_z;
02270 }
02271 else
02272 {
02273 ptr[0] = cloud.at(i).x;
02274 ptr[1] = cloud.at(i).y;
02275 ptr[2] = cloud.at(i).intensity;
02276 ptr[3] = normals.at(i).normal_x;
02277 ptr[4] = normals.at(i).normal_y;
02278 ptr[5] = normals.at(i).normal_z;
02279 }
02280 }
02281 }
02282 if(oi == 0)
02283 {
02284 return cv::Mat();
02285 }
02286 return laserScan(cv::Range::all(), cv::Range(0,oi));
02287 }
02288
02289 pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform)
02290 {
02291 pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
02292 if(laserScan.isEmpty())
02293 {
02294 return cloud;
02295 }
02296
02297 if(laserScan.format() == LaserScan::kXY || laserScan.format() == LaserScan::kXYZ)
02298 {
02299 pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
02300 }
02301 else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
02302 {
02303 pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
02304 }
02305 else if(laserScan.format() == LaserScan::kXYNormal || laserScan.format() == LaserScan::kXYZNormal)
02306 {
02307 pcl::toPCLPointCloud2(*laserScanToPointCloudNormal(laserScan, transform), *cloud);
02308 }
02309 else if(laserScan.format() == LaserScan::kXYINormal || laserScan.format() == LaserScan::kXYZINormal)
02310 {
02311 pcl::toPCLPointCloud2(*laserScanToPointCloudINormal(laserScan, transform), *cloud);
02312 }
02313 else if(laserScan.format() == LaserScan::kXYZRGB)
02314 {
02315 pcl::toPCLPointCloud2(*laserScanToPointCloudRGB(laserScan, transform), *cloud);
02316 }
02317 else if(laserScan.format() == LaserScan::kXYZRGBNormal)
02318 {
02319 pcl::toPCLPointCloud2(*laserScanToPointCloudRGBNormal(laserScan, transform), *cloud);
02320 }
02321 else
02322 {
02323 UERROR("Unknown conversion from LaserScan format %d to PointCloud2.", laserScan.format());
02324 }
02325 return cloud;
02326 }
02327
02328 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform)
02329 {
02330 pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
02331 output->resize(laserScan.size());
02332 output->is_dense = true;
02333 bool nullTransform = transform.isNull();
02334 Eigen::Affine3f transform3f = transform.toEigen3f();
02335 for(int i=0; i<laserScan.size(); ++i)
02336 {
02337 output->at(i) = util3d::laserScanToPoint(laserScan, i);
02338 if(!nullTransform)
02339 {
02340 output->at(i) = pcl::transformPoint(output->at(i), transform3f);
02341 }
02342 }
02343 return output;
02344 }
02345
02346 pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform)
02347 {
02348 pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
02349 output->resize(laserScan.size());
02350 output->is_dense = true;
02351 bool nullTransform = transform.isNull();
02352 for(int i=0; i<laserScan.size(); ++i)
02353 {
02354 output->at(i) = laserScanToPointNormal(laserScan, i);
02355 if(!nullTransform)
02356 {
02357 output->at(i) = util3d::transformPoint(output->at(i), transform);
02358 }
02359 }
02360 return output;
02361 }
02362
02363 pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
02364 {
02365 pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
02366 output->resize(laserScan.size());
02367 output->is_dense = true;
02368 bool nullTransform = transform.isNull() || transform.isIdentity();
02369 Eigen::Affine3f transform3f = transform.toEigen3f();
02370 for(int i=0; i<laserScan.size(); ++i)
02371 {
02372 output->at(i) = util3d::laserScanToPointRGB(laserScan, i, r, g, b);
02373 if(!nullTransform)
02374 {
02375 output->at(i) = pcl::transformPoint(output->at(i), transform3f);
02376 }
02377 }
02378 return output;
02379 }
02380
02381 pcl::PointCloud<pcl::PointXYZI>::Ptr laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform, float intensity)
02382 {
02383 pcl::PointCloud<pcl::PointXYZI>::Ptr output(new pcl::PointCloud<pcl::PointXYZI>);
02384 output->resize(laserScan.size());
02385 output->is_dense = true;
02386 bool nullTransform = transform.isNull() || transform.isIdentity();
02387 Eigen::Affine3f transform3f = transform.toEigen3f();
02388 for(int i=0; i<laserScan.size(); ++i)
02389 {
02390 output->at(i) = util3d::laserScanToPointI(laserScan, i, intensity);
02391 if(!nullTransform)
02392 {
02393 output->at(i) = pcl::transformPoint(output->at(i), transform3f);
02394 }
02395 }
02396 return output;
02397 }
02398
02399 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
02400 {
02401 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
02402 output->resize(laserScan.size());
02403 output->is_dense = true;
02404 bool nullTransform = transform.isNull() || transform.isIdentity();
02405 for(int i=0; i<laserScan.size(); ++i)
02406 {
02407 output->at(i) = util3d::laserScanToPointRGBNormal(laserScan, i, r, g, b);
02408 if(!nullTransform)
02409 {
02410 output->at(i) = util3d::transformPoint(output->at(i), transform);
02411 }
02412 }
02413 return output;
02414 }
02415
02416 pcl::PointCloud<pcl::PointXYZINormal>::Ptr laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform, float intensity)
02417 {
02418 pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZINormal>);
02419 output->resize(laserScan.size());
02420 output->is_dense = true;
02421 bool nullTransform = transform.isNull() || transform.isIdentity();
02422 for(int i=0; i<laserScan.size(); ++i)
02423 {
02424 output->at(i) = util3d::laserScanToPointINormal(laserScan, i, intensity);
02425 if(!nullTransform)
02426 {
02427 output->at(i) = util3d::transformPoint(output->at(i), transform);
02428 }
02429 }
02430 return output;
02431 }
02432
02433 pcl::PointXYZ laserScanToPoint(const LaserScan & laserScan, int index)
02434 {
02435 UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
02436 pcl::PointXYZ output;
02437 const float * ptr = laserScan.data().ptr<float>(0, index);
02438 output.x = ptr[0];
02439 output.y = ptr[1];
02440 if(!laserScan.is2d())
02441 {
02442 output.z = ptr[2];
02443 }
02444 return output;
02445 }
02446
02447 pcl::PointNormal laserScanToPointNormal(const LaserScan & laserScan, int index)
02448 {
02449 UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
02450 pcl::PointNormal output;
02451 const float * ptr = laserScan.data().ptr<float>(0, index);
02452 output.x = ptr[0];
02453 output.y = ptr[1];
02454 if(!laserScan.is2d())
02455 {
02456 output.z = ptr[2];
02457 }
02458 if(laserScan.hasNormals())
02459 {
02460 int offset = laserScan.getNormalsOffset();
02461 output.normal_x = ptr[offset];
02462 output.normal_y = ptr[offset+1];
02463 output.normal_z = ptr[offset+2];
02464 }
02465 return output;
02466 }
02467
02468 pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
02469 {
02470 UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
02471 pcl::PointXYZRGB output;
02472 const float * ptr = laserScan.data().ptr<float>(0, index);
02473 output.x = ptr[0];
02474 output.y = ptr[1];
02475 if(!laserScan.is2d())
02476 {
02477 output.z = ptr[2];
02478 }
02479
02480 if(laserScan.hasRGB())
02481 {
02482 int * ptrInt = (int*)ptr;
02483 int indexRGB = laserScan.getRGBOffset();
02484 output.b = (unsigned char)(ptrInt[indexRGB] & 0xFF);
02485 output.g = (unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF);
02486 output.r = (unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF);
02487 }
02488 else
02489 {
02490 output.r = r;
02491 output.g = g;
02492 output.b = b;
02493 }
02494 return output;
02495 }
02496
02497 pcl::PointXYZI laserScanToPointI(const LaserScan & laserScan, int index, float intensity)
02498 {
02499 UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
02500 pcl::PointXYZI output;
02501 const float * ptr = laserScan.data().ptr<float>(0, index);
02502 output.x = ptr[0];
02503 output.y = ptr[1];
02504 if(!laserScan.is2d())
02505 {
02506 output.z = ptr[2];
02507 }
02508
02509 if(laserScan.hasIntensity())
02510 {
02511 int offset = laserScan.getIntensityOffset();
02512 output.intensity = ptr[offset];
02513 }
02514 else
02515 {
02516 output.intensity = intensity;
02517 }
02518
02519 return output;
02520 }
02521
02522 pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
02523 {
02524 UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
02525 pcl::PointXYZRGBNormal output;
02526 const float * ptr = laserScan.data().ptr<float>(0, index);
02527 output.x = ptr[0];
02528 output.y = ptr[1];
02529 if(!laserScan.is2d())
02530 {
02531 output.z = ptr[2];
02532 }
02533
02534 if(laserScan.hasRGB())
02535 {
02536 int * ptrInt = (int*)ptr;
02537 int indexRGB = laserScan.getRGBOffset();
02538 output.b = (unsigned char)(ptrInt[indexRGB] & 0xFF);
02539 output.g = (unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF);
02540 output.r = (unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF);
02541 }
02542 else
02543 {
02544 output.r = r;
02545 output.g = g;
02546 output.b = b;
02547 }
02548
02549 if(laserScan.hasNormals())
02550 {
02551 int offset = laserScan.getNormalsOffset();
02552 output.normal_x = ptr[offset];
02553 output.normal_y = ptr[offset+1];
02554 output.normal_z = ptr[offset+2];
02555 }
02556
02557 return output;
02558 }
02559
02560 pcl::PointXYZINormal laserScanToPointINormal(const LaserScan & laserScan, int index, float intensity)
02561 {
02562 UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
02563 pcl::PointXYZINormal output;
02564 const float * ptr = laserScan.data().ptr<float>(0, index);
02565 output.x = ptr[0];
02566 output.y = ptr[1];
02567 if(!laserScan.is2d())
02568 {
02569 output.z = ptr[2];
02570 }
02571
02572 if(laserScan.hasIntensity())
02573 {
02574 int offset = laserScan.getIntensityOffset();
02575 output.intensity = ptr[offset];
02576 }
02577 else
02578 {
02579 output.intensity = intensity;
02580 }
02581
02582 if(laserScan.hasNormals())
02583 {
02584 int offset = laserScan.getNormalsOffset();
02585 output.normal_x = ptr[offset];
02586 output.normal_y = ptr[offset+1];
02587 output.normal_z = ptr[offset+2];
02588 }
02589
02590 return output;
02591 }
02592
02593 void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max)
02594 {
02595 UASSERT(!laserScan.empty());
02596 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));
02597
02598 const float * ptr = laserScan.ptr<float>(0, 0);
02599 min.x = max.x = ptr[0];
02600 min.y = max.y = ptr[1];
02601 bool is3d = laserScan.channels() >= 3 && laserScan.channels() != 5;
02602 min.z = max.z = is3d?ptr[2]:0.0f;
02603 for(int i=1; i<laserScan.cols; ++i)
02604 {
02605 ptr = laserScan.ptr<float>(0, i);
02606
02607 if(ptr[0] < min.x) min.x = ptr[0];
02608 else if(ptr[0] > max.x) max.x = ptr[0];
02609
02610 if(ptr[1] < min.y) min.y = ptr[1];
02611 else if(ptr[1] > max.y) max.y = ptr[1];
02612
02613 if(is3d)
02614 {
02615 if(ptr[2] < min.z) min.z = ptr[2];
02616 else if(ptr[2] > max.z) max.z = ptr[2];
02617 }
02618 }
02619 }
02620 void getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max)
02621 {
02622 cv::Point3f minCV, maxCV;
02623 getMinMax3D(laserScan, minCV, maxCV);
02624 min.x = minCV.x;
02625 min.y = minCV.y;
02626 min.z = minCV.z;
02627 max.x = maxCV.x;
02628 max.y = maxCV.y;
02629 max.z = maxCV.z;
02630 }
02631
02632
02633 cv::Point3f projectDisparityTo3D(
02634 const cv::Point2f & pt,
02635 float disparity,
02636 const StereoCameraModel & model)
02637 {
02638 if(disparity > 0.0f && model.baseline() > 0.0f && model.left().fx() > 0.0f)
02639 {
02640
02641 float c = 0.0f;
02642 if(model.right().cx()>0.0f && model.left().cx()>0.0f)
02643 {
02644 c = model.right().cx() - model.left().cx();
02645 }
02646 float W = model.baseline()/(disparity + c);
02647 return cv::Point3f((pt.x - model.left().cx())*W, (pt.y - model.left().cy())*W, model.left().fx()*W);
02648 }
02649 float bad_point = std::numeric_limits<float>::quiet_NaN ();
02650 return cv::Point3f(bad_point, bad_point, bad_point);
02651 }
02652
02653 cv::Point3f projectDisparityTo3D(
02654 const cv::Point2f & pt,
02655 const cv::Mat & disparity,
02656 const StereoCameraModel & model)
02657 {
02658 UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
02659 int u = int(pt.x+0.5f);
02660 int v = int(pt.y+0.5f);
02661 float bad_point = std::numeric_limits<float>::quiet_NaN ();
02662 if(uIsInBounds(u, 0, disparity.cols) &&
02663 uIsInBounds(v, 0, disparity.rows))
02664 {
02665 float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
02666 return projectDisparityTo3D(pt, d, model);
02667 }
02668 return cv::Point3f(bad_point, bad_point, bad_point);
02669 }
02670
02671
02672 cv::Mat projectCloudToCamera(
02673 const cv::Size & imageSize,
02674 const cv::Mat & cameraMatrixK,
02675 const cv::Mat & laserScan,
02676 const rtabmap::Transform & cameraTransform)
02677 {
02678 UASSERT(!cameraTransform.isNull());
02679 UASSERT(!laserScan.empty());
02680 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));
02681 UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
02682
02683 float fx = cameraMatrixK.at<double>(0,0);
02684 float fy = cameraMatrixK.at<double>(1,1);
02685 float cx = cameraMatrixK.at<double>(0,2);
02686 float cy = cameraMatrixK.at<double>(1,2);
02687
02688 cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
02689 Transform t = cameraTransform.inverse();
02690
02691 int count = 0;
02692 for(int i=0; i<laserScan.cols; ++i)
02693 {
02694 const float* ptr = laserScan.ptr<float>(0, i);
02695
02696
02697 cv::Point3f ptScan;
02698 if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC(5))
02699 {
02700
02701 ptScan.x = ptr[0];
02702 ptScan.y = ptr[1];
02703 ptScan.z = 0;
02704 }
02705 else
02706 {
02707 ptScan.x = ptr[0];
02708 ptScan.y = ptr[1];
02709 ptScan.z = ptr[2];
02710 }
02711 ptScan = util3d::transformPoint(ptScan, t);
02712
02713
02714 float z = ptScan.z;
02715
02716 bool set = false;
02717 if(z > 0.0f)
02718 {
02719 float invZ = 1.0f/z;
02720 float dx = (fx*ptScan.x)*invZ + cx;
02721 float dy = (fy*ptScan.y)*invZ + cy;
02722 int dx_low = dx;
02723 int dy_low = dy;
02724 int dx_high = dx + 0.5f;
02725 int dy_high = dy + 0.5f;
02726
02727 if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
02728 {
02729 float &zReg = registered.at<float>(dy_low, dx_low);
02730 if(zReg == 0 || z < zReg)
02731 {
02732 zReg = z;
02733 }
02734 set = true;
02735 }
02736 if((dx_low != dx_high || dy_low != dy_high) &&
02737 uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
02738 {
02739 float &zReg = registered.at<float>(dy_high, dx_high);
02740 if(zReg == 0 || z < zReg)
02741 {
02742 zReg = z;
02743 }
02744 set = true;
02745 }
02746 }
02747 if(set)
02748 {
02749 count++;
02750 }
02751 }
02752 UDEBUG("Points in camera=%d/%d", count, laserScan.cols);
02753
02754 return registered;
02755 }
02756
02757 cv::Mat projectCloudToCamera(
02758 const cv::Size & imageSize,
02759 const cv::Mat & cameraMatrixK,
02760 const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan,
02761 const rtabmap::Transform & cameraTransform)
02762 {
02763 UASSERT(!cameraTransform.isNull());
02764 UASSERT(!laserScan->empty());
02765 UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
02766
02767 float fx = cameraMatrixK.at<double>(0,0);
02768 float fy = cameraMatrixK.at<double>(1,1);
02769 float cx = cameraMatrixK.at<double>(0,2);
02770 float cy = cameraMatrixK.at<double>(1,2);
02771
02772 cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
02773 Transform t = cameraTransform.inverse();
02774
02775 int count = 0;
02776 for(int i=0; i<(int)laserScan->size(); ++i)
02777 {
02778
02779 pcl::PointXYZ ptScan = laserScan->at(i);
02780 ptScan = util3d::transformPoint(ptScan, t);
02781
02782
02783 float z = ptScan.z;
02784 bool set = false;
02785 if(z > 0.0f)
02786 {
02787 float invZ = 1.0f/z;
02788 float dx = (fx*ptScan.x)*invZ + cx;
02789 float dy = (fy*ptScan.y)*invZ + cy;
02790 int dx_low = dx;
02791 int dy_low = dy;
02792 int dx_high = dx + 0.5f;
02793 int dy_high = dy + 0.5f;
02794 if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
02795 {
02796 set = true;
02797 float &zReg = registered.at<float>(dy_low, dx_low);
02798 if(zReg == 0 || z < zReg)
02799 {
02800 zReg = z;
02801 }
02802 }
02803 if((dx_low != dx_high || dy_low != dy_high) &&
02804 uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
02805 {
02806 set = true;
02807 float &zReg = registered.at<float>(dy_high, dx_high);
02808 if(zReg == 0 || z < zReg)
02809 {
02810 zReg = z;
02811 }
02812 }
02813 }
02814 if(set)
02815 {
02816 count++;
02817 }
02818 }
02819 UDEBUG("Points in camera=%d/%d", count, (int)laserScan->size());
02820
02821 return registered;
02822 }
02823
02824 cv::Mat projectCloudToCamera(
02825 const cv::Size & imageSize,
02826 const cv::Mat & cameraMatrixK,
02827 const pcl::PCLPointCloud2::Ptr laserScan,
02828 const rtabmap::Transform & cameraTransform)
02829 {
02830 UASSERT(!cameraTransform.isNull());
02831 UASSERT(!laserScan->data.empty());
02832 UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
02833
02834 float fx = cameraMatrixK.at<double>(0,0);
02835 float fy = cameraMatrixK.at<double>(1,1);
02836 float cx = cameraMatrixK.at<double>(0,2);
02837 float cy = cameraMatrixK.at<double>(1,2);
02838
02839 cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
02840 Transform t = cameraTransform.inverse();
02841
02842 pcl::MsgFieldMap field_map;
02843 pcl::createMapping<pcl::PointXYZ> (laserScan->fields, field_map);
02844
02845 int count = 0;
02846 if(field_map.size() == 1)
02847 {
02848 for (uint32_t row = 0; row < laserScan->height; ++row)
02849 {
02850 const uint8_t* row_data = &laserScan->data[row * laserScan->row_step];
02851 for (uint32_t col = 0; col < laserScan->width; ++col)
02852 {
02853 const uint8_t* msg_data = row_data + col * laserScan->point_step;
02854 pcl::PointXYZ ptScan;
02855 memcpy (&ptScan, msg_data + field_map.front().serialized_offset, field_map.front().size);
02856 ptScan = util3d::transformPoint(ptScan, t);
02857
02858
02859 float z = ptScan.z;
02860 bool set = false;
02861 if(z > 0.0f)
02862 {
02863 float invZ = 1.0f/z;
02864 float dx = (fx*ptScan.x)*invZ + cx;
02865 float dy = (fy*ptScan.y)*invZ + cy;
02866 int dx_low = dx;
02867 int dy_low = dy;
02868 int dx_high = dx + 0.5f;
02869 int dy_high = dy + 0.5f;
02870 if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
02871 {
02872 set = true;
02873 float &zReg = registered.at<float>(dy_low, dx_low);
02874 if(zReg == 0 || z < zReg)
02875 {
02876 zReg = z;
02877 }
02878 }
02879 if((dx_low != dx_high || dy_low != dy_high) &&
02880 uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
02881 {
02882 set = true;
02883 float &zReg = registered.at<float>(dy_high, dx_high);
02884 if(zReg == 0 || z < zReg)
02885 {
02886 zReg = z;
02887 }
02888 }
02889 }
02890 if(set)
02891 {
02892 count++;
02893 }
02894 }
02895 }
02896 }
02897 else
02898 {
02899 UERROR("field map pcl::pointXYZ not found!");
02900 }
02901
02902
02903
02904
02905
02906
02907
02908
02909
02910
02911
02912
02913
02914
02915
02916
02917
02918
02919
02920
02921
02922
02923
02924
02925
02926
02927
02928
02929
02930
02931
02932
02933
02934
02935
02936
02937
02938
02939
02940
02941
02942
02943
02944
02945
02946
02947 UDEBUG("Points in camera=%d/%d", count, (int)laserScan->data.size());
02948
02949 return registered;
02950 }
02951
02952 void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection, bool fillToBorder)
02953 {
02954 UASSERT(registeredDepth.type() == CV_32FC1);
02955 if(verticalDirection)
02956 {
02957
02958 for(int x=0; x<registeredDepth.cols; ++x)
02959 {
02960 float valueA = 0.0f;
02961 int indexA = -1;
02962 for(int y=0; y<registeredDepth.rows; ++y)
02963 {
02964 float v = registeredDepth.at<float>(y,x);
02965 if(fillToBorder && y == registeredDepth.rows-1 && v<=0.0f && indexA>=0)
02966 {
02967 v = valueA;
02968 }
02969 if(v > 0.0f)
02970 {
02971 if(fillToBorder && indexA < 0)
02972 {
02973 indexA = 0;
02974 valueA = v;
02975 }
02976 if(indexA >=0)
02977 {
02978 int range = y-indexA;
02979 if(range > 1)
02980 {
02981 float slope = (v-valueA)/(range);
02982 for(int k=1; k<range; ++k)
02983 {
02984 registeredDepth.at<float>(indexA+k,x) = valueA+slope*float(k);
02985 }
02986 }
02987 }
02988 valueA = v;
02989 indexA = y;
02990 }
02991 }
02992 }
02993 }
02994 else
02995 {
02996
02997 for(int y=0; y<registeredDepth.rows; ++y)
02998 {
02999 float valueA = 0.0f;
03000 int indexA = -1;
03001 for(int x=0; x<registeredDepth.cols; ++x)
03002 {
03003 float v = registeredDepth.at<float>(y,x);
03004 if(fillToBorder && x == registeredDepth.cols-1 && v<=0.0f && indexA>=0)
03005 {
03006 v = valueA;
03007 }
03008 if(v > 0.0f)
03009 {
03010 if(fillToBorder && indexA < 0)
03011 {
03012 indexA = 0;
03013 valueA = v;
03014 }
03015 if(indexA >=0)
03016 {
03017 int range = x-indexA;
03018 if(range > 1)
03019 {
03020 float slope = (v-valueA)/(range);
03021 for(int k=1; k<range; ++k)
03022 {
03023 registeredDepth.at<float>(y,indexA+k) = valueA+slope*float(k);
03024 }
03025 }
03026 }
03027 valueA = v;
03028 indexA = x;
03029 }
03030 }
03031 }
03032 }
03033 }
03034
03035 bool isFinite(const cv::Point3f & pt)
03036 {
03037 return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z);
03038 }
03039
03040 pcl::PointCloud<pcl::PointXYZ>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds)
03041 {
03042 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
03043 for(std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
03044 {
03045 *cloud += *(*iter);
03046 }
03047 return cloud;
03048 }
03049
03050 pcl::PointCloud<pcl::PointXYZRGB>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds)
03051 {
03052 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
03053 for(std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
03054 {
03055 *cloud+=*(*iter);
03056 }
03057 return cloud;
03058 }
03059
03060 pcl::IndicesPtr concatenate(const std::vector<pcl::IndicesPtr> & indices)
03061 {
03062
03063 unsigned int totalSize = 0;
03064 for(unsigned int i=0; i<indices.size(); ++i)
03065 {
03066 totalSize += (unsigned int)indices[i]->size();
03067 }
03068 pcl::IndicesPtr ind(new std::vector<int>(totalSize));
03069 unsigned int io = 0;
03070 for(unsigned int i=0; i<indices.size(); ++i)
03071 {
03072 for(unsigned int j=0; j<indices[i]->size(); ++j)
03073 {
03074 ind->at(io++) = indices[i]->at(j);
03075 }
03076 }
03077 return ind;
03078 }
03079
03080 pcl::IndicesPtr concatenate(const pcl::IndicesPtr & indicesA, const pcl::IndicesPtr & indicesB)
03081 {
03082 pcl::IndicesPtr ind(new std::vector<int>(*indicesA));
03083 ind->resize(ind->size()+indicesB->size());
03084 unsigned int oi = (unsigned int)indicesA->size();
03085 for(unsigned int i=0; i<indicesB->size(); ++i)
03086 {
03087 ind->at(oi++) = indicesB->at(i);
03088 }
03089 return ind;
03090 }
03091
03092 void savePCDWords(
03093 const std::string & fileName,
03094 const std::multimap<int, pcl::PointXYZ> & words,
03095 const Transform & transform)
03096 {
03097 if(words.size())
03098 {
03099 pcl::PointCloud<pcl::PointXYZ> cloud;
03100 cloud.resize(words.size());
03101 int i=0;
03102 for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
03103 {
03104 cloud[i++] = transformPoint(iter->second, transform);
03105 }
03106 pcl::io::savePCDFile(fileName, cloud);
03107 }
03108 }
03109
03110 void savePCDWords(
03111 const std::string & fileName,
03112 const std::multimap<int, cv::Point3f> & words,
03113 const Transform & transform)
03114 {
03115 if(words.size())
03116 {
03117 pcl::PointCloud<pcl::PointXYZ> cloud;
03118 cloud.resize(words.size());
03119 int i=0;
03120 for(std::multimap<int, cv::Point3f>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
03121 {
03122 cv::Point3f pt = transformPoint(iter->second, transform);
03123 cloud[i++] = pcl::PointXYZ(pt.x, pt.y, pt.z);
03124 }
03125 pcl::io::savePCDFile(fileName, cloud);
03126 }
03127 }
03128
03129 cv::Mat loadBINScan(const std::string & fileName)
03130 {
03131 cv::Mat output;
03132 long bytes = UFile::length(fileName);
03133 if(bytes)
03134 {
03135 int dim = 4;
03136 UASSERT(bytes % sizeof(float) == 0);
03137 size_t num = bytes/sizeof(float);
03138 UASSERT(num % dim == 0);
03139 output = cv::Mat(1, num/dim, CV_32FC(dim));
03140
03141
03142 FILE *stream;
03143 stream = fopen (fileName.c_str(),"rb");
03144 size_t actualReadNum = fread(output.data,sizeof(float),num,stream);
03145 UASSERT(num == actualReadNum);
03146 fclose(stream);
03147 }
03148
03149 return output;
03150 }
03151
03152 pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName)
03153 {
03154 return laserScanToPointCloud(loadScan(fileName));
03155 }
03156
03157 pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, int dim)
03158 {
03159 return loadBINCloud(fileName);
03160 }
03161
03162 LaserScan loadScan(const std::string & path)
03163 {
03164 std::string fileName = UFile::getName(path);
03165 if(UFile::getExtension(fileName).compare("bin") == 0)
03166 {
03167 return LaserScan(loadBINScan(path), 0, 0, LaserScan::kXYZI);
03168 }
03169 else if(UFile::getExtension(fileName).compare("pcd") == 0)
03170 {
03171 pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
03172 pcl::io::loadPCDFile(path, *cloud);
03173 return laserScanFromPointCloud(*cloud);
03174 }
03175 else
03176 {
03177 pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
03178 pcl::io::loadPLYFile(path, *cloud);
03179 return laserScanFromPointCloud(*cloud);
03180 }
03181 return LaserScan();
03182 }
03183
03184 pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
03185 const std::string & path,
03186 const Transform & transform,
03187 int downsampleStep,
03188 float voxelSize)
03189 {
03190 UASSERT(!transform.isNull());
03191 UDEBUG("Loading cloud (step=%d, voxel=%f m) : %s", downsampleStep, voxelSize, path.c_str());
03192 std::string fileName = UFile::getName(path);
03193 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
03194 if(UFile::getExtension(fileName).compare("bin") == 0)
03195 {
03196 cloud = util3d::loadBINCloud(path);
03197 }
03198 else if(UFile::getExtension(fileName).compare("pcd") == 0)
03199 {
03200 pcl::io::loadPCDFile(path, *cloud);
03201 }
03202 else
03203 {
03204 pcl::io::loadPLYFile(path, *cloud);
03205 }
03206 int previousSize = (int)cloud->size();
03207 if(downsampleStep > 1 && cloud->size())
03208 {
03209 cloud = util3d::downsample(cloud, downsampleStep);
03210 UDEBUG("Downsampling scan (step=%d): %d -> %d", downsampleStep, previousSize, (int)cloud->size());
03211 }
03212 previousSize = (int)cloud->size();
03213 if(voxelSize > 0.0f && cloud->size())
03214 {
03215 cloud = util3d::voxelize(cloud, voxelSize);
03216 UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", voxelSize, previousSize, (int)cloud->size());
03217 }
03218 if(transform.isIdentity())
03219 {
03220 return cloud;
03221 }
03222 return util3d::transformPointCloud(cloud, transform);
03223 }
03224
03225 }
03226
03227 }