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/Memory.h"
00031 #include "rtabmap/core/Signature.h"
00032 #include "rtabmap/core/util3d.h"
00033 #include "rtabmap/core/EpipolarGeometry.h"
00034 #include "rtabmap/utilite/ULogger.h"
00035 #include "rtabmap/utilite/UTimer.h"
00036 #include "rtabmap/utilite/UConversion.h"
00037 #include "rtabmap/utilite/UStl.h"
00038 #include <opencv2/imgproc/imgproc.hpp>
00039 #include <opencv2/calib3d/calib3d.hpp>
00040 #include <opencv2/video/tracking.hpp>
00041
00042 namespace rtabmap {
00043
00044 OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
00045 Odometry(parameters),
00046 flowWinSize_(Parameters::defaultOdomFlowWinSize()),
00047 flowIterations_(Parameters::defaultOdomFlowIterations()),
00048 flowEps_(Parameters::defaultOdomFlowEps()),
00049 flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
00050 localHistoryMaxSize_(Parameters::defaultOdomBowLocalHistorySize()),
00051 initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
00052 initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
00053 minTranslation_(Parameters::defaultOdomMonoMinTranslation()),
00054 fundMatrixReprojError_(Parameters::defaultVhEpRansacParam1()),
00055 fundMatrixConfidence_(Parameters::defaultVhEpRansacParam2()),
00056 maxVariance_(Parameters::defaultOdomMonoMaxVariance())
00057 {
00058 Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
00059 Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
00060 Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
00061 Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
00062 Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), localHistoryMaxSize_);
00063
00064 Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_);
00065 Parameters::parse(parameters, Parameters::kOdomMonoInitMinTranslation(), initMinTranslation_);
00066 Parameters::parse(parameters, Parameters::kOdomMonoMinTranslation(), minTranslation_);
00067 Parameters::parse(parameters, Parameters::kOdomMonoMaxVariance(), maxVariance_);
00068
00069 Parameters::parse(parameters, Parameters::kVhEpRansacParam1(), fundMatrixReprojError_);
00070 Parameters::parse(parameters, Parameters::kVhEpRansacParam2(), fundMatrixConfidence_);
00071
00072
00073 ParametersMap customParameters;
00074 customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
00075 customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
00076 customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
00077 customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
00078 customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "true"));
00079 customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
00080 customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
00081 customParameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "false"));
00082 int nn = Parameters::defaultOdomBowNNType();
00083 float nndr = Parameters::defaultOdomBowNNDR();
00084 int featureType = Parameters::defaultOdomFeatureType();
00085 int maxFeatures = Parameters::defaultOdomMaxFeatures();
00086 Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
00087 Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
00088 Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
00089 Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
00090 customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
00091 customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
00092 customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
00093 customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
00094
00095 int subPixWinSize = Parameters::defaultOdomSubPixWinSize();
00096 int subPixIterations = Parameters::defaultOdomSubPixIterations();
00097 double subPixEps = Parameters::defaultOdomSubPixEps();
00098 Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize);
00099 Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations);
00100 Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps);
00101 customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
00102 customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
00103 customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));
00104
00105
00106 for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00107 {
00108 std::string group = uSplit(iter->first, '/').front();
00109 if(group.compare("SURF") == 0 ||
00110 group.compare("SIFT") == 0 ||
00111 group.compare("BRIEF") == 0 ||
00112 group.compare("FAST") == 0 ||
00113 group.compare("ORB") == 0 ||
00114 group.compare("FREAK") == 0 ||
00115 group.compare("GFTT") == 0 ||
00116 group.compare("BRISK") == 0)
00117 {
00118 customParameters.insert(*iter);
00119 }
00120 }
00121
00122 memory_ = new Memory(customParameters);
00123 if(!memory_->init("", false, ParametersMap()))
00124 {
00125 UERROR("Error initializing the memory for Mono Odometry.");
00126 }
00127 }
00128
00129 OdometryMono::~OdometryMono()
00130 {
00131 delete memory_;
00132 }
00133
00134 void OdometryMono::reset(const Transform & initialPose)
00135 {
00136 Odometry::reset(initialPose);
00137 memory_->init("", false, ParametersMap());
00138 localMap_.clear();
00139 refDepth_ = cv::Mat();
00140 cornersMap_.clear();
00141 keyFrameWords3D_.clear();
00142 keyFramePoses_.clear();
00143 }
00144
00145 Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * info)
00146 {
00147 UASSERT(!data.image().empty());
00148 UASSERT(data.fx());
00149
00150 UTimer timer;
00151 Transform output;
00152
00153 int inliers = 0;
00154 int correspondences = 0;
00155 int nFeatures = 0;
00156
00157 cv::Mat newFrame;
00158
00159 if(data.image().channels() > 1)
00160 {
00161 cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
00162 }
00163 else
00164 {
00165 newFrame = data.image().clone();
00166 }
00167
00168 if(memory_->getStMem().size() >= 1)
00169 {
00170 if(localMap_.size())
00171 {
00172
00173 UDEBUG("PnP");
00174
00175 if(this->isInfoDataFilled() && info)
00176 {
00177 info->type = 0;
00178 }
00179
00180
00181 if(memory_->update(SensorData(newFrame)))
00182 {
00183 UDEBUG("");
00184 bool newPtsAdded = false;
00185 const Signature * newS = memory_->getLastWorkingSignature();
00186 UDEBUG("newWords=%d", (int)newS->getWords().size());
00187 nFeatures = (int)newS->getWords().size();
00188 if((int)newS->getWords().size() > this->getMinInliers())
00189 {
00190 cv::Mat K = (cv::Mat_<double>(3,3) <<
00191 data.fx(), 0, data.cx(),
00192 0, data.fy()==0?data.fx():data.fy(), data.cy(),
00193 0, 0, 1);
00194 Transform guess = (this->getPose() * data.localTransform()).inverse();
00195 cv::Mat R = (cv::Mat_<double>(3,3) <<
00196 (double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
00197 (double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
00198 (double)guess.r31(), (double)guess.r32(), (double)guess.r33());
00199 cv::Mat rvec(1,3, CV_64FC1);
00200 cv::Rodrigues(R, rvec);
00201 cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
00202
00203 std::vector<cv::Point3f> objectPoints;
00204 std::vector<cv::Point2f> imagePoints;
00205 std::vector<int> matches;
00206
00207 UDEBUG("compute PnP from optical flow");
00208
00209 std::vector<int> ids = uKeys(localMap_);
00210 objectPoints = uValues(localMap_);
00211
00212
00213 UDEBUG("project points to previous image");
00214 std::vector<cv::Point2f> prevImagePoints;
00215 const Signature * prevS = memory_->getSignature(*(++memory_->getStMem().rbegin()));
00216 Transform prevGuess = (keyFramePoses_.at(prevS->id()) * data.localTransform()).inverse();
00217 cv::Mat prevR = (cv::Mat_<double>(3,3) <<
00218 (double)prevGuess.r11(), (double)prevGuess.r12(), (double)prevGuess.r13(),
00219 (double)prevGuess.r21(), (double)prevGuess.r22(), (double)prevGuess.r23(),
00220 (double)prevGuess.r31(), (double)prevGuess.r32(), (double)prevGuess.r33());
00221 cv::Mat prevRvec(1,3, CV_64FC1);
00222 cv::Rodrigues(prevR, prevRvec);
00223 cv::Mat prevTvec = (cv::Mat_<double>(1,3) << (double)prevGuess.x(), (double)prevGuess.y(), (double)prevGuess.z());
00224 cv::projectPoints(objectPoints, prevRvec, prevTvec, K, cv::Mat(), prevImagePoints);
00225
00226
00227 UDEBUG("project points to previous image");
00228 cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), imagePoints);
00229
00230
00231 std::vector<cv::Point3f> objectPointsTmp(objectPoints.size());
00232 std::vector<cv::Point2f> refCorners(objectPoints.size());
00233 std::vector<cv::Point2f> newCorners(objectPoints.size());
00234 matches.resize(objectPoints.size());
00235 int oi=0;
00236 for(unsigned int i=0; i<objectPoints.size(); ++i)
00237 {
00238 if(uIsInBounds(int(imagePoints[i].x), 0, newFrame.cols) &&
00239 uIsInBounds(int(imagePoints[i].y), 0, newFrame.rows) &&
00240 uIsInBounds(int(prevImagePoints[i].x), 0, prevS->getImageRaw().cols) &&
00241 uIsInBounds(int(prevImagePoints[i].y), 0, prevS->getImageRaw().rows))
00242 {
00243 refCorners[oi] = prevImagePoints[i];
00244 newCorners[oi] = imagePoints[i];
00245 if(localMap_.count(ids[i]) == 1)
00246 {
00247 if(prevS->getWords().count(ids[i]) == 1)
00248 {
00249
00250 refCorners[oi] = prevS->getWords().find(ids[i])->second.pt;
00251 }
00252 if(newS->getWords().count(ids[i]) == 1)
00253 {
00254
00255 newCorners[oi] = newS->getWords().find(ids[i])->second.pt;
00256 }
00257 }
00258 objectPointsTmp[oi] = objectPoints[i];
00259 matches[oi] = ids[i];
00260 ++oi;
00261 }
00262 }
00263 objectPointsTmp.resize(oi);
00264 refCorners.resize(oi);
00265 newCorners.resize(oi);
00266 matches.resize(oi);
00267
00268
00269 std::vector<unsigned char> statusFlowInliers;
00270 std::vector<float> err;
00271 UDEBUG("cv::calcOpticalFlowPyrLK() begin");
00272 cv::calcOpticalFlowPyrLK(
00273 prevS->getImageRaw(),
00274 newFrame,
00275 refCorners,
00276 newCorners,
00277 statusFlowInliers,
00278 err,
00279 cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
00280 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
00281 cv::OPTFLOW_LK_GET_MIN_EIGENVALS | cv::OPTFLOW_USE_INITIAL_FLOW, 1e-4);
00282 UDEBUG("cv::calcOpticalFlowPyrLK() end");
00283
00284 objectPoints.resize(statusFlowInliers.size());
00285 imagePoints.resize(statusFlowInliers.size());
00286 std::vector<int> matchesTmp(statusFlowInliers.size());
00287 oi = 0;
00288 for(unsigned int i=0; i<statusFlowInliers.size(); ++i)
00289 {
00290 if(statusFlowInliers[i])
00291 {
00292 objectPoints[oi] = objectPointsTmp[i];
00293 imagePoints[oi] = newCorners[i];
00294 matchesTmp[oi] = matches[i];
00295 ++oi;
00296
00297 if(this->isInfoDataFilled() && info)
00298 {
00299 cv::KeyPoint kpt;
00300 if(newS->getWords().count(matches[i]) == 1)
00301 {
00302 kpt = newS->getWords().find(matches[i])->second;
00303 }
00304 kpt.pt = newCorners[i];
00305 info->words.insert(std::make_pair(matches[i], kpt));
00306 }
00307 }
00308 }
00309 UDEBUG("Flow inliers= %d/%d", oi, (int)statusFlowInliers.size());
00310 objectPoints.resize(oi);
00311 imagePoints.resize(oi);
00312 matchesTmp.resize(oi);
00313 matches = matchesTmp;
00314
00315 if(this->isInfoDataFilled() && info)
00316 {
00317 info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end());
00318 }
00319 correspondences = (int)matches.size();
00320
00321 if((int)matches.size() < this->getMinInliers())
00322 {
00323 UWARN("not enough matches (%d < %d)...", (int)matches.size(), this->getMinInliers());
00324 }
00325 else
00326 {
00327
00328 std::vector<int> inliersV;
00329 cv::solvePnPRansac(
00330 objectPoints,
00331 imagePoints,
00332 K,
00333 cv::Mat(),
00334 rvec,
00335 tvec,
00336 true,
00337 this->getIterations(),
00338 this->getPnPReprojError(),
00339 0,
00340 inliersV,
00341 this->getPnPFlags());
00342
00343 UDEBUG("inliers=%d/%d", (int)inliersV.size(), (int)objectPoints.size());
00344
00345 inliers = (int)inliersV.size();
00346 if((int)inliersV.size() < this->getMinInliers())
00347 {
00348 UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
00349 }
00350 else
00351 {
00352 cv::Mat R(3,3,CV_64FC1);
00353 cv::Rodrigues(rvec, R);
00354 Transform pnp = Transform(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
00355 R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
00356 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
00357 output = this->getPose().inverse() * pnp.inverse() * data.localTransform().inverse();
00358
00359 if(this->isInfoDataFilled() && info && inliersV.size())
00360 {
00361 info->wordInliers.resize(inliersV.size());
00362 for(unsigned int i=0; i<inliersV.size(); ++i)
00363 {
00364 info->wordInliers[i] = matches[inliersV[i]];
00365 }
00366 }
00367
00368
00369 std::set<int> stMem = memory_->getStMem();
00370 stMem.erase(newS->id());
00371 std::map<int, float> likelihood = memory_->computeLikelihood(newS, std::list<int>(stMem.begin(), stMem.end()));
00372 int maxLikelihoodId = -1;
00373 float maxLikelihood = 0;
00374 for(std::map<int, float>::iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
00375 {
00376 if(iter->second > maxLikelihood)
00377 {
00378 maxLikelihood = iter->second;
00379 maxLikelihoodId = iter->first;
00380 }
00381 }
00382 UASSERT(maxLikelihoodId != -1);
00383
00384
00385 const Signature* previousS = memory_->getSignature(maxLikelihoodId);
00386 UASSERT(previousS!=0);
00387 Transform cameraTransform = keyFramePoses_.at(previousS->id()).inverse()*this->getPose()*output;
00388 UDEBUG("cameraTransform guess= %s (norm^2=%f)", cameraTransform.prettyPrint().c_str(), cameraTransform.getNormSquared());
00389 if(cameraTransform.getNorm() < minTranslation_)
00390 {
00391 UWARN("Translation with the nearest frame is too small (%f<%f) to add new points to local map",
00392 cameraTransform.getNorm(), minTranslation_);
00393 }
00394 else
00395 {
00396
00397 double variance = 0;
00398 const std::multimap<int, pcl::PointXYZ> & previousGuess = keyFrameWords3D_.find(previousS->id())->second;
00399 std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
00400 previousS->getWords(),
00401 newS->getWords(),
00402 data.fx(), data.fy()?data.fy():data.fx(),
00403 data.cx(), data.cy(),
00404 data.localTransform(),
00405 cameraTransform,
00406 this->getIterations(),
00407 this->getPnPReprojError(),
00408 this->getPnPFlags(),
00409 fundMatrixReprojError_,
00410 fundMatrixConfidence_,
00411 previousGuess,
00412 &variance);
00413
00414 if((int)inliers3D.size() < this->getMinInliers())
00415 {
00416 UWARN("Epipolar geometry not enough inliers (%d < %d), rejecting the transform (%s)...",
00417 (int)inliers3D.size(), this->getMinInliers(), cameraTransform.prettyPrint().c_str());
00418 }
00419 else if(variance == 0 || variance > maxVariance_)
00420 {
00421 UWARN("Variance too high %f (max = %f)", variance, maxVariance_);
00422 }
00423 else
00424 {
00425 UDEBUG("inliers3D=%d/%d variance= %f", inliers3D.size(), newS->getWords().size(), variance);
00426 Transform newPose = keyFramePoses_.at(previousS->id())*cameraTransform;
00427 UDEBUG("cameraTransform= %s", cameraTransform.prettyPrint().c_str());
00428
00429 std::multimap<int, cv::Point3f> wordsToAdd;
00430 for(std::multimap<int, pcl::PointXYZ>::iterator iter=inliers3D.begin();
00431 iter != inliers3D.end();
00432 ++iter)
00433 {
00434
00435 iter->second = util3d::transformPoint(iter->second, cameraTransform.inverse());
00436
00437 if(!uContains(localMap_, iter->first))
00438 {
00439
00440 pcl::PointXYZ newPt = util3d::transformPoint(iter->second, newPose);
00441 wordsToAdd.insert(std::make_pair(iter->first, cv::Point3f(newPt.x, newPt.y, newPt.z)));
00442 }
00443 }
00444
00445 if((int)wordsToAdd.size())
00446 {
00447 localMap_.insert(wordsToAdd.begin(), wordsToAdd.end());
00448 newPtsAdded = true;
00449 UDEBUG("Added %d words", (int)wordsToAdd.size());
00450 }
00451
00452 if(newPtsAdded)
00453 {
00454 keyFrameWords3D_.insert(std::make_pair(newS->id(), inliers3D));
00455 keyFramePoses_.insert(std::make_pair(newS->id(), newPose));
00456
00457
00458 while(localHistoryMaxSize_ && (int)localMap_.size() > localHistoryMaxSize_ && memory_->getStMem().size()>2)
00459 {
00460 int nodeId = *memory_->getStMem().begin();
00461 std::list<int> removedPts;
00462 memory_->deleteLocation(nodeId, &removedPts);
00463 keyFrameWords3D_.erase(nodeId);
00464 keyFramePoses_.erase(nodeId);
00465 for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
00466 {
00467 localMap_.erase(*iter);
00468 }
00469 }
00470 }
00471 }
00472 }
00473 }
00474 }
00475 }
00476
00477 if(!newPtsAdded)
00478 {
00479
00480 memory_->deleteLocation(newS->id());
00481 }
00482 }
00483 }
00484 else if(cornersMap_.size())
00485 {
00486
00487
00488 if(this->isInfoDataFilled() && info)
00489 {
00490 info->type = 1;
00491 }
00492
00493 const Signature * refS = memory_->getLastWorkingSignature();
00494
00495 std::vector<cv::Point2f> refCorners(cornersMap_.size());
00496 std::vector<cv::Point2f> refCornersGuess(cornersMap_.size());
00497 std::vector<int> cornerIds(cornersMap_.size());
00498 int ii=0;
00499 for(std::map<int, cv::Point2f>::iterator iter=cornersMap_.begin(); iter!=cornersMap_.end(); ++iter)
00500 {
00501 std::multimap<int, cv::KeyPoint>::const_iterator jter=refS->getWords().find(iter->first);
00502 UASSERT(jter != refS->getWords().end());
00503 refCorners[ii] = jter->second.pt;
00504 refCornersGuess[ii] = iter->second;
00505 cornerIds[ii] = iter->first;
00506 ++ii;
00507 }
00508
00509 UDEBUG("flow");
00510
00511 std::vector<unsigned char> statusFlowInliers;
00512 std::vector<float> err;
00513 UDEBUG("cv::calcOpticalFlowPyrLK() begin");
00514 cv::calcOpticalFlowPyrLK(
00515 refS->getImageRaw(),
00516 newFrame,
00517 refCorners,
00518 refCornersGuess,
00519 statusFlowInliers,
00520 err,
00521 cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
00522 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
00523 cv::OPTFLOW_LK_GET_MIN_EIGENVALS | cv::OPTFLOW_USE_INITIAL_FLOW, 1e-4);
00524 UDEBUG("cv::calcOpticalFlowPyrLK() end");
00525
00526 UDEBUG("Filtering optical flow outliers...");
00527 float flow = 0;
00528
00529 if(this->isInfoDataFilled() && info)
00530 {
00531 info->refCorners = refCorners;
00532 info->newCorners = refCornersGuess;
00533 }
00534
00535 int oi = 0;
00536 std::vector<cv::Point2f> tmpRefCorners(statusFlowInliers.size());
00537 std::vector<cv::Point2f> newCorners(statusFlowInliers.size());
00538 std::vector<int> inliersV(statusFlowInliers.size());
00539 std::vector<int> tmpCornersId(statusFlowInliers.size());
00540 UASSERT(refCornersGuess.size() == statusFlowInliers.size());
00541 UASSERT(refCorners.size() == statusFlowInliers.size());
00542 UASSERT(cornerIds.size() == statusFlowInliers.size());
00543 for(unsigned int i=0; i<statusFlowInliers.size(); ++i)
00544 {
00545 if(statusFlowInliers[i])
00546 {
00547 float dx = refCorners[i].x - refCornersGuess[i].x;
00548 float dy = refCorners[i].y - refCornersGuess[i].y;
00549 float tmp = std::sqrt(dx*dx + dy*dy);
00550 flow+=tmp;
00551
00552 tmpRefCorners[oi] = refCorners[i];
00553 newCorners[oi] = refCornersGuess[i];
00554
00555 inliersV[oi] = i;
00556 cornersMap_.at(cornerIds[i]) = refCornersGuess[i];
00557 tmpCornersId[oi] = cornerIds[i];
00558
00559 ++oi;
00560 }
00561 else
00562 {
00563 cornersMap_.erase(cornerIds[i]);
00564 }
00565 }
00566 if(oi)
00567 {
00568 flow /=float(oi);
00569 }
00570 tmpRefCorners.resize(oi);
00571 newCorners.resize(oi);
00572 inliersV.resize((oi));
00573 tmpCornersId.resize(oi);
00574 refCorners= tmpRefCorners;
00575 cornerIds = tmpCornersId;
00576
00577 if(this->isInfoDataFilled() && info)
00578 {
00579
00580 info->cornerInliers = inliersV;
00581 inliers = (int)inliersV.size();
00582 }
00583
00584 UDEBUG("Filtering optical flow outliers...done! (inliers=%d/%d)", oi, (int)statusFlowInliers.size());
00585
00586 if(flow > initMinFlow_ && oi > this->getMinInliers())
00587 {
00588 UDEBUG("flow=%f", flow);
00589
00590 UDEBUG("Find fundamental matrix");
00591 std::vector<unsigned char> statusFInliers;
00592 cv::Mat F = cv::findFundamentalMat(
00593 refCorners,
00594 newCorners,
00595 statusFInliers,
00596 cv::RANSAC,
00597 fundMatrixReprojError_,
00598 fundMatrixConfidence_);
00599 std::cout << "F=" << F << std::endl;
00600
00601 if(!F.empty())
00602 {
00603 UDEBUG("Filtering fundamental matrix outliers...");
00604 std::vector<cv::Point2f> tmpNewCorners(statusFInliers.size());
00605 std::vector<cv::Point2f> tmpRefCorners(statusFInliers.size());
00606 tmpCornersId.resize(statusFInliers.size());
00607 oi = 0;
00608 UASSERT(newCorners.size() == statusFInliers.size());
00609 UASSERT(refCorners.size() == statusFInliers.size());
00610 UASSERT(cornerIds.size() == statusFInliers.size());
00611 std::vector<int> tmpInliers(statusFInliers.size());
00612 for(unsigned int i=0; i<statusFInliers.size(); ++i)
00613 {
00614 if(statusFInliers[i])
00615 {
00616 tmpNewCorners[oi] = newCorners[i];
00617 tmpRefCorners[oi] = refCorners[i];
00618 tmpInliers[oi] = inliersV[i];
00619 tmpCornersId[oi] = cornerIds[i];
00620 ++oi;
00621 }
00622 }
00623 tmpInliers.resize(oi);
00624 tmpNewCorners.resize(oi);
00625 tmpRefCorners.resize(oi);
00626 tmpCornersId.resize(oi);
00627 newCorners = tmpNewCorners;
00628 refCorners = tmpRefCorners;
00629 inliersV = tmpInliers;
00630 cornerIds = tmpCornersId;
00631 if(this->isInfoDataFilled() && info)
00632 {
00633
00634 info->cornerInliers = inliersV;
00635 inliers = (int)inliersV.size();
00636 }
00637 UDEBUG("Filtering fundamental matrix outliers...done! (inliers=%d/%d)", oi, (int)statusFInliers.size());
00638
00639 if((int)refCorners.size() > this->getMinInliers())
00640 {
00641 std::vector<cv::Point2f> refCornersRefined;
00642 std::vector<cv::Point2f> newCornersRefined;
00643
00644 cv::correctMatches(F, refCorners, newCorners, refCornersRefined, newCornersRefined);
00645 UASSERT(refCorners.size() == refCornersRefined.size());
00646 UASSERT(newCorners.size() == newCornersRefined.size());
00647 refCorners = refCornersRefined;
00648 newCorners = newCornersRefined;
00649
00650
00651 UDEBUG("Computing P...");
00652 cv::Mat K = (cv::Mat_<double>(3,3) <<
00653 data.fx(), 0, data.cx(),
00654 0, data.fy()==0?data.fx():data.fy(), data.cy(),
00655 0, 0, 1);
00656
00657 cv::Mat Kinv = K.inv();
00658 cv::Mat E = K.t()*F*K;
00659
00660
00661 cv::Mat x(3, (int)refCorners.size(), CV_64FC1);
00662 cv::Mat xp(3, (int)refCorners.size(), CV_64FC1);
00663 for(unsigned int i=0; i<refCorners.size(); ++i)
00664 {
00665 x.at<double>(0, i) = refCorners[i].x;
00666 x.at<double>(1, i) = refCorners[i].y;
00667 x.at<double>(2, i) = 1;
00668
00669 xp.at<double>(0, i) = newCorners[i].x;
00670 xp.at<double>(1, i) = newCorners[i].y;
00671 xp.at<double>(2, i) = 1;
00672 }
00673
00674 cv::Mat x_norm = Kinv * x;
00675 cv::Mat xp_norm = Kinv * xp;
00676 x_norm = x_norm.rowRange(0,2);
00677 xp_norm = xp_norm.rowRange(0,2);
00678
00679 cv::Mat P = EpipolarGeometry::findPFromE(E, x_norm, xp_norm);
00680 if(!P.empty())
00681 {
00682 cv::Mat P0 = cv::Mat::zeros(3, 4, CV_64FC1);
00683 P0.at<double>(0,0) = 1;
00684 P0.at<double>(1,1) = 1;
00685 P0.at<double>(2,2) = 1;
00686
00687 UDEBUG("Computing P...done!");
00688 std::cout << "P=" << P << std::endl;
00689
00690 cv::Mat R, T;
00691 EpipolarGeometry::findRTFromP(P, R, T);
00692
00693 UDEBUG("");
00694 std::vector<double> reprojErrors;
00695 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
00696 EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
00697
00698 pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRef(new pcl::PointCloud<pcl::PointXYZ>);
00699 pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRefGuess(new pcl::PointCloud<pcl::PointXYZ>);
00700 std::vector<cv::Point2f> imagePoints(cloud->size());
00701 inliersRef->resize(cloud->size());
00702 inliersRefGuess->resize(cloud->size());
00703 tmpCornersId.resize(cloud->size());
00704
00705 oi = 0;
00706 UASSERT(newCorners.size() == cloud->size());
00707 for(unsigned int i=0; i<cloud->size(); ++i)
00708 {
00709 if(cloud->at(i).z>0)
00710 {
00711 imagePoints[oi] = newCorners[i];
00712 tmpCornersId[oi] = cornerIds[i];
00713 (*inliersRef)[oi] = cloud->at(i);
00714 if(!refDepth_.empty())
00715 {
00716 (*inliersRefGuess)[oi] = util3d::projectDepthTo3D(refDepth_, refCorners[i].x, refCorners[i].y, data.cx(), data.cy(), data.fx(), data.fy(), true);
00717 }
00718 ++oi;
00719 }
00720 }
00721 imagePoints.resize(oi);
00722 inliersRef->resize(oi);
00723 inliersRefGuess->resize(oi);
00724 tmpCornersId.resize(oi);
00725 cornerIds = tmpCornersId;
00726
00727 bool reject = false;
00728
00729
00730 float scale = 1;
00731 std::multimap<float, float> scales;
00732 if(!refDepth_.empty())
00733 {
00734 UASSERT(inliersRefGuess->size() == inliersRef->size());
00735 for(unsigned int i=0; i<inliersRef->size(); ++i)
00736 {
00737 if(pcl::isFinite(inliersRefGuess->at(i)))
00738 {
00739 float s = inliersRefGuess->at(i).z/inliersRef->at(i).z;
00740 std::vector<float> errorSqrdDists(inliersRef->size());
00741 for(unsigned int j=0; j<inliersRef->size(); ++j)
00742 {
00743 if(cloud->at(j).z>0)
00744 {
00745 pcl::PointXYZ refPt = inliersRef->at(j);
00746 refPt.x *= s;
00747 refPt.y *= s;
00748 refPt.z *= s;
00749 const pcl::PointXYZ & guess = inliersRefGuess->at(j);
00750 errorSqrdDists[j] = uNormSquared(refPt.x-guess.x, refPt.y-guess.y, refPt.z-guess.z);
00751 }
00752 }
00753 std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
00754 double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
00755 float variance = 2.1981 * median_error_sqr;
00756
00757 if(variance > 0)
00758 {
00759 scales.insert(std::make_pair(variance, s));
00760 }
00761 }
00762 }
00763 UASSERT(scales.size());
00764
00765 scale = scales.begin()->second;
00766 UDEBUG("scale used = %f (variance=%f)", scale, scales.begin()->first);
00767
00768 maxVariance_ = 0.01;
00769 UDEBUG("Max noise variance = %f current variance=%f", 0.01, scales.begin()->first);
00770 if(scales.begin()->first > 0.01)
00771 {
00772 UWARN("Too high variance %f (should be < 0.01)");
00773 reject = true;
00774 }
00775
00776 }
00777 else if(inliersRef->size())
00778 {
00779
00780 Eigen::Vector4f centroid;
00781 pcl::compute3DCentroid(*inliersRef, centroid);
00782 scale = 1.0f / centroid[2];
00783 maxVariance_ = 0.01;
00784 }
00785 else
00786 {
00787 reject = true;
00788 }
00789
00790 if(!reject)
00791 {
00792
00793 std::vector<cv::Point3f> objectPoints(inliersRef->size());
00794 for(unsigned int i=0; i<inliersRef->size(); ++i)
00795 {
00796 objectPoints[i].x = inliersRef->at(i).x * scale;
00797 objectPoints[i].y = inliersRef->at(i).y * scale;
00798 objectPoints[i].z = inliersRef->at(i).z * scale;
00799 }
00800 cv::Mat rvec;
00801 cv::Mat tvec;
00802 std::vector<int> inliersPnP;
00803 cv::solvePnPRansac(
00804 objectPoints,
00805 imagePoints,
00806 K,
00807 cv::Mat(),
00808 rvec,
00809 tvec,
00810 false,
00811 this->getIterations(),
00812 this->getPnPReprojError(),
00813 0,
00814 inliersPnP,
00815 this->getPnPFlags());
00816
00817 UDEBUG("PnP inliers = %d / %d", (int)inliersPnP.size(), (int)objectPoints.size());
00818
00819 cv::Rodrigues(rvec, R);
00820 Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
00821 R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
00822 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
00823
00824 output = data.localTransform() * pnp.inverse() * data.localTransform().inverse();
00825 if(output.getNorm() < minTranslation_*5)
00826 {
00827 reject = true;
00828 UWARN("Camera must be moved at least %f m for initialization (current=%f)",
00829 minTranslation_*5, output.getNorm());
00830 }
00831
00832 if(!reject)
00833 {
00835 std::vector<int> wordsId = uKeys(memory_->getLastWorkingSignature()->getWords());
00836 UASSERT(wordsId.size());
00837 UASSERT(cornerIds.size() == objectPoints.size());
00838 std::multimap<int, pcl::PointXYZ> keyFrameWords3D;
00839 for(unsigned int i=0; i<inliersPnP.size(); ++i)
00840 {
00841 int index =inliersPnP.at(i);
00842 int id = cornerIds[index];
00843 UASSERT(id > 0 && id <= *wordsId.rbegin());
00844 pcl::PointXYZ pt = util3d::transformPoint(pcl::PointXYZ(objectPoints.at(index).x, objectPoints.at(index).y, objectPoints.at(index).z), this->getPose()*data.localTransform());
00845 localMap_.insert(std::make_pair(id, cv::Point3f(pt.x, pt.y, pt.z)));
00846 keyFrameWords3D.insert(std::make_pair(id, pt));
00847 }
00848
00849 keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), keyFrameWords3D));
00850 keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), this->getPose()));
00851 }
00852 }
00853 }
00854 else
00855 {
00856 UERROR("No valid camera matrix found!");
00857 }
00858 }
00859 else
00860 {
00861 UWARN("Not enough inliers %d/%d", (int)refCorners.size(), this->getMinInliers());
00862 }
00863 }
00864 else
00865 {
00866 UWARN("Fundamental matrix not found!");
00867 }
00868 }
00869 else
00870 {
00871 UWARN("Flow not enough high! flow=%f ki=%d", flow, oi);
00872 }
00873 }
00874 }
00875 else
00876 {
00877
00878 output = Transform::getIdentity();
00879
00880
00881 if(memory_->update(SensorData(newFrame)))
00882 {
00883 const std::multimap<int, cv::KeyPoint> & words = memory_->getLastWorkingSignature()->getWords();
00884 if((int)words.size() > this->getMinInliers())
00885 {
00886 for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
00887 {
00888 cornersMap_.insert(std::make_pair(iter->first, iter->second.pt));
00889 }
00890 refDepth_ = data.depth().clone();
00891 keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity()));
00892 }
00893 else
00894 {
00895 UWARN("Too low 2D corners (%d), ignoring new frame...",
00896 (int)words.size());
00897 memory_->deleteLocation(memory_->getLastSignatureId());
00898 }
00899 }
00900 else
00901 {
00902 UERROR("Failed creating signature");
00903 }
00904 }
00905
00906 memory_->emptyTrash();
00907
00908 if(this->isInfoDataFilled() && info)
00909 {
00910
00911 info->inliers = inliers;
00912 info->matches = correspondences;
00913 info->features = nFeatures;
00914 info->localMapSize = (int)localMap_.size();
00915 info->localMap = localMap_;
00916 }
00917
00918 UINFO("Odom update=%fs tf=[%s] inliers=%d/%d, local_map[%d]=%d, accepted=%s",
00919 timer.elapsed(),
00920 output.prettyPrint().c_str(),
00921 inliers,
00922 correspondences,
00923 (int)memory_->getStMem().size(),
00924 (int)localMap_.size(),
00925 !output.isNull()?"true":"false");
00926
00927 return output;
00928 }
00929
00930 }