OdometryMono.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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         // Setup memory
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")); // desactivate rehearsal
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         // add only feature stuff
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         // convert to grayscale
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                         //PnP
00214                         UDEBUG("PnP");
00215 
00216                         if(this->isInfoDataFilled() && info)
00217                         {
00218                                 info->type = 0;
00219                         }
00220 
00221                         // generate kpts
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                                         // compute last projection
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                                         // compute current projection
00265                                         UDEBUG("project points to previous image");
00266                                         cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), imagePoints);
00267 
00268                                         //filter points not in the image and set guess from unique correspondences
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                                                                         // set guess if unique
00288                                                                         refCorners[oi] = prevS->getWords().find(ids[i])->second.pt;
00289                                                                 }
00290                                                                 if(newS->getWords().count(ids[i]) == 1)
00291                                                                 {
00292                                                                         // set guess if unique
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                                         // Refine imagePoints using optical flow
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                                                 //PnPRansac
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, // min inliers
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]]; // index and ID should match (index starts at 0, ID starts at 1)
00404                                                                 }
00405                                                         }
00406 
00407                                                         //Find the frame with the most similar features
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                                                         // Add new points to local map
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                                                                                 // transform inliers3D in new signature referential
00473                                                                                 iter->second = util3d::transformPoint(iter->second, cameraTransform.inverse());
00474 
00475                                                                                 if(!uContains(localMap_, iter->first))
00476                                                                                 {
00477                                                                                         //UDEBUG("Add new point %d to local map", iter->first);
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                                                                                 // keep only the two last signatures
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                                         // remove new words from dictionary
00518                                         memory_->deleteLocation(newS->id());
00519                                 }
00520                         }
00521                 }
00522                 else if(cornersMap_.size())
00523                 {
00524                         //flow
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                         // Find features in the new left image
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                                 // fill flow matches info
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                                 // compute fundamental matrix
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                                 //std::cout << "F=" << F << std::endl;
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                                                 // update inliers
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                                                 //UDEBUG("Correcting matches...");
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                                                 //UDEBUG("Correcting matches...done!");
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                                                 //normalize coordinates
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                                                         //std::cout << "P=" << P << std::endl;
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                                                         //estimate scale
00806                                                         float scale = 1;
00807                                                         std::multimap<float, float> scales; // <variance, scale>
00808                                                         if(!newCorners3D.empty()) // scale known
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                                                                                         //UDEBUG("scale %d = %f variance = %f", i, s, variance);
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; // 20 cm for good initialization
00859                                                                         }
00860                                                                 }
00861 
00862                                                         }
00863                                                         else if(inliersRef.size())
00864                                                         {
00865                                                                 // find centroid of the cloud and set it to 1 meter
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                                                                 //PnPRansac
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, // 3D points in ref referential
00898                                                                                 imagePoints,  // 2D points in new referential
00899                                                                                 K,
00900                                                                                 cv::Mat(),
00901                                                                                 rvec,
00902                                                                                 tvec,
00903                                                                                 false,
00904                                                                                 iterations_,
00905                                                                                 pnpReprojError_,
00906                                                                                 0, // min inliers
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                 //return Identity
00975                 output = Transform::getIdentity();
00976                 if(info)
00977                 {
00978                         // a very high variance tells that the new pose is not linked with the previous one
00979                         info->reg.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
00980                 }
00981 
00982                         // generate kpts
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                 //info->variance = variance;
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 } // namespace rtabmap


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:21