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/EpipolarGeometry.h>
00029 #include <rtabmap/utilite/UMath.h>
00030 #include <rtabmap/utilite/UStl.h>
00031 #include <rtabmap/utilite/ULogger.h>
00032 #include <pcl/registration/transformation_estimation_svd.h>
00033 #include <pcl/registration/transformation_estimation_2D.h>
00034 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
00035 #include <pcl/registration/icp_nl.h>
00036 #include <pcl/io/pcd_io.h>
00037 #include <pcl/common/distances.h>
00038 #include <pcl/surface/gp3.h>
00039 #include <pcl/features/normal_3d_omp.h>
00040 #include <pcl/surface/mls.h>
00041 #include <pcl/ModelCoefficients.h>
00042 #include <pcl/segmentation/sac_segmentation.h>
00043
00044 #include <opencv2/calib3d/calib3d.hpp>
00045 #include <opencv2/video/tracking.hpp>
00046 #include <rtabmap/core/VWDictionary.h>
00047 #include <cmath>
00048 #include <stdio.h>
00049
00050 #include "rtabmap/utilite/UConversion.h"
00051 #include "rtabmap/utilite/UTimer.h"
00052 #include "rtabmap/core/util3d.h"
00053 #include "rtabmap/core/Signature.h"
00054
00055 #include <pcl/filters/random_sample.h>
00056
00057 namespace rtabmap
00058 {
00059
00060 namespace util3d
00061 {
00062
00063 cv::Mat bgrFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder)
00064 {
00065 cv::Mat frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
00066
00067 for(unsigned int h = 0; h < cloud.height; h++)
00068 {
00069 for(unsigned int w = 0; w < cloud.width; w++)
00070 {
00071 if(bgrOrder)
00072 {
00073 frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).b;
00074 frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
00075 frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).r;
00076 }
00077 else
00078 {
00079 frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).r;
00080 frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
00081 frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).b;
00082 }
00083 }
00084 }
00085 return frameBGR;
00086 }
00087
00088
00089 cv::Mat depthFromCloud(
00090 const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
00091 float & fx,
00092 float & fy,
00093 bool depth16U)
00094 {
00095 cv::Mat frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
00096 fx = 0.0f;
00097 fy = 0.0f;
00098 for(unsigned int h = 0; h < cloud.height; h++)
00099 {
00100 for(unsigned int w = 0; w < cloud.width; w++)
00101 {
00102 float depth = cloud.at(h*cloud.width + w).z;
00103 if(depth16U)
00104 {
00105 depth *= 1000.0f;
00106 unsigned short depthMM = 0;
00107 if(depth <= (float)USHRT_MAX)
00108 {
00109 depthMM = (unsigned short)depth;
00110 }
00111 frameDepth.at<unsigned short>(h,w) = depthMM;
00112 }
00113 else
00114 {
00115 frameDepth.at<float>(h,w) = depth;
00116 }
00117
00118
00119 if(fx == 0.0f &&
00120 uIsFinite(cloud.at(h*cloud.width + w).x) &&
00121 uIsFinite(depth) &&
00122 w != cloud.width/2 &&
00123 depth > 0)
00124 {
00125 fx = cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth);
00126 if(depth16U)
00127 {
00128 fx*=1000.0f;
00129 }
00130 }
00131 if(fy == 0.0f &&
00132 uIsFinite(cloud.at(h*cloud.width + w).y) &&
00133 uIsFinite(depth) &&
00134 h != cloud.height/2 &&
00135 depth > 0)
00136 {
00137 fy = cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth);
00138 if(depth16U)
00139 {
00140 fy*=1000.0f;
00141 }
00142 }
00143 }
00144 }
00145 return frameDepth;
00146 }
00147
00148
00149 void rgbdFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
00150 cv::Mat & frameBGR,
00151 cv::Mat & frameDepth,
00152 float & fx,
00153 float & fy,
00154 bool bgrOrder,
00155 bool depth16U)
00156 {
00157 frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
00158 frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
00159
00160 fx = 0.0f;
00161 fy = 0.0f;
00162 for(unsigned int h = 0; h < cloud.height; h++)
00163 {
00164 for(unsigned int w = 0; w < cloud.width; w++)
00165 {
00166
00167 if(bgrOrder)
00168 {
00169 frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).b;
00170 frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
00171 frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).r;
00172 }
00173 else
00174 {
00175 frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).r;
00176 frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
00177 frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).b;
00178 }
00179
00180
00181 float depth = cloud.at(h*cloud.width + w).z;
00182 if(depth16U)
00183 {
00184 depth *= 1000.0f;
00185 unsigned short depthMM = 0;
00186 if(depth <= (float)USHRT_MAX)
00187 {
00188 depthMM = (unsigned short)depth;
00189 }
00190 frameDepth.at<unsigned short>(h,w) = depthMM;
00191 }
00192 else
00193 {
00194 frameDepth.at<float>(h,w) = depth;
00195 }
00196
00197
00198 if(fx == 0.0f &&
00199 uIsFinite(cloud.at(h*cloud.width + w).x) &&
00200 uIsFinite(depth) &&
00201 w != cloud.width/2 &&
00202 depth > 0)
00203 {
00204 fx = 1.0f/(cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth));
00205 if(depth16U)
00206 {
00207 fx/=1000.0f;
00208 }
00209 }
00210 if(fy == 0.0f &&
00211 uIsFinite(cloud.at(h*cloud.width + w).y) &&
00212 uIsFinite(depth) &&
00213 h != cloud.height/2 &&
00214 depth > 0)
00215 {
00216 fy = 1.0f/(cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth));
00217 if(depth16U)
00218 {
00219 fy/=1000.0f;
00220 }
00221 }
00222 }
00223 }
00224 }
00225
00226 cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
00227 {
00228 UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
00229 cv::Mat depth16U;
00230 if(!depth32F.empty())
00231 {
00232 depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
00233 for(int i=0; i<depth32F.rows; ++i)
00234 {
00235 for(int j=0; j<depth32F.cols; ++j)
00236 {
00237 float depth = (depth32F.at<float>(i,j)*1000.0f);
00238 unsigned short depthMM = 0;
00239 if(depth <= (float)USHRT_MAX)
00240 {
00241 depthMM = (unsigned short)depth;
00242 }
00243 depth16U.at<unsigned short>(i, j) = depthMM;
00244 }
00245 }
00246 }
00247 return depth16U;
00248 }
00249
00250 cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
00251 {
00252 UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
00253 cv::Mat depth32F;
00254 if(!depth16U.empty())
00255 {
00256 depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
00257 for(int i=0; i<depth16U.rows; ++i)
00258 {
00259 for(int j=0; j<depth16U.cols; ++j)
00260 {
00261 float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
00262 depth32F.at<float>(i, j) = depth;
00263 }
00264 }
00265 }
00266 return depth32F;
00267 }
00268
00269 pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
00270 const std::vector<cv::KeyPoint> & keypoints,
00271 const cv::Mat & depth,
00272 float fx,
00273 float fy,
00274 float cx,
00275 float cy,
00276 const Transform & transform)
00277 {
00278 UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
00279 pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
00280 if(!depth.empty())
00281 {
00282 keypoints3d->resize(keypoints.size());
00283 for(unsigned int i=0; i!=keypoints.size(); ++i)
00284 {
00285 pcl::PointXYZ pt = util3d::projectDepthTo3D(
00286 depth,
00287 keypoints[i].pt.x,
00288 keypoints[i].pt.y,
00289 cx,
00290 cy,
00291 fx,
00292 fy,
00293 true);
00294
00295 if(!transform.isNull() && !transform.isIdentity())
00296 {
00297 pt = pcl::transformPoint(pt, transform.toEigen3f());
00298 }
00299 keypoints3d->at(i) = pt;
00300 }
00301 }
00302 return keypoints3d;
00303 }
00304
00305 pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
00306 const std::vector<cv::KeyPoint> & keypoints,
00307 const cv::Mat & disparity,
00308 float fx,
00309 float baseline,
00310 float cx,
00311 float cy,
00312 const Transform & transform)
00313 {
00314 UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
00315 pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
00316 keypoints3d->resize(keypoints.size());
00317 for(unsigned int i=0; i!=keypoints.size(); ++i)
00318 {
00319 pcl::PointXYZ pt = util3d::projectDisparityTo3D(
00320 keypoints[i].pt,
00321 disparity,
00322 cx,
00323 cy,
00324 fx,
00325 baseline);
00326
00327 if(pcl::isFinite(pt) && !transform.isNull() && !transform.isIdentity())
00328 {
00329 pt = pcl::transformPoint(pt, transform.toEigen3f());
00330 }
00331 keypoints3d->at(i) = pt;
00332 }
00333 return keypoints3d;
00334 }
00335
00336 pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
00337 const std::vector<cv::KeyPoint> & keypoints,
00338 const cv::Mat & leftImage,
00339 const cv::Mat & rightImage,
00340 float fx,
00341 float baseline,
00342 float cx,
00343 float cy,
00344 const Transform & transform,
00345 int flowWinSize,
00346 int flowMaxLevel,
00347 int flowIterations,
00348 double flowEps)
00349 {
00350 UASSERT(!leftImage.empty() && !rightImage.empty() &&
00351 leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
00352 leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols);
00353
00354 std::vector<cv::Point2f> leftCorners;
00355 cv::KeyPoint::convert(keypoints, leftCorners);
00356
00357
00358 std::vector<unsigned char> status;
00359 std::vector<float> err;
00360 std::vector<cv::Point2f> rightCorners;
00361 UDEBUG("cv::calcOpticalFlowPyrLK() begin");
00362 cv::calcOpticalFlowPyrLK(
00363 leftImage,
00364 rightImage,
00365 leftCorners,
00366 rightCorners,
00367 status,
00368 err,
00369 cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
00370 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
00371 cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
00372 UDEBUG("cv::calcOpticalFlowPyrLK() end");
00373
00374 pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
00375 keypoints3d->resize(keypoints.size());
00376 float bad_point = std::numeric_limits<float>::quiet_NaN ();
00377 UASSERT(status.size() == keypoints.size());
00378 for(unsigned int i=0; i<status.size(); ++i)
00379 {
00380 pcl::PointXYZ pt(bad_point, bad_point, bad_point);
00381 if(status[i])
00382 {
00383 float disparity = leftCorners[i].x - rightCorners[i].x;
00384 if(disparity > 0.0f)
00385 {
00386 pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
00387 leftCorners[i],
00388 disparity,
00389 cx, cy, fx, baseline);
00390
00391 if(pcl::isFinite(tmpPt))
00392 {
00393 pt = tmpPt;
00394 if(!transform.isNull() && !transform.isIdentity())
00395 {
00396 pt = pcl::transformPoint(pt, transform.toEigen3f());
00397 }
00398 }
00399 }
00400 }
00401
00402 keypoints3d->at(i) = pt;
00403 }
00404 return keypoints3d;
00405 }
00406
00407
00408
00409
00410
00411 std::multimap<int, pcl::PointXYZ> generateWords3DMono(
00412 const std::multimap<int, cv::KeyPoint> & refWords,
00413 const std::multimap<int, cv::KeyPoint> & nextWords,
00414 float fx,
00415 float fy,
00416 float cx,
00417 float cy,
00418 const Transform & localTransform,
00419 Transform & cameraTransform,
00420 int pnpIterations,
00421 float pnpReprojError,
00422 int pnpFlags,
00423 float ransacParam1,
00424 float ransacParam2,
00425 const std::multimap<int, pcl::PointXYZ> & refGuess3D,
00426 double * varianceOut)
00427 {
00428 std::multimap<int, pcl::PointXYZ> words3D;
00429 std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
00430 if(EpipolarGeometry::findPairsUnique(refWords, nextWords, pairs) > 8)
00431 {
00432 std::vector<unsigned char> status;
00433 cv::Mat F = EpipolarGeometry::findFFromWords(pairs, status, ransacParam1, ransacParam2);
00434 if(!F.empty())
00435 {
00436
00437
00438 int oi = 0;
00439 UASSERT(status.size() == pairs.size());
00440 std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin();
00441 std::vector<cv::Point2f> refCorners(status.size());
00442 std::vector<cv::Point2f> newCorners(status.size());
00443 std::vector<int> indexes(status.size());
00444 for(unsigned int i=0; i<status.size(); ++i)
00445 {
00446 if(status[i])
00447 {
00448 refCorners[oi] = iter->second.first.pt;
00449 newCorners[oi] = iter->second.second.pt;
00450 indexes[oi] = iter->first;
00451 ++oi;
00452 }
00453 ++iter;
00454 }
00455 refCorners.resize(oi);
00456 newCorners.resize(oi);
00457 indexes.resize(oi);
00458
00459 UDEBUG("inliers=%d/%d", oi, pairs.size());
00460 if(oi > 3)
00461 {
00462 std::vector<cv::Point2f> refCornersRefined;
00463 std::vector<cv::Point2f> newCornersRefined;
00464 cv::correctMatches(F, refCorners, newCorners, refCornersRefined, newCornersRefined);
00465 refCorners = refCornersRefined;
00466 newCorners = newCornersRefined;
00467
00468 cv::Mat x(3, (int)refCorners.size(), CV_64FC1);
00469 cv::Mat xp(3, (int)refCorners.size(), CV_64FC1);
00470 for(unsigned int i=0; i<refCorners.size(); ++i)
00471 {
00472 x.at<double>(0, i) = refCorners[i].x;
00473 x.at<double>(1, i) = refCorners[i].y;
00474 x.at<double>(2, i) = 1;
00475
00476 xp.at<double>(0, i) = newCorners[i].x;
00477 xp.at<double>(1, i) = newCorners[i].y;
00478 xp.at<double>(2, i) = 1;
00479 }
00480
00481 cv::Mat K = (cv::Mat_<double>(3,3) <<
00482 fx, 0, cx,
00483 0, fy, cy,
00484 0, 0, 1);
00485 cv::Mat Kinv = K.inv();
00486 cv::Mat E = K.t()*F*K;
00487 cv::Mat x_norm = Kinv * x;
00488 cv::Mat xp_norm = Kinv * xp;
00489 x_norm = x_norm.rowRange(0,2);
00490 xp_norm = xp_norm.rowRange(0,2);
00491
00492 cv::Mat P = EpipolarGeometry::findPFromE(E, x_norm, xp_norm);
00493 if(!P.empty())
00494 {
00495 cv::Mat P0 = cv::Mat::zeros(3, 4, CV_64FC1);
00496 P0.at<double>(0,0) = 1;
00497 P0.at<double>(1,1) = 1;
00498 P0.at<double>(2,2) = 1;
00499
00500 bool useCameraTransformGuess = !cameraTransform.isNull();
00501
00502 if(useCameraTransformGuess)
00503 {
00504 Transform t = (localTransform.inverse()*cameraTransform*localTransform).inverse();
00505 P = (cv::Mat_<double>(3,4) <<
00506 (double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.x(),
00507 (double)t.r21(), (double)t.r22(), (double)t.r23(), (double)t.y(),
00508 (double)t.r31(), (double)t.r32(), (double)t.r33(), (double)t.z());
00509 }
00510
00511
00512
00513
00514
00515 cv::Mat pts4D;
00516 cv::triangulatePoints(P0, P, x_norm, xp_norm, pts4D);
00517
00518 for(unsigned int i=0; i<indexes.size(); ++i)
00519 {
00520
00521
00522
00523
00524 pts4D.col(i) /= pts4D.at<double>(3,i);
00525 if(pts4D.at<double>(2,i) > 0)
00526 {
00527 words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), localTransform)));
00528 }
00529 }
00530
00531 if(!useCameraTransformGuess)
00532 {
00533 cv::Mat R, T;
00534 EpipolarGeometry::findRTFromP(P, R, T);
00535
00536 Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0),
00537 R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
00538 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
00539
00540 cameraTransform = (localTransform * t).inverse() * localTransform;
00541 }
00542
00543 if(refGuess3D.size())
00544 {
00545
00546 pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRef(new pcl::PointCloud<pcl::PointXYZ>);
00547 pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRefGuess(new pcl::PointCloud<pcl::PointXYZ>);
00548 util3d::findCorrespondences(
00549 words3D,
00550 refGuess3D,
00551 *inliersRef,
00552 *inliersRefGuess,
00553 0);
00554
00555 if(inliersRef->size())
00556 {
00557
00558 float scale = 1.0f;
00559 float variance = 1.0f;
00560 if(!useCameraTransformGuess)
00561 {
00562 std::multimap<float, float> scales;
00563 for(unsigned int i=0; i<inliersRef->size(); ++i)
00564 {
00565
00566 float s = inliersRefGuess->at(i).x/inliersRef->at(i).x;
00567 std::vector<float> errorSqrdDists(inliersRef->size());
00568 for(unsigned int j=0; j<inliersRef->size(); ++j)
00569 {
00570 pcl::PointXYZ refPt = inliersRef->at(j);
00571 refPt.x *= s;
00572 refPt.y *= s;
00573 refPt.z *= s;
00574 const pcl::PointXYZ & newPt = inliersRefGuess->at(j);
00575 errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
00576 }
00577 std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
00578 double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
00579 float var = 2.1981 * median_error_sqr;
00580
00581
00582 scales.insert(std::make_pair(var, s));
00583 }
00584 scale = scales.begin()->second;
00585 variance = scales.begin()->first;;
00586 }
00587 else
00588 {
00589
00590 std::vector<float> errorSqrdDists(inliersRef->size());
00591 for(unsigned int j=0; j<inliersRef->size(); ++j)
00592 {
00593 const pcl::PointXYZ & refPt = inliersRef->at(j);
00594 const pcl::PointXYZ & newPt = inliersRefGuess->at(j);
00595 errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
00596 }
00597 std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
00598 double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
00599 variance = 2.1981 * median_error_sqr;
00600 }
00601
00602 UDEBUG("scale used = %f (variance=%f)", scale, variance);
00603 if(varianceOut)
00604 {
00605 *varianceOut = variance;
00606 }
00607
00608 if(!useCameraTransformGuess)
00609 {
00610 std::vector<cv::Point3f> objectPoints(indexes.size());
00611 std::vector<cv::Point2f> imagePoints(indexes.size());
00612 int oi=0;
00613 for(unsigned int i=0; i<indexes.size(); ++i)
00614 {
00615 std::multimap<int, pcl::PointXYZ>::iterator iter = words3D.find(indexes[i]);
00616 if(pcl::isFinite(iter->second))
00617 {
00618 iter->second.x *= scale;
00619 iter->second.y *= scale;
00620 iter->second.z *= scale;
00621 objectPoints[oi].x = iter->second.x;
00622 objectPoints[oi].y = iter->second.y;
00623 objectPoints[oi].z = iter->second.z;
00624 imagePoints[oi] = newCorners[i];
00625 ++oi;
00626 }
00627 }
00628 objectPoints.resize(oi);
00629 imagePoints.resize(oi);
00630
00631
00632 Transform guess = localTransform.inverse();
00633 cv::Mat R = (cv::Mat_<double>(3,3) <<
00634 (double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
00635 (double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
00636 (double)guess.r31(), (double)guess.r32(), (double)guess.r33());
00637 cv::Mat rvec(1,3, CV_64FC1);
00638 cv::Rodrigues(R, rvec);
00639 cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
00640 std::vector<int> inliersV;
00641 cv::solvePnPRansac(
00642 objectPoints,
00643 imagePoints,
00644 K,
00645 cv::Mat(),
00646 rvec,
00647 tvec,
00648 true,
00649 pnpIterations,
00650 pnpReprojError,
00651 0,
00652 inliersV,
00653 pnpFlags);
00654
00655 UDEBUG("PnP inliers = %d / %d", (int)inliersV.size(), (int)objectPoints.size());
00656
00657 if(inliersV.size())
00658 {
00659 cv::Rodrigues(rvec, R);
00660 Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
00661 R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
00662 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
00663
00664 cameraTransform = (localTransform * pnp).inverse();
00665 }
00666 else
00667 {
00668 UWARN("No inliers after PnP!");
00669 cameraTransform = Transform();
00670 }
00671 }
00672 }
00673 else
00674 {
00675 UWARN("Cannot compute the scale, no points corresponding between the generated ref words and words guess");
00676 }
00677 }
00678 }
00679 }
00680 }
00681 }
00682 UDEBUG("wordsSet=%d / %d", (int)words3D.size(), (int)refWords.size());
00683
00684 return words3D;
00685 }
00686
00687 std::multimap<int, cv::KeyPoint> aggregate(
00688 const std::list<int> & wordIds,
00689 const std::vector<cv::KeyPoint> & keypoints)
00690 {
00691 std::multimap<int, cv::KeyPoint> words;
00692 std::vector<cv::KeyPoint>::const_iterator kpIter = keypoints.begin();
00693 for(std::list<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
00694 {
00695 words.insert(std::pair<int, cv::KeyPoint >(*iter, *kpIter));
00696 ++kpIter;
00697 }
00698 return words;
00699 }
00700
00701 std::list<std::pair<cv::Point2f, cv::Point2f> > findCorrespondences(
00702 const std::multimap<int, cv::KeyPoint> & words1,
00703 const std::multimap<int, cv::KeyPoint> & words2)
00704 {
00705 std::list<std::pair<cv::Point2f, cv::Point2f> > correspondences;
00706
00707
00708 std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
00709 rtabmap::EpipolarGeometry::findPairsUnique(words1, words2, pairs);
00710
00711 if(pairs.size() > 7)
00712 {
00713
00714 std::vector<uchar> status;
00715 cv::Mat fundamentalMatrix = rtabmap::EpipolarGeometry::findFFromWords(pairs, status);
00716
00717 if(!fundamentalMatrix.empty())
00718 {
00719 int i = 0;
00720
00721 for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
00722 {
00723 if(status[i])
00724 {
00725 correspondences.push_back(std::pair<cv::Point2f, cv::Point2f>(iter->second.first.pt, iter->second.second.pt));
00726
00727 }
00728 ++i;
00729 }
00730 }
00731 }
00732 return correspondences;
00733 }
00734
00735 void findCorrespondences(
00736 const std::multimap<int, pcl::PointXYZ> & words1,
00737 const std::multimap<int, pcl::PointXYZ> & words2,
00738 pcl::PointCloud<pcl::PointXYZ> & inliers1,
00739 pcl::PointCloud<pcl::PointXYZ> & inliers2,
00740 float maxDepth,
00741 std::set<int> * uniqueCorrespondences)
00742 {
00743 std::list<int> ids = uUniqueKeys(words1);
00744
00745 inliers1.resize(ids.size());
00746 inliers2.resize(ids.size());
00747
00748 int oi=0;
00749 for(std::list<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
00750 {
00751 if(words1.count(*iter) == 1 && words2.count(*iter) == 1)
00752 {
00753 inliers1[oi] = words1.find(*iter)->second;
00754 inliers2[oi] = words2.find(*iter)->second;
00755 if(pcl::isFinite(inliers1[oi]) &&
00756 pcl::isFinite(inliers2[oi]) &&
00757 (inliers1[oi].x != 0 || inliers1[oi].y != 0 || inliers1[oi].z != 0) &&
00758 (inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
00759 (maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
00760 {
00761 ++oi;
00762 if(uniqueCorrespondences)
00763 {
00764 uniqueCorrespondences->insert(*iter);
00765 }
00766 }
00767 }
00768 }
00769 inliers1.resize(oi);
00770 inliers2.resize(oi);
00771 }
00772
00773 float getDepth(
00774 const cv::Mat & depthImage,
00775 float x, float y,
00776 bool smoothing,
00777 float maxZError)
00778 {
00779 UASSERT(!depthImage.empty());
00780 UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
00781
00782 int u = int(x+0.5f);
00783 int v = int(y+0.5f);
00784
00785 if(!(u >=0 && u<depthImage.cols && v >=0 && v<depthImage.rows))
00786 {
00787 UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f (u=%d), y=%f (v=%d), cols=%d, rows=%d)",
00788 x,u,y,v,depthImage.cols, depthImage.rows);
00789 return 0;
00790 }
00791
00792 bool isInMM = depthImage.type() == CV_16UC1;
00793
00794
00795
00796
00797
00798
00799
00800 int u_start = std::max(u-1, 0);
00801 int v_start = std::max(v-1, 0);
00802 int u_end = std::min(u+1, depthImage.cols-1);
00803 int v_end = std::min(v+1, depthImage.rows-1);
00804
00805 float depth = isInMM?(float)depthImage.at<uint16_t>(v,u)*0.001f:depthImage.at<float>(v,u);
00806 if(depth!=0.0f && uIsFinite(depth))
00807 {
00808 if(smoothing)
00809 {
00810 float sumWeights = 0.0f;
00811 float sumDepths = 0.0f;
00812 for(int uu = u_start; uu <= u_end; ++uu)
00813 {
00814 for(int vv = v_start; vv <= v_end; ++vv)
00815 {
00816 if(!(uu == u && vv == v))
00817 {
00818 float d = isInMM?(float)depthImage.at<uint16_t>(vv,uu)*0.001f:depthImage.at<float>(vv,uu);
00819
00820 if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < maxZError)
00821 {
00822 if(uu == u || vv == v)
00823 {
00824 sumWeights+=2.0f;
00825 d*=2.0f;
00826 }
00827 else
00828 {
00829 sumWeights+=1.0f;
00830 }
00831 sumDepths += d;
00832 }
00833 }
00834 }
00835 }
00836
00837 depth *= 4.0f;
00838 sumWeights += 4.0f;
00839
00840
00841 depth = (depth+sumDepths)/sumWeights;
00842 }
00843 }
00844 else
00845 {
00846 depth = 0;
00847 }
00848 return depth;
00849 }
00850
00851 pcl::PointXYZ projectDepthTo3D(
00852 const cv::Mat & depthImage,
00853 float x, float y,
00854 float cx, float cy,
00855 float fx, float fy,
00856 bool smoothing,
00857 float maxZError)
00858 {
00859 UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
00860
00861 pcl::PointXYZ pt;
00862 float bad_point = std::numeric_limits<float>::quiet_NaN ();
00863
00864 float depth = getDepth(depthImage, x, y, smoothing, maxZError);
00865 if(depth)
00866 {
00867
00868 cx = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f;
00869 cy = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f;
00870
00871
00872 pt.x = (x - cx) * depth / fx;
00873 pt.y = (y - cy) * depth / fy;
00874 pt.z = depth;
00875 }
00876 else
00877 {
00878 pt.x = pt.y = pt.z = bad_point;
00879 }
00880 return pt;
00881 }
00882
00883 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
00884 const cv::Mat & imageDepth,
00885 float cx, float cy,
00886 float fx, float fy,
00887 int decimation)
00888 {
00889 UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
00890 UASSERT(imageDepth.rows % decimation == 0);
00891 UASSERT(imageDepth.cols % decimation == 0);
00892
00893 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00894 if(decimation < 1)
00895 {
00896 return cloud;
00897 }
00898
00899
00900 cloud->height = imageDepth.rows/decimation;
00901 cloud->width = imageDepth.cols/decimation;
00902 cloud->is_dense = false;
00903
00904 cloud->resize(cloud->height * cloud->width);
00905
00906 int count = 0 ;
00907
00908 for(int h = 0; h < imageDepth.rows; h+=decimation)
00909 {
00910 for(int w = 0; w < imageDepth.cols; w+=decimation)
00911 {
00912 pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
00913
00914 pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
00915 pt.x = ptXYZ.x;
00916 pt.y = ptXYZ.y;
00917 pt.z = ptXYZ.z;
00918 ++count;
00919 }
00920 }
00921
00922 return cloud;
00923 }
00924
00925 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
00926 const cv::Mat & imageRgb,
00927 const cv::Mat & imageDepth,
00928 float cx, float cy,
00929 float fx, float fy,
00930 int decimation)
00931 {
00932 UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
00933 UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
00934 UASSERT(imageDepth.rows % decimation == 0);
00935 UASSERT(imageDepth.cols % decimation == 0);
00936
00937 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00938 if(decimation < 1)
00939 {
00940 return cloud;
00941 }
00942
00943 bool mono;
00944 if(imageRgb.channels() == 3)
00945 {
00946 mono = false;
00947 }
00948 else if(imageRgb.channels() == 1)
00949 {
00950 mono = true;
00951 }
00952 else
00953 {
00954 return cloud;
00955 }
00956
00957
00958 cloud->height = imageDepth.rows/decimation;
00959 cloud->width = imageDepth.cols/decimation;
00960 cloud->is_dense = false;
00961 cloud->resize(cloud->height * cloud->width);
00962
00963 for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
00964 {
00965 for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
00966 {
00967 pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
00968 if(!mono)
00969 {
00970 pt.b = imageRgb.at<cv::Vec3b>(h,w)[0];
00971 pt.g = imageRgb.at<cv::Vec3b>(h,w)[1];
00972 pt.r = imageRgb.at<cv::Vec3b>(h,w)[2];
00973 }
00974 else
00975 {
00976 unsigned char v = imageRgb.at<unsigned char>(h,w);
00977 pt.b = v;
00978 pt.g = v;
00979 pt.r = v;
00980 }
00981
00982 pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
00983 pt.x = ptXYZ.x;
00984 pt.y = ptXYZ.y;
00985 pt.z = ptXYZ.z;
00986 }
00987 }
00988 return cloud;
00989 }
00990
00991 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
00992 const cv::Mat & imageDisparity,
00993 float cx, float cy,
00994 float fx, float baseline,
00995 int decimation)
00996 {
00997 UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
00998 UASSERT(imageDisparity.rows % decimation == 0);
00999 UASSERT(imageDisparity.cols % decimation == 0);
01000
01001 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
01002 if(decimation < 1)
01003 {
01004 return cloud;
01005 }
01006
01007
01008 cloud->height = imageDisparity.rows/decimation;
01009 cloud->width = imageDisparity.cols/decimation;
01010 cloud->is_dense = false;
01011 cloud->resize(cloud->height * cloud->width);
01012
01013 if(imageDisparity.type()==CV_16SC1)
01014 {
01015 for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
01016 {
01017 for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
01018 {
01019 float disp = float(imageDisparity.at<short>(h,w))/16.0f;
01020 cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
01021 }
01022 }
01023 }
01024 else
01025 {
01026 for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
01027 {
01028 for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
01029 {
01030 float disp = imageDisparity.at<float>(h,w);
01031 cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
01032 }
01033 }
01034 }
01035 return cloud;
01036 }
01037
01038 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
01039 const cv::Mat & imageRgb,
01040 const cv::Mat & imageDisparity,
01041 float cx, float cy,
01042 float fx, float baseline,
01043 int decimation)
01044 {
01045 UASSERT(imageRgb.rows == imageDisparity.rows &&
01046 imageRgb.cols == imageDisparity.cols &&
01047 (imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1));
01048 UASSERT(imageDisparity.rows % decimation == 0);
01049 UASSERT(imageDisparity.cols % decimation == 0);
01050 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
01051 if(decimation < 1)
01052 {
01053 return cloud;
01054 }
01055
01056 bool mono;
01057 if(imageRgb.channels() == 3)
01058 {
01059 mono = false;
01060 }
01061 else if(imageRgb.channels() == 1)
01062 {
01063 mono = true;
01064 }
01065 else
01066 {
01067 return cloud;
01068 }
01069
01070
01071 cloud->height = imageRgb.rows/decimation;
01072 cloud->width = imageRgb.cols/decimation;
01073 cloud->is_dense = false;
01074 cloud->resize(cloud->height * cloud->width);
01075
01076 for(int h = 0; h < imageRgb.rows && h/decimation < (int)cloud->height; h+=decimation)
01077 {
01078 for(int w = 0; w < imageRgb.cols && w/decimation < (int)cloud->width; w+=decimation)
01079 {
01080 pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
01081 if(!mono)
01082 {
01083 pt.b = imageRgb.at<cv::Vec3b>(h,w)[0];
01084 pt.g = imageRgb.at<cv::Vec3b>(h,w)[1];
01085 pt.r = imageRgb.at<cv::Vec3b>(h,w)[2];
01086 }
01087 else
01088 {
01089 unsigned char v = imageRgb.at<unsigned char>(h,w);
01090 pt.b = v;
01091 pt.g = v;
01092 pt.r = v;
01093 }
01094
01095 float disp = imageDisparity.type()==CV_16SC1?float(imageDisparity.at<short>(h,w))/16.0f:imageDisparity.at<float>(h,w);
01096 pcl::PointXYZ ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
01097 pt.x = ptXYZ.x;
01098 pt.y = ptXYZ.y;
01099 pt.z = ptXYZ.z;
01100 }
01101 }
01102 return cloud;
01103 }
01104
01105 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
01106 const cv::Mat & imageLeft,
01107 const cv::Mat & imageRight,
01108 float cx, float cy,
01109 float fx, float baseline,
01110 int decimation)
01111 {
01112 UASSERT(imageRight.type() == CV_8UC1);
01113
01114 cv::Mat leftMono;
01115 if(imageLeft.channels() == 3)
01116 {
01117 cv::cvtColor(imageLeft, leftMono, CV_BGR2GRAY);
01118 }
01119 else
01120 {
01121 leftMono = imageLeft;
01122 }
01123 return rtabmap::util3d::cloudFromDisparityRGB(
01124 imageLeft,
01125 util3d::disparityFromStereoImages(leftMono, imageRight),
01126 cx, cy,
01127 fx, baseline,
01128 decimation);
01129 }
01130
01131 cv::Mat disparityFromStereoImages(
01132 const cv::Mat & leftImage,
01133 const cv::Mat & rightImage)
01134 {
01135 UASSERT(!leftImage.empty() && !rightImage.empty() &&
01136 (leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1 &&
01137 leftImage.cols == rightImage.cols &&
01138 leftImage.rows == rightImage.rows);
01139
01140 cv::Mat leftMono;
01141 if(leftImage.channels() == 3)
01142 {
01143 cv::cvtColor(leftImage, leftMono, CV_BGR2GRAY);
01144 }
01145 else
01146 {
01147 leftMono = leftImage;
01148 }
01149
01150 cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
01151 stereo.state->SADWindowSize = 15;
01152 stereo.state->minDisparity = 0;
01153 stereo.state->numberOfDisparities = 64;
01154 stereo.state->preFilterSize = 9;
01155 stereo.state->preFilterCap = 31;
01156 stereo.state->uniquenessRatio = 15;
01157 stereo.state->textureThreshold = 10;
01158 stereo.state->speckleWindowSize = 100;
01159 stereo.state->speckleRange = 4;
01160 cv::Mat disparity;
01161 stereo(leftMono, rightImage, disparity, CV_16SC1);
01162 return disparity;
01163 }
01164
01165 cv::Mat disparityFromStereoImages(
01166 const cv::Mat & leftImage,
01167 const cv::Mat & rightImage,
01168 const std::vector<cv::Point2f> & leftCorners,
01169 int flowWinSize,
01170 int flowMaxLevel,
01171 int flowIterations,
01172 double flowEps,
01173 float maxCorrespondencesSlope)
01174 {
01175 UASSERT(!leftImage.empty() && !rightImage.empty() &&
01176 leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
01177 leftImage.cols == rightImage.cols &&
01178 leftImage.rows == rightImage.rows);
01179
01180
01181 std::vector<unsigned char> status;
01182 std::vector<float> err;
01183 std::vector<cv::Point2f> rightCorners;
01184 UDEBUG("cv::calcOpticalFlowPyrLK() begin");
01185 cv::calcOpticalFlowPyrLK(
01186 leftImage,
01187 rightImage,
01188 leftCorners,
01189 rightCorners,
01190 status,
01191 err,
01192 cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
01193 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
01194 cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
01195 UDEBUG("cv::calcOpticalFlowPyrLK() end");
01196
01197 return disparityFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, maxCorrespondencesSlope);
01198 }
01199
01200 cv::Mat depthFromStereoImages(
01201 const cv::Mat & leftImage,
01202 const cv::Mat & rightImage,
01203 const std::vector<cv::Point2f> & leftCorners,
01204 float fx,
01205 float baseline,
01206 int flowWinSize,
01207 int flowMaxLevel,
01208 int flowIterations,
01209 double flowEps)
01210 {
01211 UASSERT(!leftImage.empty() && !rightImage.empty() &&
01212 leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
01213 leftImage.cols == rightImage.cols &&
01214 leftImage.rows == rightImage.rows);
01215 UASSERT(fx > 0.0f && baseline > 0.0f);
01216
01217
01218 std::vector<unsigned char> status;
01219 std::vector<float> err;
01220 std::vector<cv::Point2f> rightCorners;
01221 UDEBUG("cv::calcOpticalFlowPyrLK() begin");
01222 cv::calcOpticalFlowPyrLK(
01223 leftImage,
01224 rightImage,
01225 leftCorners,
01226 rightCorners,
01227 status,
01228 err,
01229 cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
01230 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
01231 cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
01232 UDEBUG("cv::calcOpticalFlowPyrLK() end");
01233
01234 return depthFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, fx, baseline);
01235 }
01236
01237 cv::Mat disparityFromStereoCorrespondences(
01238 const cv::Mat & leftImage,
01239 const std::vector<cv::Point2f> & leftCorners,
01240 const std::vector<cv::Point2f> & rightCorners,
01241 const std::vector<unsigned char> & mask,
01242 float maxSlope)
01243 {
01244 UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
01245 UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
01246 cv::Mat disparity = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
01247 for(unsigned int i=0; i<leftCorners.size(); ++i)
01248 {
01249 if(mask.size() == 0 || mask[i])
01250 {
01251 float d = leftCorners[i].x - rightCorners[i].x;
01252 float slope = fabs((leftCorners[i].y - rightCorners[i].y) / (leftCorners[i].x - rightCorners[i].x));
01253 if(d > 0.0f && slope < maxSlope)
01254 {
01255 disparity.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
01256 }
01257 }
01258 }
01259 return disparity;
01260 }
01261
01262 cv::Mat depthFromStereoCorrespondences(
01263 const cv::Mat & leftImage,
01264 const std::vector<cv::Point2f> & leftCorners,
01265 const std::vector<cv::Point2f> & rightCorners,
01266 const std::vector<unsigned char> & mask,
01267 float fx, float baseline)
01268 {
01269 UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
01270 UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
01271 cv::Mat depth = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
01272 for(unsigned int i=0; i<leftCorners.size(); ++i)
01273 {
01274 if(mask.size() == 0 || mask[i])
01275 {
01276 float disparity = leftCorners[i].x - rightCorners[i].x;
01277 if(disparity > 0.0f)
01278 {
01279 float d = baseline * fx / disparity;
01280 depth.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
01281 }
01282 }
01283 }
01284 return depth;
01285 }
01286
01287
01288 pcl::PointXYZ projectDisparityTo3D(
01289 const cv::Point2f & pt,
01290 float disparity,
01291 float cx, float cy, float fx, float baseline)
01292 {
01293 if(disparity > 0.0f && baseline > 0.0f && fx > 0.0f)
01294 {
01295 float W = disparity/baseline;
01296 return pcl::PointXYZ((pt.x - cx)/W, (pt.y - cy)/W, fx/W);
01297 }
01298 float bad_point = std::numeric_limits<float>::quiet_NaN ();
01299 return pcl::PointXYZ(bad_point, bad_point, bad_point);
01300 }
01301
01302 pcl::PointXYZ projectDisparityTo3D(
01303 const cv::Point2f & pt,
01304 const cv::Mat & disparity,
01305 float cx, float cy, float fx, float baseline)
01306 {
01307 UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
01308 int u = int(pt.x+0.5f);
01309 int v = int(pt.y+0.5f);
01310 float bad_point = std::numeric_limits<float>::quiet_NaN ();
01311 if(uIsInBounds(u, 0, disparity.cols) &&
01312 uIsInBounds(v, 0, disparity.rows))
01313 {
01314 float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
01315 return projectDisparityTo3D(pt, d, cx, cy, fx, baseline);
01316 }
01317 return pcl::PointXYZ(bad_point, bad_point, bad_point);
01318 }
01319
01320 cv::Mat depthFromDisparity(const cv::Mat & disparity,
01321 float fx, float baseline,
01322 int type)
01323 {
01324 UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
01325 UASSERT(type == CV_32FC1 || type == CV_16U);
01326 cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
01327 for (int i = 0; i < disparity.rows; i++)
01328 {
01329 for (int j = 0; j < disparity.cols; j++)
01330 {
01331 float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
01332 if (disparity_value > 0.0f)
01333 {
01334
01335 float d = baseline * fx / disparity_value;
01336 if(depth.type() == CV_32FC1)
01337 {
01338 depth.at<float>(i,j) = d;
01339 }
01340 else
01341 {
01342 depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
01343 }
01344 }
01345 }
01346 }
01347 return depth;
01348 }
01349
01350 cv::Mat registerDepth(
01351 const cv::Mat & depth,
01352 const cv::Mat & depthK,
01353 const cv::Mat & colorK,
01354 const rtabmap::Transform & transform)
01355 {
01356 UASSERT(!transform.isNull());
01357 UASSERT(!depth.empty());
01358 UASSERT(depth.type() == CV_16UC1);
01359 UASSERT(depthK.type() == CV_64FC1 && depthK.cols == 3 && depthK.cols == 3);
01360 UASSERT(colorK.type() == CV_64FC1 && colorK.cols == 3 && colorK.cols == 3);
01361
01362 float fx = depthK.at<double>(0,0);
01363 float fy = depthK.at<double>(1,1);
01364 float cx = depthK.at<double>(0,2);
01365 float cy = depthK.at<double>(1,2);
01366
01367 float rfx = colorK.at<double>(0,0);
01368 float rfy = colorK.at<double>(1,1);
01369 float rcx = colorK.at<double>(0,2);
01370 float rcy = colorK.at<double>(1,2);
01371
01372 Eigen::Affine3f proj = transform.toEigen3f();
01373 Eigen::Vector4f P4,P3;
01374 P4[3] = 1;
01375 cv::Mat registered = cv::Mat::zeros(depth.rows, depth.cols, depth.type());
01376
01377 for(int y=0; y<depth.rows; ++y)
01378 {
01379 for(int x=0; x<depth.cols; ++x)
01380 {
01381
01382 float dz = float(depth.at<unsigned short>(y,x))*0.001f;
01383 if(dz>=0.0f)
01384 {
01385
01386 P4[0] = (x - cx) * dz / fx;
01387 P4[1] = (y - cy) * dz / fy;
01388 P4[2] = dz;
01389
01390 P3 = proj * P4;
01391 float z = P3[2];
01392 float invZ = 1.0f/z;
01393 int dx = (rfx*P3[0])*invZ + rcx;
01394 int dy = (rfy*P3[1])*invZ + rcy;
01395
01396 if(uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
01397 {
01398 unsigned short z16 = z * 1000;
01399 unsigned short &zReg = registered.at<unsigned short>(dy, dx);
01400 if(zReg == 0 || z16 < zReg)
01401 {
01402 zReg = z16;
01403 }
01404 }
01405 }
01406 }
01407 }
01408 return registered;
01409 }
01410
01411 void fillRegisteredDepthHoles(cv::Mat & registeredDepth, bool vertical, bool horizontal, bool fillDoubleHoles)
01412 {
01413 UASSERT(registeredDepth.type() == CV_16UC1);
01414 int margin = fillDoubleHoles?2:1;
01415 for(int x=1; x<registeredDepth.cols-margin; ++x)
01416 {
01417 for(int y=1; y<registeredDepth.rows-margin; ++y)
01418 {
01419 unsigned short & b = registeredDepth.at<unsigned short>(y, x);
01420 bool set = false;
01421 if(vertical)
01422 {
01423 const unsigned short & a = registeredDepth.at<unsigned short>(y-1, x);
01424 unsigned short & c = registeredDepth.at<unsigned short>(y+1, x);
01425 if(a && c)
01426 {
01427 unsigned short error = 0.01*((a+c)/2);
01428 if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
01429 (a>c?a-c<=error:c-a<=error))
01430 {
01431 b = (a+c)/2;
01432 set = true;
01433 if(!horizontal)
01434 {
01435 ++y;
01436 }
01437 }
01438 }
01439 if(!set && fillDoubleHoles)
01440 {
01441 const unsigned short & d = registeredDepth.at<unsigned short>(y+2, x);
01442 if(a && d && (b==0 || c==0))
01443 {
01444 unsigned short error = 0.01*((a+d)/2);
01445 if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
01446 ((c == 0 && a && d) || (c > a+error && c > d+error)) &&
01447 (a>d?a-d<=error:d-a<=error))
01448 {
01449 if(a>d)
01450 {
01451 unsigned short tmp = (a-d)/4;
01452 b = d + tmp;
01453 c = d + 3*tmp;
01454 }
01455 else
01456 {
01457 unsigned short tmp = (d-a)/4;
01458 b = a + tmp;
01459 c = a + 3*tmp;
01460 }
01461 set = true;
01462 if(!horizontal)
01463 {
01464 y+=2;
01465 }
01466 }
01467 }
01468 }
01469 }
01470 if(!set && horizontal)
01471 {
01472 const unsigned short & a = registeredDepth.at<unsigned short>(y, x-1);
01473 unsigned short & c = registeredDepth.at<unsigned short>(y, x+1);
01474 if(a && c)
01475 {
01476 unsigned short error = 0.01*((a+c)/2);
01477 if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
01478 (a>c?a-c<=error:c-a<=error))
01479 {
01480 b = (a+c)/2;
01481 set = true;
01482 }
01483 }
01484 if(!set && fillDoubleHoles)
01485 {
01486 const unsigned short & d = registeredDepth.at<unsigned short>(y, x+2);
01487 if(a && d && (b==0 || c==0))
01488 {
01489 unsigned short error = 0.01*((a+d)/2);
01490 if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
01491 ((c == 0 && a && d) || (c > a+error && c > d+error)) &&
01492 (a>d?a-d<=error:d-a<=error))
01493 {
01494 if(a>d)
01495 {
01496 unsigned short tmp = (a-d)/4;
01497 b = d + tmp;
01498 c = d + 3*tmp;
01499 }
01500 else
01501 {
01502 unsigned short tmp = (d-a)/4;
01503 b = a + tmp;
01504 c = a + 3*tmp;
01505 }
01506 }
01507 }
01508 }
01509 }
01510 }
01511 }
01512 }
01513
01514 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
01515 {
01516 cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
01517 for(unsigned int i=0; i<cloud.size(); ++i)
01518 {
01519 laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
01520 laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
01521 }
01522 return laserScan;
01523 }
01524
01525 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
01526 {
01527 UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
01528
01529 pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
01530 output->resize(laserScan.cols);
01531 for(int i=0; i<laserScan.cols; ++i)
01532 {
01533 output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
01534 output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
01535 }
01536 return output;
01537 }
01538
01539 void extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
01540 const std::multimap<int, pcl::PointXYZ> & words2,
01541 pcl::PointCloud<pcl::PointXYZ> & cloud1,
01542 pcl::PointCloud<pcl::PointXYZ> & cloud2)
01543 {
01544 const std::list<int> & ids = uUniqueKeys(words1);
01545 for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
01546 {
01547 if(words1.count(*i) == 1 && words2.count(*i) == 1)
01548 {
01549 const pcl::PointXYZ & pt1 = words1.find(*i)->second;
01550 const pcl::PointXYZ & pt2 = words2.find(*i)->second;
01551 if(pcl::isFinite(pt1) && pcl::isFinite(pt2))
01552 {
01553 cloud1.push_back(pt1);
01554 cloud2.push_back(pt2);
01555 }
01556 }
01557 }
01558 }
01559
01560 void extractXYZCorrespondencesRANSAC(const std::multimap<int, pcl::PointXYZ> & words1,
01561 const std::multimap<int, pcl::PointXYZ> & words2,
01562 pcl::PointCloud<pcl::PointXYZ> & cloud1,
01563 pcl::PointCloud<pcl::PointXYZ> & cloud2)
01564 {
01565 std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> > pairs;
01566 const std::list<int> & ids = uUniqueKeys(words1);
01567 for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
01568 {
01569 if(words1.count(*i) == 1 && words2.count(*i) == 1)
01570 {
01571 const pcl::PointXYZ & pt1 = words1.find(*i)->second;
01572 const pcl::PointXYZ & pt2 = words2.find(*i)->second;
01573 if(pcl::isFinite(pt1) && pcl::isFinite(pt2))
01574 {
01575 pairs.push_back(std::pair<pcl::PointXYZ, pcl::PointXYZ>(pt1, pt2));
01576 }
01577 }
01578 }
01579
01580 if(pairs.size() > 7)
01581 {
01582
01583 std::vector<uchar> status(pairs.size(), 0);
01584
01585
01586 cv::Mat points1(1, (int)pairs.size(), CV_32FC2);
01587 cv::Mat points2(1, (int)pairs.size(), CV_32FC2);
01588
01589 float * points1data = points1.ptr<float>(0);
01590 float * points2data = points2.ptr<float>(0);
01591
01592
01593 int i=0;
01594 for(std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> >::const_iterator iter = pairs.begin();
01595 iter != pairs.end();
01596 ++iter )
01597 {
01598 points1data[i*2] = (*iter).first.x;
01599 points1data[i*2+1] = (*iter).first.y;
01600
01601 points2data[i*2] = (*iter).second.x;
01602 points2data[i*2+1] = (*iter).second.y;
01603
01604 ++i;
01605 }
01606
01607
01608 cv::Mat fundamentalMatrix = cv::findFundamentalMat(
01609 points1,
01610 points2,
01611 status,
01612 cv::FM_RANSAC,
01613 3.0,
01614 0.99);
01615
01616 if(!fundamentalMatrix.empty())
01617 {
01618 int i = 0;
01619 for(std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
01620 {
01621 if(status[i])
01622 {
01623 cloud1.push_back(iter->first);
01624 cloud2.push_back(iter->second);
01625 }
01626 ++i;
01627 }
01628 }
01629 }
01630 }
01631
01632 void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
01633 const cv::Mat & depthImage1,
01634 const cv::Mat & depthImage2,
01635 float cx, float cy,
01636 float fx, float fy,
01637 float maxDepth,
01638 pcl::PointCloud<pcl::PointXYZ> & cloud1,
01639 pcl::PointCloud<pcl::PointXYZ> & cloud2)
01640 {
01641 cloud1.resize(correspondences.size());
01642 cloud2.resize(correspondences.size());
01643 int oi=0;
01644 for(std::list<std::pair<cv::Point2f, cv::Point2f> >::const_iterator iter = correspondences.begin();
01645 iter!=correspondences.end();
01646 ++iter)
01647 {
01648 pcl::PointXYZ pt1 = projectDepthTo3D(depthImage1, iter->first.x, iter->first.y, cx, cy, fx, fy, true);
01649 pcl::PointXYZ pt2 = projectDepthTo3D(depthImage2, iter->second.x, iter->second.y, cx, cy, fx, fy, true);
01650 if(pcl::isFinite(pt1) && pcl::isFinite(pt2) &&
01651 (maxDepth <= 0 || (pt1.z <= maxDepth && pt2.z<=maxDepth)))
01652 {
01653 cloud1[oi] = pt1;
01654 cloud2[oi] = pt2;
01655 ++oi;
01656 }
01657 }
01658 cloud1.resize(oi);
01659 cloud2.resize(oi);
01660 }
01661
01662 template<typename PointT>
01663 inline void extractXYZCorrespondencesImpl(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
01664 const pcl::PointCloud<PointT> & cloud1,
01665 const pcl::PointCloud<PointT> & cloud2,
01666 pcl::PointCloud<pcl::PointXYZ> & inliers1,
01667 pcl::PointCloud<pcl::PointXYZ> & inliers2,
01668 char depthAxis)
01669 {
01670 for(std::list<std::pair<cv::Point2f, cv::Point2f> >::const_iterator iter = correspondences.begin();
01671 iter!=correspondences.end();
01672 ++iter)
01673 {
01674 PointT pt1 = cloud1.at(int(iter->first.y+0.5f) * cloud1.width + int(iter->first.x+0.5f));
01675 PointT pt2 = cloud2.at(int(iter->second.y+0.5f) * cloud2.width + int(iter->second.x+0.5f));
01676 if(pcl::isFinite(pt1) &&
01677 pcl::isFinite(pt2))
01678 {
01679 inliers1.push_back(pcl::PointXYZ(pt1.x, pt1.y, pt1.z));
01680 inliers2.push_back(pcl::PointXYZ(pt2.x, pt2.y, pt2.z));
01681 }
01682 }
01683 }
01684
01685 void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
01686 const pcl::PointCloud<pcl::PointXYZ> & cloud1,
01687 const pcl::PointCloud<pcl::PointXYZ> & cloud2,
01688 pcl::PointCloud<pcl::PointXYZ> & inliers1,
01689 pcl::PointCloud<pcl::PointXYZ> & inliers2,
01690 char depthAxis)
01691 {
01692 extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
01693 }
01694 void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
01695 const pcl::PointCloud<pcl::PointXYZRGB> & cloud1,
01696 const pcl::PointCloud<pcl::PointXYZRGB> & cloud2,
01697 pcl::PointCloud<pcl::PointXYZ> & inliers1,
01698 pcl::PointCloud<pcl::PointXYZ> & inliers2,
01699 char depthAxis)
01700 {
01701 extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
01702 }
01703
01704 int countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
01705 const std::multimap<int, pcl::PointXYZ> & wordsB)
01706 {
01707 const std::list<int> & ids = uUniqueKeys(wordsA);
01708 int pairs = 0;
01709 for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
01710 {
01711 std::list<pcl::PointXYZ> ptsA = uValues(wordsA, *i);
01712 std::list<pcl::PointXYZ> ptsB = uValues(wordsB, *i);
01713 if(ptsA.size() == 1 && ptsB.size() == 1)
01714 {
01715 ++pairs;
01716 }
01717 }
01718 return pairs;
01719 }
01720
01721 void filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
01722 pcl::PointCloud<pcl::PointXYZ> & inliers2,
01723 float maxDepth,
01724 char depthAxis,
01725 bool removeDuplicates)
01726 {
01727 std::list<pcl::PointXYZ> addedPts;
01728 if(maxDepth > 0.0f && inliers1.size() && inliers1.size() == inliers2.size())
01729 {
01730 pcl::PointCloud<pcl::PointXYZ> tmpInliers1;
01731 pcl::PointCloud<pcl::PointXYZ> tmpInliers2;
01732 for(unsigned int i=0; i<inliers1.size(); ++i)
01733 {
01734 if((depthAxis == 'x' && inliers1.at(i).x < maxDepth && inliers2.at(i).x < maxDepth) ||
01735 (depthAxis == 'y' && inliers1.at(i).y < maxDepth && inliers2.at(i).y < maxDepth) ||
01736 (depthAxis == 'z' && inliers1.at(i).z < maxDepth && inliers2.at(i).z < maxDepth))
01737 {
01738 bool dup = false;
01739 if(removeDuplicates)
01740 {
01741 for(std::list<pcl::PointXYZ>::iterator iter = addedPts.begin(); iter!=addedPts.end(); ++iter)
01742 {
01743 if(iter->x == inliers1.at(i).x &&
01744 iter->y == inliers1.at(i).y &&
01745 iter->z == inliers1.at(i).z)
01746 {
01747 dup = true;
01748 }
01749 }
01750 if(!dup)
01751 {
01752 addedPts.push_back(inliers1.at(i));
01753 }
01754 }
01755
01756 if(!dup)
01757 {
01758 tmpInliers1.push_back(inliers1.at(i));
01759 tmpInliers2.push_back(inliers2.at(i));
01760 }
01761 }
01762 }
01763 inliers1 = tmpInliers1;
01764 inliers2 = tmpInliers2;
01765 }
01766 }
01767
01768
01769 Transform transformFromXYZCorrespondences(
01770 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
01771 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
01772 double inlierThreshold,
01773 int iterations,
01774 bool refineModel,
01775 double refineModelSigma,
01776 int refineModelIterations,
01777 std::vector<int> * inliersOut,
01778 double * varianceOut)
01779 {
01780
01781
01782
01783
01784 if(varianceOut)
01785 {
01786 *varianceOut = 1.0;
01787 }
01788 Transform transform;
01789 if(cloud1->size() >=3 && cloud1->size() == cloud2->size())
01790 {
01791
01792 UDEBUG("iterations=%d inlierThreshold=%f", iterations, inlierThreshold);
01793 std::vector<int> source_indices (cloud2->size());
01794 std::vector<int> target_indices (cloud1->size());
01795
01796
01797 for (int i = 0; i < (int)cloud1->size(); ++i)
01798 {
01799 source_indices[i] = i;
01800 target_indices[i] = i;
01801 }
01802
01803
01804
01805 pcl::SampleConsensusModelRegistration<pcl::PointXYZ>::Ptr model;
01806 model.reset(new pcl::SampleConsensusModelRegistration<pcl::PointXYZ>(cloud2, source_indices));
01807
01808 model->setInputTarget (cloud1, target_indices);
01809
01810 pcl::RandomSampleConsensus<pcl::PointXYZ> sac (model, inlierThreshold);
01811 sac.setMaxIterations(iterations);
01812
01813
01814 if(sac.computeModel())
01815 {
01816 std::vector<int> inliers;
01817 Eigen::VectorXf model_coefficients;
01818
01819 sac.getInliers(inliers);
01820 sac.getModelCoefficients (model_coefficients);
01821
01822 if (refineModel)
01823 {
01824 double inlier_distance_threshold_sqr = inlierThreshold * inlierThreshold;
01825 double error_threshold = inlierThreshold;
01826 double sigma_sqr = refineModelSigma * refineModelSigma;
01827 int refine_iterations = 0;
01828 bool inlier_changed = false, oscillating = false;
01829 std::vector<int> new_inliers, prev_inliers = inliers;
01830 std::vector<size_t> inliers_sizes;
01831 Eigen::VectorXf new_model_coefficients = model_coefficients;
01832 do
01833 {
01834
01835 model->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
01836 inliers_sizes.push_back (prev_inliers.size ());
01837
01838
01839 model->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
01840 UDEBUG("RANSAC refineModel: Number of inliers found (before/after): %d/%d, with an error threshold of %f.",
01841 (int)prev_inliers.size (), (int)new_inliers.size (), error_threshold);
01842
01843 if (new_inliers.empty ())
01844 {
01845 ++refine_iterations;
01846 if (refine_iterations >= refineModelIterations)
01847 {
01848 break;
01849 }
01850 continue;
01851 }
01852
01853
01854 double variance = model->computeVariance ();
01855 error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
01856
01857 UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f) on iteration %d out of %d.",
01858 error_threshold, variance, refine_iterations, refineModelIterations);
01859 inlier_changed = false;
01860 std::swap (prev_inliers, new_inliers);
01861
01862
01863 if (new_inliers.size () != prev_inliers.size ())
01864 {
01865
01866 if (inliers_sizes.size () >= 4)
01867 {
01868 if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
01869 inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
01870 {
01871 oscillating = true;
01872 break;
01873 }
01874 }
01875 inlier_changed = true;
01876 continue;
01877 }
01878
01879
01880 for (size_t i = 0; i < prev_inliers.size (); ++i)
01881 {
01882
01883 if (prev_inliers[i] != new_inliers[i])
01884 {
01885 inlier_changed = true;
01886 break;
01887 }
01888 }
01889 }
01890 while (inlier_changed && ++refine_iterations < refineModelIterations);
01891
01892
01893 if (new_inliers.empty ())
01894 {
01895 UWARN ("RANSAC refineModel: Refinement failed: got an empty set of inliers!");
01896 }
01897
01898 if (oscillating)
01899 {
01900 UDEBUG("RANSAC refineModel: Detected oscillations in the model refinement.");
01901 }
01902
01903 std::swap (inliers, new_inliers);
01904 model_coefficients = new_model_coefficients;
01905 }
01906
01907 if (inliers.size() >= 3)
01908 {
01909 if(inliersOut)
01910 {
01911 *inliersOut = inliers;
01912 }
01913 if(varianceOut)
01914 {
01915 *varianceOut = model->computeVariance();
01916 }
01917
01918
01919 Eigen::Matrix4f bestTransformation;
01920 bestTransformation.row (0) = model_coefficients.segment<4>(0);
01921 bestTransformation.row (1) = model_coefficients.segment<4>(4);
01922 bestTransformation.row (2) = model_coefficients.segment<4>(8);
01923 bestTransformation.row (3) = model_coefficients.segment<4>(12);
01924
01925 transform = Transform::fromEigen4f(bestTransformation);
01926 UDEBUG("RANSAC inliers=%d/%d tf=%s", (int)inliers.size(), (int)cloud1->size(), transform.prettyPrint().c_str());
01927
01928 return transform.inverse();
01929 }
01930 else
01931 {
01932 UDEBUG("RANSAC: Model with inliers < 3");
01933 }
01934 }
01935 else
01936 {
01937 UDEBUG("RANSAC: Failed to find model");
01938 }
01939 }
01940 else
01941 {
01942 UDEBUG("Not enough points to compute the transform");
01943 }
01944 return Transform();
01945 }
01946
01947
01948 Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
01949 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
01950 double maxCorrespondenceDistance,
01951 int maximumIterations,
01952 bool * hasConvergedOut,
01953 double * variance,
01954 int * correspondencesOut)
01955 {
01956 pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
01957
01958 icp.setInputTarget (cloud_target);
01959 icp.setInputSource (cloud_source);
01960
01961
01962 icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
01963
01964 icp.setMaximumIterations (maximumIterations);
01965
01966
01967
01968
01969
01970
01971
01972 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
01973 icp.align (*cloud_source_registered);
01974 bool hasConverged = icp.hasConverged();
01975
01976
01977 if((correspondencesOut || variance) && hasConverged)
01978 {
01979 pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
01980 est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
01981 est->setInputTarget(cloud_target);
01982 est->setInputSource(cloud_source_registered);
01983 pcl::Correspondences correspondences;
01984 est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
01985 if(variance)
01986 {
01987 if(correspondences.size()>=3)
01988 {
01989 std::vector<double> distances(correspondences.size());
01990 for(unsigned int i=0; i<correspondences.size(); ++i)
01991 {
01992 distances[i] = correspondences[i].distance;
01993 }
01994
01995
01996 std::sort(distances.begin (), distances.end ());
01997 double median_error_sqr = distances[distances.size () >> 1];
01998 *variance = (2.1981 * median_error_sqr);
01999 }
02000 else
02001 {
02002 hasConverged = false;
02003 *variance = -1.0;
02004 }
02005 }
02006
02007 if(correspondencesOut)
02008 {
02009 *correspondencesOut = (int)correspondences.size();
02010 }
02011 }
02012 else
02013 {
02014 if(correspondencesOut)
02015 {
02016 *correspondencesOut = 0;
02017 }
02018 if(variance)
02019 {
02020 *variance = -1;
02021 }
02022 }
02023
02024 if(hasConvergedOut)
02025 {
02026 *hasConvergedOut = hasConverged;
02027 }
02028
02029 return Transform::fromEigen4f(icp.getFinalTransformation());
02030 }
02031
02032
02033 Transform icpPointToPlane(
02034 const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
02035 const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
02036 double maxCorrespondenceDistance,
02037 int maximumIterations,
02038 bool * hasConvergedOut,
02039 double * variance,
02040 int * correspondencesOut)
02041 {
02042 pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
02043
02044 icp.setInputTarget (cloud_target);
02045 icp.setInputSource (cloud_source);
02046
02047 pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr est;
02048 est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);
02049 icp.setTransformationEstimation(est);
02050
02051
02052 icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
02053
02054 icp.setMaximumIterations (maximumIterations);
02055
02056
02057
02058
02059
02060
02061
02062 pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointNormal>);
02063 icp.align (*cloud_source_registered);
02064 bool hasConverged = icp.hasConverged();
02065
02066
02067 if((correspondencesOut || variance) && hasConverged)
02068 {
02069 pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
02070 est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
02071 est->setInputTarget(cloud_target);
02072 est->setInputSource(cloud_source_registered);
02073 pcl::Correspondences correspondences;
02074 est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
02075 if(variance)
02076 {
02077 if(correspondences.size()>=3)
02078 {
02079 std::vector<double> distances(correspondences.size());
02080 for(unsigned int i=0; i<correspondences.size(); ++i)
02081 {
02082 distances[i] = correspondences[i].distance;
02083 }
02084
02085
02086 std::sort(distances.begin (), distances.end ());
02087 double median_error_sqr = distances[distances.size () >> 1];
02088 *variance = (2.1981 * median_error_sqr);
02089 }
02090 else
02091 {
02092 hasConverged = false;
02093 *variance = -1.0;
02094 }
02095 }
02096
02097 if(correspondencesOut)
02098 {
02099 *correspondencesOut = (int)correspondences.size();
02100 }
02101 }
02102 else
02103 {
02104 if(correspondencesOut)
02105 {
02106 *correspondencesOut = 0;
02107 }
02108 if(variance)
02109 {
02110 *variance = -1;
02111 }
02112 }
02113
02114 if(hasConvergedOut)
02115 {
02116 *hasConvergedOut = hasConverged;
02117 }
02118
02119 return Transform::fromEigen4f(icp.getFinalTransformation());
02120 }
02121
02122
02123 Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
02124 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
02125 double maxCorrespondenceDistance,
02126 int maximumIterations,
02127 bool * hasConvergedOut,
02128 double * variance,
02129 int * correspondencesOut)
02130 {
02131 pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
02132
02133 icp.setInputTarget (cloud_target);
02134 icp.setInputSource (cloud_source);
02135
02136 pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
02137 est.reset(new pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>);
02138 icp.setTransformationEstimation(est);
02139
02140
02141 icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
02142
02143 icp.setMaximumIterations (maximumIterations);
02144
02145
02146
02147
02148
02149
02150
02151 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
02152 icp.align (*cloud_source_registered);
02153 bool hasConverged = icp.hasConverged();
02154
02155
02156 if((correspondencesOut || variance) && hasConverged)
02157 {
02158 pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
02159 est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
02160 est->setInputTarget(cloud_target);
02161 est->setInputSource(cloud_source_registered);
02162 pcl::Correspondences correspondences;
02163 est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
02164 if(variance)
02165 {
02166 if(correspondences.size()>=3)
02167 {
02168 std::vector<double> distances(correspondences.size());
02169 for(unsigned int i=0; i<correspondences.size(); ++i)
02170 {
02171 distances[i] = correspondences[i].distance;
02172 }
02173
02174
02175 std::sort(distances.begin (), distances.end ());
02176 double median_error_sqr = distances[distances.size () >> 1];
02177 *variance = (2.1981 * median_error_sqr);
02178 }
02179 else
02180 {
02181 hasConverged = false;
02182 *variance = -1.0;
02183 }
02184 }
02185
02186 if(correspondencesOut)
02187 {
02188 *correspondencesOut = (int)correspondences.size();
02189 }
02190 }
02191 else
02192 {
02193 if(correspondencesOut)
02194 {
02195 *correspondencesOut = 0;
02196 }
02197 if(variance)
02198 {
02199 *variance = -1;
02200 }
02201 }
02202
02203 if(hasConvergedOut)
02204 {
02205 *hasConvergedOut = hasConverged;
02206 }
02207
02208 return Transform::fromEigen4f(icp.getFinalTransformation());
02209 }
02210
02211 pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
02212 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
02213 int normalKSearch)
02214 {
02215 pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
02216 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
02217 tree->setInputCloud (cloud);
02218
02219
02220 pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
02221 pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
02222 n.setInputCloud (cloud);
02223 n.setSearchMethod (tree);
02224 n.setKSearch (normalKSearch);
02225 n.compute (*normals);
02226
02227
02228
02229 pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
02230
02231
02232 return cloud_with_normals;
02233 }
02234
02235 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
02236 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
02237 int normalKSearch)
02238 {
02239 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
02240 pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
02241 tree->setInputCloud (cloud);
02242
02243
02244 pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal> n;
02245 pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
02246 n.setInputCloud (cloud);
02247 n.setSearchMethod (tree);
02248 n.setKSearch (normalKSearch);
02249 n.compute (*normals);
02250
02251
02252
02253 pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
02254
02255
02256 return cloud_with_normals;
02257 }
02258
02259 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormalsSmoothed(
02260 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
02261 float smoothingSearchRadius,
02262 bool smoothingPolynomialFit)
02263 {
02264 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
02265 pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
02266 tree->setInputCloud (cloud);
02267
02268
02269 pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> mls;
02270
02271 mls.setComputeNormals (true);
02272
02273
02274 mls.setInputCloud (cloud);
02275 mls.setPolynomialFit (smoothingPolynomialFit);
02276 mls.setSearchMethod (tree);
02277 mls.setSearchRadius (smoothingSearchRadius);
02278
02279
02280 mls.process (*cloud_with_normals);
02281
02282 return cloud_with_normals;
02283 }
02284
02285
02286
02287 int getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
02288 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
02289 float maxDistance)
02290 {
02291 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
02292 kdTree->setInputCloud(cloud_target);
02293 int count = 0;
02294 float sqrdMaxDistance = maxDistance * maxDistance;
02295 for(unsigned int i=0; i<cloud_source->size(); ++i)
02296 {
02297 std::vector<int> ind(1);
02298 std::vector<float> dist(1);
02299 if(kdTree->nearestKSearch(cloud_source->at(i), 1, ind, dist) && dist[0] < sqrdMaxDistance)
02300 {
02301 ++count;
02302 }
02303 }
02304 return count;
02305 }
02306
02311 void findCorrespondences(
02312 const std::multimap<int, cv::KeyPoint> & wordsA,
02313 const std::multimap<int, cv::KeyPoint> & wordsB,
02314 std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs)
02315 {
02316 const std::list<int> & ids = uUniqueKeys(wordsA);
02317 pairs.clear();
02318 for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
02319 {
02320 std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
02321 std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
02322 if(ptsA.size() == 1 && ptsB.size() == 1)
02323 {
02324 pairs.push_back(std::pair<cv::Point2f, cv::Point2f>(ptsA.front().pt, ptsB.front().pt));
02325 }
02326 }
02327 }
02328
02329 pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
02330 const cv::Mat & matrix,
02331 const Transform & tranform)
02332 {
02333 UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
02334 UASSERT(matrix.rows == 1);
02335
02336 Eigen::Affine3f t = tranform.toEigen3f();
02337 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
02338 cloud->resize(matrix.cols);
02339 if(matrix.channels() == 2)
02340 {
02341 for(int i=0; i<matrix.cols; ++i)
02342 {
02343 cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
02344 cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
02345 cloud->at(i).z = 0.0f;
02346 cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
02347 }
02348 }
02349 else
02350 {
02351 for(int i=0; i<matrix.cols; ++i)
02352 {
02353 cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
02354 cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
02355 cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
02356 cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
02357 }
02358 }
02359 return cloud;
02360 }
02361
02362
02363 pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
02364 const cv::Mat & depth,
02365 float fx,
02366 float fy,
02367 float cx,
02368 float cy,
02369 int decimation,
02370 double maxDepth,
02371 float voxel,
02372 int samples,
02373 const Transform & transform)
02374 {
02375 UASSERT(!depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1));
02376 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
02377 cloud = cloudFromDepth(
02378 depth,
02379 cx,
02380 cy,
02381 fx,
02382 fy,
02383 decimation);
02384
02385 if(cloud->size())
02386 {
02387 if(maxDepth>0.0)
02388 {
02389 cloud = passThrough<pcl::PointXYZ>(cloud, "z", 0, maxDepth);
02390 }
02391
02392 if(cloud->size())
02393 {
02394 if(voxel>0)
02395 {
02396 cloud = voxelize<pcl::PointXYZ>(cloud, voxel);
02397 }
02398 else if(samples>0 && (int)cloud->size() > samples)
02399 {
02400 cloud = sampling<pcl::PointXYZ>(cloud, samples);
02401 }
02402
02403 if(cloud->size())
02404 {
02405 if(!transform.isNull() && !transform.isIdentity())
02406 {
02407 cloud = transformPointCloud<pcl::PointXYZ>(cloud, transform);
02408 }
02409 }
02410 }
02411 }
02412
02413 return cloud;
02414 }
02415
02416 pcl::PointCloud<pcl::PointXYZ>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds)
02417 {
02418 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
02419 for(std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
02420 {
02421 *cloud += *(*iter);
02422 }
02423 return cloud;
02424 }
02425
02426 pcl::PointCloud<pcl::PointXYZRGB>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds)
02427 {
02428 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
02429 for(std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
02430 {
02431 *cloud+=*(*iter);
02432 }
02433 return cloud;
02434 }
02435
02436 pcl::PointCloud<pcl::PointXYZ>::Ptr get3DFASTKpts(
02437 const cv::Mat & image,
02438 const cv::Mat & imageDepth,
02439 float constant,
02440 int fastThreshold,
02441 bool fastNonmaxSuppression,
02442 float maxDepth)
02443 {
02444
02445 cv::FastFeatureDetector detector(fastThreshold, fastNonmaxSuppression);
02446 std::vector<cv::KeyPoint> kpts;
02447 detector.detect(image, kpts);
02448
02449 pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
02450 for(unsigned int i=0; i<kpts.size(); ++i)
02451 {
02452 pcl::PointXYZ pt = projectDepthTo3D(imageDepth, kpts[i].pt.x, kpts[i].pt.y, 0, 0, 1.0f/constant, 1.0f/constant, true);
02453 if(uIsFinite(pt.z) && (maxDepth <= 0 || pt.z <= maxDepth))
02454 {
02455 points->push_back(pt);
02456 }
02457 }
02458 UDEBUG("points %d -> %d", (int)kpts.size(), (int)points->size());
02459 return points;
02460 }
02461
02462 pcl::PolygonMesh::Ptr createMesh(
02463 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
02464 float gp3SearchRadius,
02465 float gp3Mu,
02466 int gp3MaximumNearestNeighbors,
02467 float gp3MaximumSurfaceAngle,
02468 float gp3MinimumAngle,
02469 float gp3MaximumAngle,
02470 bool gp3NormalConsistency)
02471 {
02472 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormalsNoNaN = removeNaNNormalsFromPointCloud<pcl::PointXYZRGBNormal>(cloudWithNormals);
02473
02474
02475 pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
02476 tree2->setInputCloud (cloudWithNormalsNoNaN);
02477
02478
02479 pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
02480 pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
02481
02482
02483 gp3.setSearchRadius (gp3SearchRadius);
02484
02485
02486 gp3.setMu (gp3Mu);
02487 gp3.setMaximumNearestNeighbors (gp3MaximumNearestNeighbors);
02488 gp3.setMaximumSurfaceAngle(gp3MaximumSurfaceAngle);
02489 gp3.setMinimumAngle(gp3MinimumAngle);
02490 gp3.setMaximumAngle(gp3MaximumAngle);
02491 gp3.setNormalConsistency(gp3NormalConsistency);
02492
02493
02494 gp3.setInputCloud (cloudWithNormalsNoNaN);
02495 gp3.setSearchMethod (tree2);
02496 gp3.reconstruct (*mesh);
02497
02498 return mesh;
02499 }
02500
02501 void occupancy2DFromLaserScan(
02502 const cv::Mat & scan,
02503 cv::Mat & ground,
02504 cv::Mat & obstacles,
02505 float cellSize)
02506 {
02507 if(scan.empty())
02508 {
02509 return;
02510 }
02511
02512 std::map<int, Transform> poses;
02513 poses.insert(std::make_pair(1, Transform::getIdentity()));
02514
02515 pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud = util3d::laserScanToPointCloud(scan);
02516
02517
02518 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> scans;
02519 scans.insert(std::make_pair(1, obstaclesCloud));
02520
02521 float xMin, yMin;
02522 cv::Mat map8S = create2DMap(poses, scans, cellSize, false, xMin, yMin);
02523
02524
02525 std::list<int> groundIndices;
02526 for(unsigned int i=0; i< map8S.total(); ++i)
02527 {
02528 if(map8S.data[i] == 0)
02529 {
02530 groundIndices.push_back(i);
02531 }
02532 }
02533
02534
02535 ground = cv::Mat();
02536 if(groundIndices.size())
02537 {
02538 ground = cv::Mat((int)groundIndices.size(), 1, CV_32FC2);
02539 int i=0;
02540 for(std::list<int>::iterator iter=groundIndices.begin();iter!=groundIndices.end(); ++iter)
02541 {
02542 int x = *iter / map8S.cols;
02543 int y = *iter - x*map8S.cols;
02544 ground.at<cv::Vec2f>(i)[0] = (float(y)+0.5)*cellSize + xMin;
02545 ground.at<cv::Vec2f>(i)[1] = (float(x)+0.5)*cellSize + yMin;
02546 ++i;
02547 }
02548 }
02549
02550
02551 obstacles = cv::Mat();
02552 if(obstaclesCloud->size())
02553 {
02554 obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
02555 for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
02556 {
02557 obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
02558 obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
02559 }
02560 }
02561 }
02562
02576 cv::Mat create2DMapFromOccupancyLocalMaps(
02577 const std::map<int, Transform> & poses,
02578 const std::map<int, std::pair<cv::Mat, cv::Mat> > & occupancy,
02579 float cellSize,
02580 float & xMin,
02581 float & yMin,
02582 float minMapSize,
02583 bool erode)
02584 {
02585 UASSERT(minMapSize >= 0.0f);
02586 UDEBUG("cellSize=%f m, minMapSize=%f m, erode=%d", cellSize, minMapSize, erode?1:0);
02587 UTimer timer;
02588
02589 std::map<int, cv::Mat> emptyLocalMaps;
02590 std::map<int, cv::Mat> occupiedLocalMaps;
02591
02592 float minX=-minMapSize/2.0, minY=-minMapSize/2.0, maxX=minMapSize/2.0, maxY=minMapSize/2.0;
02593 bool undefinedSize = minMapSize == 0.0f;
02594 float x=0.0f,y=0.0f,z=0.0f,roll=0.0f,pitch=0.0f,yaw=0.0f,cosT=0.0f,sinT=0.0f;
02595 cv::Mat affineTransform(2,3,CV_32FC1);
02596 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
02597 {
02598 UASSERT(!iter->second.isNull());
02599
02600 iter->second.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
02601
02602 if(undefinedSize)
02603 {
02604 minX = maxX = x;
02605 minY = maxY = y;
02606 undefinedSize = false;
02607 }
02608 else
02609 {
02610 if(minX > x)
02611 minX = x;
02612 else if(maxX < x)
02613 maxX = x;
02614
02615 if(minY > y)
02616 minY = y;
02617 else if(maxY < y)
02618 maxY = y;
02619 }
02620
02621 if(uContains(occupancy, iter->first))
02622 {
02623 const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
02624 cosT = cos(yaw);
02625 sinT = sin(yaw);
02626 affineTransform.at<float>(0,0) = cosT;
02627 affineTransform.at<float>(0,1) = -sinT;
02628 affineTransform.at<float>(1,0) = sinT;
02629 affineTransform.at<float>(1,1) = cosT;
02630 affineTransform.at<float>(0,2) = x;
02631 affineTransform.at<float>(1,2) = y;
02632
02633
02634 if(pair.first.rows)
02635 {
02636 UASSERT(pair.first.type() == CV_32FC2);
02637 cv::Mat ground(pair.first.rows, pair.first.cols, pair.first.type());
02638 cv::transform(pair.first, ground, affineTransform);
02639 for(int i=0; i<ground.rows; ++i)
02640 {
02641 if(minX > ground.at<float>(i,0))
02642 minX = ground.at<float>(i,0);
02643 else if(maxX < ground.at<float>(i,0))
02644 maxX = ground.at<float>(i,0);
02645
02646 if(minY > ground.at<float>(i,1))
02647 minY = ground.at<float>(i,1);
02648 else if(maxY < ground.at<float>(i,1))
02649 maxY = ground.at<float>(i,1);
02650 }
02651 emptyLocalMaps.insert(std::make_pair(iter->first, ground));
02652 }
02653
02654
02655 if(pair.second.rows)
02656 {
02657 UASSERT(pair.second.type() == CV_32FC2);
02658 cv::Mat obstacles(pair.second.rows, pair.second.cols, pair.second.type());
02659 cv::transform(pair.second, obstacles, affineTransform);
02660 for(int i=0; i<obstacles.rows; ++i)
02661 {
02662 if(minX > obstacles.at<float>(i,0))
02663 minX = obstacles.at<float>(i,0);
02664 else if(maxX < obstacles.at<float>(i,0))
02665 maxX = obstacles.at<float>(i,0);
02666
02667 if(minY > obstacles.at<float>(i,1))
02668 minY = obstacles.at<float>(i,1);
02669 else if(maxY < obstacles.at<float>(i,1))
02670 maxY = obstacles.at<float>(i,1);
02671 }
02672 occupiedLocalMaps.insert(std::make_pair(iter->first, obstacles));
02673 }
02674 }
02675 }
02676 UDEBUG("timer=%fs", timer.ticks());
02677
02678 cv::Mat map;
02679 if(minX != maxX && minY != maxY)
02680 {
02681
02682 float margin = cellSize*10.0f;
02683 xMin = minX-margin;
02684 yMin = minY-margin;
02685 float xMax = maxX+margin;
02686 float yMax = maxY+margin;
02687 if(fabs((yMax - yMin) / cellSize) > 99999 ||
02688 fabs((xMax - xMin) / cellSize) > 99999)
02689 {
02690 UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
02691 "There's maybe an error with the poses provided! The map will not be created!",
02692 xMin, yMin, xMax, yMax);
02693 }
02694 else
02695 {
02696 UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
02697
02698
02699 map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
02700 for(std::map<int, Transform>::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
02701 {
02702 std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
02703 std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
02704 if(iter!=emptyLocalMaps.end())
02705 {
02706 for(int i=0; i<iter->second.rows; ++i)
02707 {
02708 cv::Point2i pt((iter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (iter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
02709 map.at<char>(pt.y, pt.x) = 0;
02710 }
02711 }
02712 if(jter!=occupiedLocalMaps.end())
02713 {
02714 for(int i=0; i<jter->second.rows; ++i)
02715 {
02716 cv::Point2i pt((jter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (jter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
02717 map.at<char>(pt.y, pt.x) = 100;
02718 }
02719 }
02720
02721
02722 }
02723
02724
02725 cv::Mat updatedMap = map.clone();
02726 std::list<std::pair<int, int> > obstacleIndices;
02727 for(int i=2; i<map.rows-2; ++i)
02728 {
02729 for(int j=2; j<map.cols-2; ++j)
02730 {
02731 if(map.at<char>(i, j) == -1 &&
02732 map.at<char>(i+1, j) != -1 &&
02733 map.at<char>(i-1, j) != -1 &&
02734 map.at<char>(i, j+1) != -1 &&
02735 map.at<char>(i, j-1) != -1)
02736 {
02737 updatedMap.at<char>(i, j) = 0;
02738 }
02739 else if(map.at<char>(i, j) == 100)
02740 {
02741
02742
02743 if(map.at<char>(i-1, j) == 0 &&
02744 map.at<char>(i-2, j) == -1)
02745 {
02746 updatedMap.at<char>(i-1, j) = -1;
02747 }
02748 else if(map.at<char>(i+1, j) == 0 &&
02749 map.at<char>(i+2, j) == -1)
02750 {
02751 updatedMap.at<char>(i+1, j) = -1;
02752 }
02753 if(map.at<char>(i, j-1) == 0 &&
02754 map.at<char>(i, j-2) == -1)
02755 {
02756 updatedMap.at<char>(i, j-1) = -1;
02757 }
02758 else if(map.at<char>(i, j+1) == 0 &&
02759 map.at<char>(i, j+2) == -1)
02760 {
02761 updatedMap.at<char>(i, j+1) = -1;
02762 }
02763
02764 if(erode)
02765 {
02766 obstacleIndices.push_back(std::make_pair(i, j));
02767 }
02768 }
02769 else if(map.at<char>(i, j) == 0)
02770 {
02771
02772 if(map.at<char>(i-1, j) == 100 &&
02773 map.at<char>(i+1, j) == 100)
02774 {
02775 updatedMap.at<char>(i, j) = -1;
02776 }
02777 else if(map.at<char>(i, j-1) == 100 &&
02778 map.at<char>(i, j+1) == 100)
02779 {
02780 updatedMap.at<char>(i, j) = -1;
02781 }
02782 }
02783
02784 }
02785 }
02786 map = updatedMap;
02787
02788 if(erode)
02789 {
02790
02791 cv::Mat erodedMap = map.clone();
02792 for(std::list<std::pair<int,int> >::iterator iter = obstacleIndices.begin();
02793 iter!= obstacleIndices.end();
02794 ++iter)
02795 {
02796 int i = iter->first;
02797 int j = iter->second;
02798 bool touchEmpty = map.at<char>(i+1, j) == 0 ||
02799 map.at<char>(i-1, j) == 0 ||
02800 map.at<char>(i, j+1) == 0 ||
02801 map.at<char>(i, j-1) == 0;
02802 if(touchEmpty && map.at<char>(i+1, j) != -1 &&
02803 map.at<char>(i-1, j) != -1 &&
02804 map.at<char>(i, j+1) != -1 &&
02805 map.at<char>(i, j-1) != -1)
02806 {
02807 erodedMap.at<char>(i, j) = 0;
02808 }
02809 }
02810 map = erodedMap;
02811 }
02812 }
02813 }
02814 UDEBUG("timer=%fs", timer.ticks());
02815 return map;
02816 }
02817
02830 cv::Mat create2DMap(const std::map<int, Transform> & poses,
02831 const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
02832 float cellSize,
02833 bool unknownSpaceFilled,
02834 float & xMin,
02835 float & yMin,
02836 float minMapSize)
02837 {
02838 UDEBUG("poses=%d, scans = %d", poses.size(), scans.size());
02839 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
02840
02841 pcl::PointCloud<pcl::PointXYZ> minMax;
02842 if(minMapSize > 0.0f)
02843 {
02844 minMax.push_back(pcl::PointXYZ(-minMapSize/2.0, -minMapSize/2.0, 0));
02845 minMax.push_back(pcl::PointXYZ(minMapSize/2.0, minMapSize/2.0, 0));
02846 }
02847 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
02848 {
02849 if(uContains(scans, iter->first) && scans.at(iter->first)->size())
02850 {
02851 UASSERT(!iter->second.isNull());
02852 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = transformPointCloud<pcl::PointXYZ>(scans.at(iter->first), iter->second);
02853 pcl::PointXYZ min, max;
02854 pcl::getMinMax3D(*cloud, min, max);
02855 minMax.push_back(min);
02856 minMax.push_back(max);
02857 minMax.push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
02858 localScans.insert(std::make_pair(iter->first, cloud));
02859 }
02860 }
02861
02862 cv::Mat map;
02863 if(minMax.size())
02864 {
02865
02866 pcl::PointXYZ min, max;
02867 pcl::getMinMax3D(minMax, min, max);
02868
02869
02870 float marging = cellSize*10.0f;
02871 xMin = min.x-marging;
02872 yMin = min.y-marging;
02873 float xMax = max.x+marging;
02874 float yMax = max.y+marging;
02875
02876 UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
02877
02878 UTimer timer;
02879
02880 map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
02881 std::vector<float> maxSquaredLength(localScans.size(), 0.0f);
02882 int j=0;
02883 for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
02884 {
02885 const Transform & pose = poses.at(iter->first);
02886 cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f);
02887 for(unsigned int i=0; i<iter->second->size(); ++i)
02888 {
02889 cv::Point2i end((iter->second->points[i].x-xMin)/cellSize + 0.5f, (iter->second->points[i].y-yMin)/cellSize + 0.5f);
02890 map.at<char>(end.y, end.x) = 100;
02891 rayTrace(start, end, map, true);
02892
02893 if(unknownSpaceFilled)
02894 {
02895 float dx = iter->second->points[i].x - pose.x();
02896 float dy = iter->second->points[i].y - pose.y();
02897 float l = dx*dx + dy*dy;
02898 if(l > maxSquaredLength[j])
02899 {
02900 maxSquaredLength[j] = l;
02901 }
02902 }
02903 }
02904 ++j;
02905 }
02906 UDEBUG("Ray trace known space=%fs", timer.ticks());
02907
02908
02909 if(unknownSpaceFilled)
02910 {
02911 j=0;
02912 for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
02913 {
02914 if(iter->second->size() > 1 && maxSquaredLength[j] > 0.0f)
02915 {
02916 float maxLength = sqrt(maxSquaredLength[j]);
02917 if(maxLength > cellSize)
02918 {
02919
02920 float a = (CV_PI/2.0f) / (maxLength / cellSize);
02921
02922 UASSERT_MSG(a >= 0 && a < 5.0f*CV_PI/8.0f, uFormat("a=%f length=%f cell=%f", a, maxLength, cellSize).c_str());
02923
02924 const Transform & pose = poses.at(iter->first);
02925 cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f);
02926
02927
02928
02929 cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(a), -sin(a),
02930 sin(a), cos(a));
02931 cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F);
02932 origin.at<float>(0) = pose.x();
02933 origin.at<float>(1) = pose.y();
02934 endFirst.at<float>(0) = iter->second->points[0].x;
02935 endFirst.at<float>(1) = iter->second->points[0].y;
02936 endLast.at<float>(0) = iter->second->points[iter->second->points.size()-1].x;
02937 endLast.at<float>(1) = iter->second->points[iter->second->points.size()-1].y;
02938
02939
02940
02941 cv::Mat tmp = (endFirst - origin);
02942 cv::Mat endRotated = rotation*((tmp/cv::norm(tmp))*maxLength) + origin;
02943 cv::Mat endLastVector(3,1,CV_32F), endRotatedVector(3,1,CV_32F);
02944 endLastVector.at<float>(0) = endLast.at<float>(0) - origin.at<float>(0);
02945 endLastVector.at<float>(1) = endLast.at<float>(1) - origin.at<float>(1);
02946 endLastVector.at<float>(2) = 0.0f;
02947 endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);
02948 endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
02949 endRotatedVector.at<float>(2) = 0.0f;
02950
02951 while(endRotatedVector.cross(endLastVector).at<float>(2) > 0.0f)
02952 {
02953 cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize + 0.5f, (endRotated.at<float>(1)-yMin)/cellSize + 0.5f);
02954
02955 end.x = end.x < 0?0:end.x;
02956 end.x = end.x >= map.cols?map.cols-1:end.x;
02957 end.y = end.y < 0?0:end.y;
02958 end.y = end.y >= map.rows?map.rows-1:end.y;
02959 rayTrace(start, end, map, true);
02960
02961
02962 endRotated = rotation*(endRotated - origin) + origin;
02963 endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);
02964 endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
02965
02966 }
02967 }
02968 }
02969 ++j;
02970 }
02971 UDEBUG("Fill empty space=%fs", timer.ticks());
02972 }
02973 }
02974 return map;
02975 }
02976
02977 void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid, bool stopOnObstacle)
02978 {
02979 UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
02980 UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
02981 UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
02982 UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
02983
02984 cv::Point2i ptA, ptB;
02985 ptA = start;
02986 ptB = end;
02987
02988 float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
02989
02990 bool swapped = false;
02991 if(slope<-1.0f || slope>1.0f)
02992 {
02993
02994 slope = 1.0f/slope;
02995
02996 int tmp = ptA.x;
02997 ptA.x = ptA.y;
02998 ptA.y = tmp;
02999
03000 tmp = ptB.x;
03001 ptB.x = ptB.y;
03002 ptB.y = tmp;
03003
03004 swapped = true;
03005 }
03006
03007 float b = ptA.y - slope*ptA.x;
03008 for(int x=ptA.x; ptA.x<ptB.x?x<ptB.x:x>ptB.x; ptA.x<ptB.x?++x:--x)
03009 {
03010 int upperbound = float(x)*slope + b;
03011 int lowerbound = upperbound;
03012 if(x != ptA.x)
03013 {
03014 lowerbound = (ptA.x<ptB.x?x+1:x-1)*slope + b;
03015 }
03016
03017 if(lowerbound > upperbound)
03018 {
03019 int tmp = upperbound;
03020 upperbound = lowerbound;
03021 lowerbound = tmp;
03022 }
03023
03024 if(!swapped)
03025 {
03026 UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%f grid.rows=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.rows, x, slope, b, x).c_str());
03027 UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.rows=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.rows, x+1, slope, b, x).c_str());
03028 }
03029 else
03030 {
03031 UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.cols, uFormat("lowerbound=%f grid.cols=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.cols, x, slope, b, x).c_str());
03032 UASSERT_MSG(upperbound >= 0 && upperbound < grid.cols, uFormat("upperbound=%f grid.cols=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.cols, x+1, slope, b, x).c_str());
03033 }
03034
03035 for(int y = lowerbound; y<=(int)upperbound; ++y)
03036 {
03037 char * v;
03038 if(swapped)
03039 {
03040 v = &grid.at<char>(x, y);
03041 }
03042 else
03043 {
03044 v = &grid.at<char>(y, x);
03045 }
03046 if(*v == 100 && stopOnObstacle)
03047 {
03048 return;
03049 }
03050 else
03051 {
03052 *v = 0;
03053 }
03054 }
03055 }
03056 }
03057
03058
03059 cv::Mat convertMap2Image8U(const cv::Mat & map8S)
03060 {
03061 UASSERT(map8S.channels() == 1 && map8S.type() == CV_8S);
03062 cv::Mat map8U = cv::Mat(map8S.rows, map8S.cols, CV_8U);
03063 for (int i = 0; i < map8S.rows; ++i)
03064 {
03065 for (int j = 0; j < map8S.cols; ++j)
03066 {
03067 char v = map8S.at<char>(i, j);
03068 unsigned char gray;
03069 if(v == 0)
03070 {
03071 gray = 178;
03072 }
03073 else if(v == 100)
03074 {
03075 gray = 0;
03076 }
03077 else
03078 {
03079 gray = 89;
03080 }
03081 map8U.at<unsigned char>(i, j) = gray;
03082 }
03083 }
03084 return map8U;
03085 }
03086
03087 pcl::IndicesPtr concatenate(const std::vector<pcl::IndicesPtr> & indices)
03088 {
03089
03090 unsigned int totalSize = 0;
03091 for(unsigned int i=0; i<indices.size(); ++i)
03092 {
03093 totalSize += (unsigned int)indices[i]->size();
03094 }
03095 pcl::IndicesPtr ind(new std::vector<int>(totalSize));
03096 unsigned int io = 0;
03097 for(unsigned int i=0; i<indices.size(); ++i)
03098 {
03099 for(unsigned int j=0; j<indices[i]->size(); ++j)
03100 {
03101 ind->at(io++) = indices[i]->at(j);
03102 }
03103 }
03104 return ind;
03105 }
03106
03107 pcl::IndicesPtr concatenate(const pcl::IndicesPtr & indicesA, const pcl::IndicesPtr & indicesB)
03108 {
03109 pcl::IndicesPtr ind(new std::vector<int>(*indicesA));
03110 ind->resize(ind->size()+indicesB->size());
03111 unsigned int oi = (unsigned int)indicesA->size();
03112 for(unsigned int i=0; i<indicesB->size(); ++i)
03113 {
03114 ind->at(oi++) = indicesB->at(i);
03115 }
03116 return ind;
03117 }
03118
03119 cv::Mat decimate(const cv::Mat & image, int decimation)
03120 {
03121 UASSERT(decimation >= 1);
03122 cv::Mat out;
03123 if(!image.empty())
03124 {
03125 if(decimation > 1)
03126 {
03127 if((image.type() == CV_32FC1 || image.type()==CV_16UC1))
03128 {
03129 UASSERT_MSG(image.rows % decimation == 0 && image.cols % decimation == 0, "Decimation of depth images should be exact!");
03130
03131 out = cv::Mat(image.rows/decimation, image.cols/decimation, image.type());
03132 if(image.type() == CV_32FC1)
03133 {
03134 for(int j=0; j<out.rows; ++j)
03135 {
03136 for(int i=0; i<out.cols; ++i)
03137 {
03138 out.at<float>(j, i) = image.at<float>(j*decimation, i*decimation);
03139 }
03140 }
03141 }
03142 else
03143 {
03144 for(int j=0; j<out.rows; ++j)
03145 {
03146 for(int i=0; i<out.cols; ++i)
03147 {
03148 out.at<unsigned short>(j, i) = image.at<unsigned short>(j*decimation, i*decimation);
03149 }
03150 }
03151 }
03152 }
03153 else
03154 {
03155 cv::resize(image, out, cv::Size(), 1.0f/float(decimation), 1.0f/float(decimation), cv::INTER_AREA);
03156 }
03157 }
03158 else
03159 {
03160 out = image;
03161 }
03162 }
03163 return out;
03164 }
03165
03166 void savePCDWords(
03167 const std::string & fileName,
03168 const std::multimap<int, pcl::PointXYZ> & words,
03169 const Transform & transform)
03170 {
03171 if(words.size())
03172 {
03173 pcl::PointCloud<pcl::PointXYZ> cloud;
03174 cloud.resize(words.size());
03175 int i=0;
03176 for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
03177 {
03178 cloud[i++] = util3d::transformPoint(iter->second, transform);
03179 }
03180 pcl::io::savePCDFile(fileName, cloud);
03181 }
03182 }
03183
03184 }
03185
03186 }