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/Odometry.h"
00029 #include "rtabmap/core/OdometryInfo.h"
00030 #include "rtabmap/core/Features2d.h"
00031 #include "rtabmap/core/util3d.h"
00032 #include "rtabmap/utilite/ULogger.h"
00033 #include "rtabmap/utilite/UTimer.h"
00034 #include "rtabmap/utilite/UConversion.h"
00035 #include "rtabmap/utilite/UStl.h"
00036 #include <opencv2/imgproc/imgproc.hpp>
00037 #include <opencv2/video/tracking.hpp>
00038
00039 namespace rtabmap {
00040
00041 OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
00042 Odometry(parameters),
00043 flowWinSize_(Parameters::defaultOdomFlowWinSize()),
00044 flowIterations_(Parameters::defaultOdomFlowIterations()),
00045 flowEps_(Parameters::defaultOdomFlowEps()),
00046 flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
00047 stereoWinSize_(Parameters::defaultStereoWinSize()),
00048 stereoIterations_(Parameters::defaultStereoIterations()),
00049 stereoEps_(Parameters::defaultStereoEps()),
00050 stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
00051 stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
00052 subPixWinSize_(Parameters::defaultOdomSubPixWinSize()),
00053 subPixIterations_(Parameters::defaultOdomSubPixIterations()),
00054 subPixEps_(Parameters::defaultOdomSubPixEps()),
00055 refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
00056 {
00057 Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
00058 Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
00059 Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
00060 Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
00061 Parameters::parse(parameters, Parameters::kStereoWinSize(), stereoWinSize_);
00062 Parameters::parse(parameters, Parameters::kStereoIterations(), stereoIterations_);
00063 Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_);
00064 Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_);
00065 Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_);
00066 Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize_);
00067 Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations_);
00068 Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps_);
00069
00070 ParametersMap::const_iterator iter;
00071 Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
00072 if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
00073 {
00074 detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
00075 }
00076
00077 ParametersMap customParameters;
00078 int maxFeatures = Parameters::defaultOdomMaxFeatures();
00079 Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
00080 customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
00081
00082 for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00083 {
00084 std::string group = uSplit(iter->first, '/').front();
00085 if(group.compare("SURF") == 0 ||
00086 group.compare("SIFT") == 0 ||
00087 group.compare("BRIEF") == 0 ||
00088 group.compare("FAST") == 0 ||
00089 group.compare("ORB") == 0 ||
00090 group.compare("FREAK") == 0 ||
00091 group.compare("GFTT") == 0 ||
00092 group.compare("BRISK") == 0)
00093 {
00094 customParameters.insert(*iter);
00095 }
00096 }
00097
00098 feature2D_ = Feature2D::create(detectorStrategy, customParameters);
00099 }
00100
00101 OdometryOpticalFlow::~OdometryOpticalFlow()
00102 {
00103 delete feature2D_;
00104 }
00105
00106
00107 void OdometryOpticalFlow::reset(const Transform & initialPose)
00108 {
00109 Odometry::reset(initialPose);
00110 refFrame_ = cv::Mat();
00111 refCorners_.clear();
00112 refCorners3D_->clear();
00113 }
00114
00115
00116 Transform OdometryOpticalFlow::computeTransform(
00117 const SensorData & data,
00118 OdometryInfo * info)
00119 {
00120 UDEBUG("");
00121
00122 if(info)
00123 {
00124 info->type = 1;
00125 }
00126
00127 if(!data.rightImage().empty())
00128 {
00129
00130 return computeTransformStereo(data, info);
00131 }
00132 else
00133 {
00134
00135 return computeTransformRGBD(data, info);
00136 }
00137 }
00138
00139 Transform OdometryOpticalFlow::computeTransformStereo(
00140 const SensorData & data,
00141 OdometryInfo * info)
00142 {
00143 UTimer timer;
00144 Transform output;
00145
00146 double variance = 0;
00147 int inliers = 0;
00148 int correspondences = 0;
00149
00150 cv::Mat newLeftFrame;
00151
00152 if(data.image().channels() > 1)
00153 {
00154 cv::cvtColor(data.image(), newLeftFrame, cv::COLOR_BGR2GRAY);
00155 }
00156 else
00157 {
00158 newLeftFrame = data.image().clone();
00159 }
00160 cv::Mat newRightFrame = data.rightImage().clone();
00161
00162 std::vector<cv::Point2f> newCorners;
00163 UDEBUG("lastCorners_.size()=%d lastFrame_=%d lastRightFrame_=%d", (int)refCorners_.size(), refFrame_.empty()?0:1, refRightFrame_.empty()?0:1);
00164 if(!refFrame_.empty() && !refRightFrame_.empty() && refCorners_.size())
00165 {
00166 UDEBUG("");
00167
00168 std::vector<unsigned char> status;
00169 std::vector<float> err;
00170 UDEBUG("cv::calcOpticalFlowPyrLK() begin");
00171 cv::calcOpticalFlowPyrLK(
00172 refFrame_,
00173 newLeftFrame,
00174 refCorners_,
00175 newCorners,
00176 status,
00177 err,
00178 cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
00179 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
00180 cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
00181 UDEBUG("cv::calcOpticalFlowPyrLK() end");
00182
00183 std::vector<cv::Point2f> lastCornersKept(status.size());
00184 std::vector<cv::Point2f> newCornersKept(status.size());
00185 int ki = 0;
00186 for(unsigned int i=0; i<status.size(); ++i)
00187 {
00188 if(status[i])
00189 {
00190 lastCornersKept[ki] = refCorners_[i];
00191 newCornersKept[ki] = newCorners[i];
00192 ++ki;
00193 }
00194 }
00195 lastCornersKept.resize(ki);
00196 newCornersKept.resize(ki);
00197
00198 if(ki && ki >= this->getMinInliers())
00199 {
00200 std::vector<unsigned char> statusLast;
00201 std::vector<float> errLast;
00202 std::vector<cv::Point2f> lastCornersKeptRight;
00203 UDEBUG("previous stereo disparity");
00204 cv::calcOpticalFlowPyrLK(
00205 refFrame_,
00206 refRightFrame_,
00207 lastCornersKept,
00208 lastCornersKeptRight,
00209 statusLast,
00210 errLast,
00211 cv::Size(stereoWinSize_, stereoWinSize_), stereoMaxLevel_,
00212 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoIterations_, stereoEps_),
00213 cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
00214
00215 UDEBUG("new stereo disparity");
00216 std::vector<unsigned char> statusNew;
00217 std::vector<float> errNew;
00218 std::vector<cv::Point2f> newCornersKeptRight;
00219 cv::calcOpticalFlowPyrLK(
00220 newLeftFrame,
00221 newRightFrame,
00222 newCornersKept,
00223 newCornersKeptRight,
00224 statusNew,
00225 errNew,
00226 cv::Size(stereoWinSize_, stereoWinSize_), stereoMaxLevel_,
00227 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoIterations_, stereoEps_),
00228 cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
00229
00230 if(this->isPnPEstimationUsed())
00231 {
00232
00233 if(this->isInfoDataFilled() && info)
00234 {
00235 info->refCorners.resize(statusLast.size());
00236 info->newCorners.resize(statusLast.size());
00237 }
00238
00239 int flowInliers = 0;
00240 std::vector<cv::Point3f> objectPoints(statusLast.size());
00241 std::vector<cv::Point2f> imagePoints(statusLast.size());
00242 std::vector<pcl::PointXYZ> image3DPoints(statusLast.size());
00243 int oi=0;
00244 float bad_point = std::numeric_limits<float>::quiet_NaN ();
00245 for(unsigned int i=0; i<statusLast.size(); ++i)
00246 {
00247 if(statusLast[i])
00248 {
00249 float lastDisparity = lastCornersKept[i].x - lastCornersKeptRight[i].x;
00250 float lastSlope = fabs((lastCornersKept[i].y-lastCornersKeptRight[i].y) / (lastCornersKept[i].x-lastCornersKeptRight[i].x));
00251 float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x;
00252 float newSlope = fabs((newCornersKept[i].y-newCornersKeptRight[i].y) / (newCornersKept[i].x-newCornersKeptRight[i].x));
00253 if(lastDisparity > 0.0f && lastSlope < stereoMaxSlope_)
00254 {
00255 pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
00256 lastCornersKept[i],
00257 lastDisparity,
00258 data.cx(), data.cy(), data.fx(), data.baseline());
00259
00260 if(pcl::isFinite(lastPt3D) &&
00261 (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())))
00262 {
00263
00264 lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
00265 objectPoints[oi].x = lastPt3D.x;
00266 objectPoints[oi].y = lastPt3D.y;
00267 objectPoints[oi].z = lastPt3D.z;
00268 imagePoints[oi] = newCornersKept.at(i);
00269
00270
00271 image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point);
00272 if(newDisparity > 0.0f && newSlope < stereoMaxSlope_)
00273 {
00274 pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D(
00275 newCornersKept[i],
00276 newDisparity,
00277 data.cx(), data.cy(), data.fx(), data.baseline());
00278 if(pcl::isFinite(newPt3D) &&
00279 (this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth())))
00280 {
00281 image3DPoints[oi] = util3d::transformPoint(newPt3D, data.localTransform());
00282 }
00283 }
00284
00285 if(this->isInfoDataFilled() && info)
00286 {
00287 info->refCorners[oi] = lastCornersKept[i];
00288 info->newCorners[oi] = newCornersKept[i];
00289 }
00290 ++oi;
00291 }
00292 }
00293 ++flowInliers;
00294 }
00295 }
00296 objectPoints.resize(oi);
00297 imagePoints.resize(oi);
00298 image3DPoints.resize(oi);
00299 UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
00300
00301 if(this->isInfoDataFilled() && info)
00302 {
00303 info->refCorners.resize(oi);
00304 info->newCorners.resize(oi);
00305 }
00306
00307 correspondences = oi;
00308
00309 if(correspondences >= this->getMinInliers())
00310 {
00311
00312 cv::Mat K = (cv::Mat_<double>(3,3) <<
00313 data.fx(), 0, data.cx(),
00314 0, data.fx(), data.cy(),
00315 0, 0, 1);
00316 Transform guess = (data.localTransform()).inverse();
00317 cv::Mat R = (cv::Mat_<double>(3,3) <<
00318 (double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
00319 (double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
00320 (double)guess.r31(), (double)guess.r32(), (double)guess.r33());
00321 cv::Mat rvec(1,3, CV_64FC1);
00322 cv::Rodrigues(R, rvec);
00323 cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
00324 std::vector<int> inliersV;
00325 cv::solvePnPRansac(objectPoints,
00326 imagePoints,
00327 K,
00328 cv::Mat(),
00329 rvec,
00330 tvec,
00331 true,
00332 this->getIterations(),
00333 this->getPnPReprojError(),
00334 0,
00335 inliersV,
00336 this->getPnPFlags());
00337
00338 inliers = (int)inliersV.size();
00339 if((int)inliersV.size() >= this->getMinInliers())
00340 {
00341 cv::Rodrigues(rvec, R);
00342 Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
00343 R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
00344 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
00345
00346
00347 output = (data.localTransform() * pnp).inverse();
00348
00349 UDEBUG("Odom transform = %s", output.prettyPrint().c_str());
00350
00351
00352 std::vector<float> errorSqrdDists(inliersV.size());
00353 int ii=0;
00354 for(unsigned int i=0; i<inliersV.size(); ++i)
00355 {
00356 pcl::PointXYZ & newPt = image3DPoints[inliersV[i]];
00357 if(pcl::isFinite(newPt))
00358 {
00359 newPt = util3d::transformPoint(newPt, output);
00360 const cv::Point3f & objPt = objectPoints[inliersV[i]];
00361 errorSqrdDists[ii++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
00362 }
00363 }
00364 errorSqrdDists.resize(ii);
00365 if(errorSqrdDists.size())
00366 {
00367 std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
00368 double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
00369 variance = 2.1981 * median_error_sqr;
00370 }
00371 }
00372 else
00373 {
00374 UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
00375 }
00376
00377 if(this->isInfoDataFilled() && info)
00378 {
00379 info->cornerInliers = inliersV;
00380 }
00381 }
00382 else
00383 {
00384 UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
00385 }
00386 }
00387 else
00388 {
00389 UDEBUG("Getting correspondences begin");
00390
00391 pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
00392 pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
00393 correspondencesLast->resize(statusLast.size());
00394 correspondencesNew->resize(statusLast.size());
00395 int oi = 0;
00396 if(this->isInfoDataFilled() && info)
00397 {
00398 info->refCorners.resize(statusLast.size());
00399 info->newCorners.resize(statusLast.size());
00400 }
00401 for(unsigned int i=0; i<statusLast.size(); ++i)
00402 {
00403 if(statusLast[i] && statusNew[i])
00404 {
00405 float lastDisparity = lastCornersKept[i].x - lastCornersKeptRight[i].x;
00406 float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x;
00407 float lastSlope = fabs((lastCornersKept[i].y-lastCornersKeptRight[i].y) / (lastCornersKept[i].x-lastCornersKeptRight[i].x));
00408 float newSlope = fabs((newCornersKept[i].y-newCornersKeptRight[i].y) / (newCornersKept[i].x-newCornersKeptRight[i].x));
00409 if(lastDisparity > 0.0f && newDisparity > 0.0f &&
00410 lastSlope < stereoMaxSlope_ && newSlope < stereoMaxSlope_)
00411 {
00412 pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
00413 lastCornersKept[i],
00414 lastDisparity,
00415 data.cx(), data.cy(), data.fx(), data.baseline());
00416 pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D(
00417 newCornersKept[i],
00418 newDisparity,
00419 data.cx(), data.cy(), data.fx(), data.baseline());
00420
00421 if(pcl::isFinite(lastPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())) &&
00422 pcl::isFinite(newPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth())))
00423 {
00424
00425 lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
00426 newPt3D = util3d::transformPoint(newPt3D, data.localTransform());
00427 correspondencesLast->at(oi) = lastPt3D;
00428 correspondencesNew->at(oi) = newPt3D;
00429 if(this->isInfoDataFilled() && info)
00430 {
00431 info->refCorners[oi] = lastCornersKept[i];
00432 info->newCorners[oi] = newCornersKept[i];
00433 }
00434 ++oi;
00435 }
00436 }
00437 }
00438 }
00439 correspondencesLast->resize(oi);
00440 correspondencesNew->resize(oi);
00441 if(this->isInfoDataFilled() && info)
00442 {
00443 info->refCorners.resize(oi);
00444 info->newCorners.resize(oi);
00445 }
00446 correspondences = oi;
00447 refCorners3D_ = correspondencesNew;
00448 UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)statusLast.size());
00449
00450 if(correspondences >= this->getMinInliers())
00451 {
00452 std::vector<int> inliersV;
00453 UTimer timerRANSAC;
00454 Transform t = util3d::transformFromXYZCorrespondences(
00455 correspondencesNew,
00456 correspondencesLast,
00457 this->getInlierDistance(),
00458 this->getIterations(),
00459 this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
00460 &inliersV,
00461 &variance);
00462 UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
00463
00464 inliers = (int)inliersV.size();
00465 if(!t.isNull() && inliers >= this->getMinInliers())
00466 {
00467 output = t;
00468 }
00469 else
00470 {
00471 UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
00472 }
00473
00474 if(this->isInfoDataFilled() && info)
00475 {
00476 info->cornerInliers = inliersV;
00477 }
00478 }
00479 else
00480 {
00481 UWARN("Not enough correspondences (%d)", correspondences);
00482 }
00483 }
00484 }
00485 }
00486 else
00487 {
00488
00489 output = Transform::getIdentity();
00490 }
00491
00492 newCorners.clear();
00493 if(!output.isNull())
00494 {
00495
00496 if(data.keypoints().size())
00497 {
00498 newCorners.resize(data.keypoints().size());
00499 for(unsigned int i=0; i<data.keypoints().size(); ++i)
00500 {
00501 newCorners[i] = data.keypoints().at(i).pt;
00502 }
00503 }
00504 else
00505 {
00506
00507 std::vector<cv::KeyPoint> newKtps;
00508 cv::Rect roi = Feature2D::computeRoi(newLeftFrame, this->getRoiRatios());
00509 newKtps = feature2D_->generateKeypoints(newLeftFrame, roi);
00510
00511 if(newKtps.size())
00512 {
00513 cv::KeyPoint::convert(newKtps, newCorners);
00514
00515 if(subPixWinSize_ > 0 && subPixIterations_ > 0)
00516 {
00517 UDEBUG("cv::cornerSubPix() begin");
00518 cv::cornerSubPix(newLeftFrame, newCorners,
00519 cv::Size( subPixWinSize_, subPixWinSize_ ),
00520 cv::Size( -1, -1 ),
00521 cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
00522 UDEBUG("cv::cornerSubPix() end");
00523 }
00524 }
00525 }
00526
00527 if((int)newCorners.size() > this->getMinInliers())
00528 {
00529 refFrame_ = newLeftFrame;
00530 refRightFrame_ = newRightFrame;
00531 refCorners_ = newCorners;
00532 }
00533 else
00534 {
00535 UWARN("Too low 2D corners (%d), ignoring new frame...",
00536 (int)newCorners.size());
00537 output.setNull();
00538 }
00539 }
00540
00541 if(info)
00542 {
00543 info->type = 1;
00544 info->variance = variance;
00545 info->inliers = inliers;
00546 info->features = (int)newCorners.size();
00547 info->matches = correspondences;
00548 }
00549
00550 UINFO("Odom update time = %fs lost=%s inliers=%d/%d, new corners=%d, transform accepted=%s",
00551 timer.elapsed(),
00552 output.isNull()?"true":"false",
00553 inliers,
00554 correspondences,
00555 (int)newCorners.size(),
00556 !output.isNull()?"true":"false");
00557
00558 return output;
00559 }
00560
00561 Transform OdometryOpticalFlow::computeTransformRGBD(
00562 const SensorData & data,
00563 OdometryInfo * info)
00564 {
00565 UTimer timer;
00566 Transform output;
00567
00568 double variance = 0;
00569 int inliers = 0;
00570 int correspondences = 0;
00571
00572 cv::Mat newFrame;
00573
00574 if(data.image().channels() > 1)
00575 {
00576 cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
00577 }
00578 else
00579 {
00580 newFrame = data.image().clone();
00581 }
00582
00583 std::vector<cv::Point2f> newCorners;
00584 if(!refFrame_.empty() &&
00585 (int)refCorners_.size() >= this->getMinInliers() &&
00586 (int)refCorners3D_->size() >= this->getMinInliers())
00587 {
00588 std::vector<unsigned char> status;
00589 std::vector<float> err;
00590 UDEBUG("cv::calcOpticalFlowPyrLK() begin");
00591 cv::calcOpticalFlowPyrLK(
00592 refFrame_,
00593 newFrame,
00594 refCorners_,
00595 newCorners,
00596 status,
00597 err,
00598 cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
00599 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
00600 cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
00601 UDEBUG("cv::calcOpticalFlowPyrLK() end");
00602
00603 if(this->isPnPEstimationUsed())
00604 {
00605
00606 if(this->isInfoDataFilled() && info)
00607 {
00608 info->refCorners.resize(refCorners_.size());
00609 info->newCorners.resize(refCorners_.size());
00610 }
00611
00612 UASSERT(refCorners_.size() == refCorners3D_->size());
00613 UDEBUG("lastCorners3D_ = %d", refCorners3D_->size());
00614 int flowInliers = 0;
00615 std::vector<cv::Point3f> objectPoints(refCorners_.size());
00616 std::vector<cv::Point2f> imagePoints(refCorners_.size());
00617 std::vector<pcl::PointXYZ> image3DPoints(refCorners_.size());
00618 int oi=0;
00619 float bad_point = std::numeric_limits<float>::quiet_NaN ();
00620 for(unsigned int i=0; i<status.size(); ++i)
00621 {
00622 if(status[i])
00623 {
00624 if(pcl::isFinite(refCorners3D_->at(i)))
00625 {
00626 objectPoints[oi].x = refCorners3D_->at(i).x;
00627 objectPoints[oi].y = refCorners3D_->at(i).y;
00628 objectPoints[oi].z = refCorners3D_->at(i).z;
00629 imagePoints[oi] = newCorners.at(i);
00630
00631
00632 image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point);
00633 if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
00634 uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
00635 {
00636 pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
00637 data.cx(), data.cy(), data.fx(), data.fy(), true);
00638 if(pcl::isFinite(pt) &&
00639 (this->getMaxDepth() == 0.0f || (
00640 uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
00641 uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
00642 uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
00643 {
00644 image3DPoints[oi] = util3d::transformPoint(pt, data.localTransform());
00645 }
00646 }
00647
00648 if(this->isInfoDataFilled() && info)
00649 {
00650 info->refCorners[oi] = refCorners_[i];
00651 info->newCorners[oi] = newCorners[i];
00652 }
00653
00654 ++oi;
00655 }
00656 ++flowInliers;
00657 }
00658 }
00659 objectPoints.resize(oi);
00660 imagePoints.resize(oi);
00661 image3DPoints.resize(oi);
00662 UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
00663
00664 if(this->isInfoDataFilled() && info)
00665 {
00666 info->refCorners.resize(oi);
00667 info->newCorners.resize(oi);
00668 }
00669
00670 correspondences = oi;
00671
00672 if(correspondences >= this->getMinInliers())
00673 {
00674
00675 cv::Mat K = (cv::Mat_<double>(3,3) <<
00676 data.fx(), 0, data.cx(),
00677 0, data.fy(), data.cy(),
00678 0, 0, 1);
00679 Transform guess = (data.localTransform()).inverse();
00680 cv::Mat R = (cv::Mat_<double>(3,3) <<
00681 (double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
00682 (double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
00683 (double)guess.r31(), (double)guess.r32(), (double)guess.r33());
00684 cv::Mat rvec(1,3, CV_64FC1);
00685 cv::Rodrigues(R, rvec);
00686 cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
00687 std::vector<int> inliersV;
00688 cv::solvePnPRansac(objectPoints,
00689 imagePoints,
00690 K,
00691 cv::Mat(),
00692 rvec,
00693 tvec,
00694 true,
00695 this->getIterations(),
00696 this->getPnPReprojError(),
00697 0,
00698 inliersV,
00699 this->getPnPFlags());
00700
00701 inliers = (int)inliersV.size();
00702 if((int)inliersV.size() >= this->getMinInliers())
00703 {
00704 cv::Rodrigues(rvec, R);
00705 Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
00706 R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
00707 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
00708
00709
00710 output = (data.localTransform() * pnp).inverse();
00711
00712 UDEBUG("Odom transform = %s", output.prettyPrint().c_str());
00713
00714
00715 std::vector<float> errorSqrdDists(inliersV.size());
00716 int ii=0;
00717 for(unsigned int i=0; i<inliersV.size(); ++i)
00718 {
00719 pcl::PointXYZ & newPt = image3DPoints[inliersV[i]];
00720 if(pcl::isFinite(newPt))
00721 {
00722 newPt = util3d::transformPoint(newPt, output);
00723 const cv::Point3f & objPt = objectPoints[inliersV[i]];
00724 errorSqrdDists[ii++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
00725 }
00726 }
00727 errorSqrdDists.resize(ii);
00728 if(errorSqrdDists.size())
00729 {
00730 std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
00731 double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
00732 variance = 2.1981 * median_error_sqr;
00733 }
00734 }
00735 else
00736 {
00737 UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
00738 }
00739
00740 if(this->isInfoDataFilled() && info)
00741 {
00742 info->cornerInliers = inliersV;
00743 }
00744 }
00745 else
00746 {
00747 UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
00748 }
00749 }
00750 else
00751 {
00752 pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
00753 pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
00754 correspondencesLast->resize(refCorners_.size());
00755 correspondencesNew->resize(refCorners_.size());
00756 int oi=0;
00757
00758 if(this->isInfoDataFilled() && info)
00759 {
00760 info->refCorners.resize(refCorners_.size());
00761 info->newCorners.resize(refCorners_.size());
00762 }
00763
00764 UASSERT(refCorners_.size() == refCorners3D_->size());
00765 UDEBUG("lastCorners3D_ = %d", refCorners3D_->size());
00766 int flowInliers = 0;
00767 for(unsigned int i=0; i<status.size(); ++i)
00768 {
00769 if(status[i] && pcl::isFinite(refCorners3D_->at(i)) &&
00770 uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
00771 uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
00772 {
00773 pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
00774 data.cx(), data.cy(), data.fx(), data.fy(), true);
00775 if(pcl::isFinite(pt) &&
00776 (this->getMaxDepth() == 0.0f || (
00777 uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
00778 uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
00779 uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
00780 {
00781 pt = util3d::transformPoint(pt, data.localTransform());
00782 correspondencesLast->at(oi) = refCorners3D_->at(i);
00783 correspondencesNew->at(oi) = pt;
00784
00785 if(this->isInfoDataFilled() && info)
00786 {
00787 info->refCorners[oi] = refCorners_[i];
00788 info->newCorners[oi] = newCorners[i];
00789 }
00790
00791 ++oi;
00792 }
00793 ++flowInliers;
00794 }
00795 else if(status[i])
00796 {
00797 ++flowInliers;
00798 }
00799 }
00800 UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
00801
00802 if(this->isInfoDataFilled() && info)
00803 {
00804 info->refCorners.resize(oi);
00805 info->newCorners.resize(oi);
00806 }
00807 correspondencesLast->resize(oi);
00808 correspondencesNew->resize(oi);
00809 correspondences = oi;
00810 if(correspondences >= this->getMinInliers())
00811 {
00812 std::vector<int> inliersV;
00813 UTimer timerRANSAC;
00814 output = util3d::transformFromXYZCorrespondences(
00815 correspondencesNew,
00816 correspondencesLast,
00817 this->getInlierDistance(),
00818 this->getIterations(),
00819 this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
00820 &inliersV,
00821 &variance);
00822 UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
00823
00824 inliers = (int)inliersV.size();
00825 if(inliers < this->getMinInliers())
00826 {
00827 output.setNull();
00828 UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
00829 }
00830
00831 if(this->isInfoDataFilled() && info)
00832 {
00833 info->cornerInliers = inliersV;
00834 }
00835 }
00836 else
00837 {
00838 UWARN("Not enough correspondences (%d)", correspondences);
00839 }
00840 }
00841 }
00842 else
00843 {
00844
00845 output = Transform::getIdentity();
00846 }
00847
00848 newCorners.clear();
00849 if(!output.isNull())
00850 {
00851
00852 if(data.keypoints().size())
00853 {
00854 newCorners.resize(data.keypoints().size());
00855 for(unsigned int i=0; i<data.keypoints().size(); ++i)
00856 {
00857 newCorners[i] = data.keypoints().at(i).pt;
00858 }
00859 }
00860 else
00861 {
00862
00863 std::vector<cv::KeyPoint> newKtps;
00864 cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios());
00865 newKtps = feature2D_->generateKeypoints(newFrame, roi);
00866 Feature2D::filterKeypointsByDepth(newKtps, data.depth(), this->getMaxDepth());
00867
00868 if(newKtps.size())
00869 {
00870 cv::KeyPoint::convert(newKtps, newCorners);
00871
00872 if(subPixWinSize_ > 0 && subPixIterations_ > 0)
00873 {
00874 cv::cornerSubPix(newFrame, newCorners,
00875 cv::Size( subPixWinSize_, subPixWinSize_ ),
00876 cv::Size( -1, -1 ),
00877 cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
00878 }
00879 }
00880 }
00881
00882 if((int)newCorners.size() > this->getMinInliers())
00883 {
00884
00885 pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
00886 newCorners3D->resize(newCorners.size());
00887 std::vector<cv::Point2f> newCornersFiltered(newCorners.size());
00888 int oi=0;
00889 for(unsigned int i=0; i<newCorners.size(); ++i)
00890 {
00891 if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
00892 uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
00893 {
00894 pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
00895 data.cx(), data.cy(), data.fx(), data.fy(), true);
00896 if(pcl::isFinite(pt) &&
00897 (this->getMaxDepth() == 0.0f || (
00898 uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
00899 uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
00900 uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
00901 {
00902 pt = util3d::transformPoint(pt, data.localTransform());
00903 newCorners3D->at(oi) = pt;
00904 newCornersFiltered[oi] = newCorners[i];
00905 ++oi;
00906 }
00907 }
00908 }
00909 newCornersFiltered.resize(oi);
00910 newCorners3D->resize(oi);
00911 if((int)newCornersFiltered.size() > this->getMinInliers())
00912 {
00913 refFrame_ = newFrame;
00914 refCorners_ = newCornersFiltered;
00915 refCorners3D_ = newCorners3D;
00916 }
00917 else
00918 {
00919 UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
00920 (int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers());
00921 output.setNull();
00922 }
00923 }
00924 else
00925 {
00926 UWARN("Too low 2D corners (%d), ignoring new frame...",
00927 (int)newCorners.size());
00928 output.setNull();
00929 }
00930 }
00931
00932 if(info)
00933 {
00934 info->type = 1;
00935 info->variance = variance;
00936 info->inliers = inliers;
00937 info->features = (int)newCorners.size();
00938 info->matches = correspondences;
00939 }
00940
00941 UINFO("Odom update time = %fs lost=%s inliers=%d/%d, variance=%f, new corners=%d",
00942 timer.elapsed(),
00943 output.isNull()?"true":"false",
00944 inliers,
00945 correspondences,
00946 variance,
00947 (int)newCorners.size());
00948 return output;
00949 }
00950
00951 }