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