rtabmap: OdometryF2M.cpp Source File

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/OdometryInfo.h"
00029 #include "rtabmap/core/Memory.h"
00030 #include "rtabmap/core/VisualWord.h"
00031 #include "rtabmap/core/Signature.h"
00032 #include "rtabmap/core/RegistrationVis.h"
00033 #include "rtabmap/core/util3d_transforms.h"
00034 #include "rtabmap/core/util3d_registration.h"
00035 #include "rtabmap/core/util3d_correspondences.h"
00036 #include "rtabmap/core/util3d_motion_estimation.h"
00037 #include "rtabmap/core/util3d_filtering.h"
00038 #include "rtabmap/core/Optimizer.h"
00039 #include "rtabmap/core/VWDictionary.h"
00040 #include "rtabmap/core/util3d.h"
00041 #include "rtabmap/core/Graph.h"
00042 #include "rtflann/flann.hpp"
00043 #include "rtabmap/utilite/ULogger.h"
00044 #include "rtabmap/utilite/UTimer.h"
00045 #include "rtabmap/utilite/UMath.h"
00046 #include "rtabmap/utilite/UConversion.h"
00047 #include <opencv2/calib3d/calib3d.hpp>
00048 #include <rtabmap/core/OdometryF2M.h>
00049 #include <pcl/common/io.h>
00050 
00051 #if _MSC_VER
00052         #define ISFINITE(value) _finite(value)
00053 #else
00054         #define ISFINITE(value) std::isfinite(value)
00055 #endif
00056 
00057 namespace rtabmap {
00058 
00059 OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
00060         Odometry(parameters),
00061         maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
00062         keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
00063         visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()),
00064         maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
00065         scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
00066         scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
00067         scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
00068         scanSubtractAngle_(Parameters::defaultOdomF2MScanSubtractAngle()),
00069         bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
00070         bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
00071         map_(new Signature(-1)),
00072         lastFrame_(new Signature(1)),
00073         lastFrameOldestNewId_(0),
00074         bundleSeq_(0),
00075         sba_(0)
00076 {
00077         UDEBUG("");
00078         Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
00079         Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
00080         Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_);
00081         Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
00082         Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
00083         Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
00084         Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
00085         if(Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractAngle(), scanSubtractAngle_))
00086         {
00087                 scanSubtractAngle_ *= M_PI/180.0f;
00088         }
00089         Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
00090         Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
00091         UASSERT(bundleMaxFrames_ >= 0);
00092         ParametersMap bundleParameters = parameters;
00093         if(bundleAdjustment_ > 0)
00094         {
00095                 if((bundleAdjustment_==1 && Optimizer::isAvailable(Optimizer::kTypeG2O)) ||
00096                    (bundleAdjustment_==2 && Optimizer::isAvailable(Optimizer::kTypeCVSBA)))
00097                 {
00098                         // disable bundle in RegistrationVis as we do it already here
00099                         uInsert(bundleParameters, ParametersPair(Parameters::kVisBundleAdjustment(), "0"));
00100                         sba_ = Optimizer::create(bundleAdjustment_==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, bundleParameters);
00101                 }
00102                 else
00103                 {
00104                         UWARN("Selected bundle adjustment approach (\"%s\"=\"%d\") is not available, "
00105                                         "local bundle adjustment is then disabled.", Parameters::kOdomF2MBundleAdjustment().c_str(), bundleAdjustment_);
00106                         bundleAdjustment_ = 0;
00107                 }
00108         }
00109         UASSERT(maximumMapSize_ >= 0);
00110         UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
00111         UASSERT(visKeyFrameThr_>=0);
00112         UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
00113         UASSERT(maxNewFeatures_ >= 0);
00114 
00115         int corType = Parameters::defaultVisCorType();
00116         Parameters::parse(parameters, Parameters::kVisCorType(), corType);
00117         if(corType != 0)
00118         {
00119                 UWARN("%s=%d is not supported by OdometryF2M, using Features matching approach instead (type=0).",
00120                                 Parameters::kVisCorType().c_str(),
00121                                 corType);
00122                 corType = 0;
00123         }
00124         uInsert(bundleParameters, ParametersPair(Parameters::kVisCorType(), uNumber2Str(corType)));
00125 
00126         regPipeline_ = Registration::create(bundleParameters);
00127         if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
00128         {
00129                 UWARN("%s=%d cannot be used with registration not done only with images (%s=%s), disabling bundle adjustment.",
00130                                 Parameters::kOdomF2MBundleAdjustment().c_str(),
00131                                 bundleAdjustment_,
00132                                 Parameters::kRegStrategy().c_str(),
00133                                 uValue(bundleParameters, Parameters::kRegStrategy(), uNumber2Str(Parameters::defaultRegStrategy())).c_str());
00134                 bundleAdjustment_ = 0;
00135         }
00136 
00137         parameters_ = bundleParameters;
00138 }
00139 
00140 OdometryF2M::~OdometryF2M()
00141 {
00142         delete map_;
00143         delete lastFrame_;
00144         scansBuffer_.clear();
00145         bundleWordReferences_.clear();
00146         bundlePoses_.clear();
00147         bundleLinks_.clear();
00148         bundleModels_.clear();
00149         bundlePoseReferences_.clear();
00150         delete sba_;
00151         delete regPipeline_;
00152         UDEBUG("");
00153 }
00154 
00155 
00156 void OdometryF2M::reset(const Transform & initialPose)
00157 {
00158         Odometry::reset(initialPose);
00159         *lastFrame_ = Signature(1);
00160         *map_ = Signature(-1);
00161         scansBuffer_.clear();
00162         bundleWordReferences_.clear();
00163         bundlePoses_.clear();
00164         bundleLinks_.clear();
00165         bundleModels_.clear();
00166         bundlePoseReferences_.clear();
00167         bundleSeq_ = 0;
00168         lastFrameOldestNewId_ = 0;
00169 }
00170 
00171 // return not null transform if odometry is correctly computed
00172 Transform OdometryF2M::computeTransform(
00173                 SensorData & data,
00174                 const Transform & guess,
00175                 OdometryInfo * info)
00176 {
00177         UTimer timer;
00178         Transform output;
00179 
00180         if(info)
00181         {
00182                 info->type = 0;
00183         }
00184 
00185         RegistrationInfo regInfo;
00186         int nFeatures = 0;
00187 
00188         delete lastFrame_;
00189         int id = data.id();
00190         data.setId(++bundleSeq_); // generate our own unique ids, to make sure they are correctly set
00191         lastFrame_ = new Signature(data);
00192         data.setId(id);
00193 
00194         if(bundleAdjustment_ > 0 &&
00195            data.cameraModels().size() > 1)
00196         {
00197                 UERROR("Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.");
00198                 bundleAdjustment_ = 0;
00199         }
00200         bool addKeyFrame = false;
00201         int totalBundleWordReferencesUsed = 0;
00202         int totalBundleOutliers = 0;
00203         float bundleTime = 0.0f;
00204 
00205         // Generate keypoints from the new data
00206         if(lastFrame_->sensorData().isValid())
00207         {
00208                 if((map_->getWords3().size() || !map_->sensorData().laserScanRaw().isEmpty()) &&
00209                         lastFrame_->sensorData().isValid())
00210                 {
00211                         Signature tmpMap;
00212                         Transform transform;
00213                         UDEBUG("guess=%s frames=%d image required=%d", guess.prettyPrint().c_str(), this->framesProcessed(), regPipeline_->isImageRequired()?1:0);
00214 
00215                         // bundle adjustment stuff if used
00216                         std::map<int, cv::Point3f> points3DMap;
00217                         std::map<int, Transform> bundlePoses;
00218                         std::multimap<int, Link> bundleLinks;
00219                         std::map<int, CameraModel> bundleModels;
00220                         std::map<int, StereoCameraModel> bundleStereoModels;
00221 
00222                         for(int guessIteration=0;
00223                                         guessIteration<(!guess.isNull()&&regPipeline_->isImageRequired()?2:1) && transform.isNull();
00224                                         ++guessIteration)
00225                         {
00226                                 tmpMap = *map_;
00227                                 // reset matches, but keep already extracted features in lastFrame_->sensorData()
00228                                 lastFrame_->setWords(std::multimap<int, cv::KeyPoint>());
00229                                 lastFrame_->setWords3(std::multimap<int, cv::Point3f>());
00230                                 lastFrame_->setWordsDescriptors(std::multimap<int, cv::Mat>());
00231 
00232                                 points3DMap.clear();
00233                                 bundlePoses.clear();
00234                                 bundleLinks.clear();
00235                                 bundleModels.clear();
00236                                 bundleStereoModels.clear();
00237 
00238                                 float maxCorrespondenceDistance = 0.0f;
00239                                 float pmOutlierRatio = 0.0f;
00240                                 if(guess.isNull() &&
00241                                         !regPipeline_->isImageRequired() &&
00242                                         regPipeline_->isScanRequired() &&
00243                                         this->framesProcessed() < 2)
00244                                 {
00245                                         // only on initialization (first frame to register), increase icp max correspondences in case the robot is already moving
00246                                         maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
00247                                         pmOutlierRatio = Parameters::defaultIcpPMOutlierRatio();
00248                                         Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
00249                                         Parameters::parse(parameters_, Parameters::kIcpPMOutlierRatio(), pmOutlierRatio);
00250                                         ParametersMap params;
00251                                         params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
00252                                         params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(0.95f)));
00253                                         regPipeline_->parseParameters(params);
00254                                 }
00255 
00256                                 if(guessIteration == 1)
00257                                 {
00258                                         UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
00259                                 }
00260 
00261                                 transform = regPipeline_->computeTransformationMod(
00262                                                 tmpMap,
00263                                                 *lastFrame_,
00264                                                 // special case for ICP-only odom, set guess to identity if we just started or reset
00265                                                 guessIteration==0 && !guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(),
00266                                                 &regInfo);
00267 
00268                                 if(maxCorrespondenceDistance>0.0f)
00269                                 {
00270                                         // set it back
00271                                         ParametersMap params;
00272                                         params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance)));
00273                                         params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(pmOutlierRatio)));
00274                                         regPipeline_->parseParameters(params);
00275                                 }
00276 
00277                                 data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
00278 
00279                                 UDEBUG("Registration time = %fs", regInfo.totalTime);
00280                                 if(!transform.isNull())
00281                                 {
00282                                         // local bundle adjustment
00283                                         if(bundleAdjustment_>0 && sba_ &&
00284                                            regPipeline_->isImageRequired() &&
00285                                            lastFrame_->sensorData().cameraModels().size() <= 1 && // multi-cameras not supported
00286                                            regInfo.inliersIDs.size())
00287                                         {
00288                                                 UDEBUG("Local Bundle Adjustment");
00289 
00290                                                 // make sure the IDs of words in the map are not modified (Optical Flow Registration issue)
00291                                                 UASSERT(map_->getWords().size() && tmpMap.getWords().size());
00292                                                 if(map_->getWords().size() != tmpMap.getWords().size() ||
00293                                                    map_->getWords().begin()->first != tmpMap.getWords().begin()->first ||
00294                                                    map_->getWords().rbegin()->first != tmpMap.getWords().rbegin()->first)
00295                                                 {
00296                                                         UERROR("Bundle Adjustment cannot be used with a registration approach recomputing features from the \"from\" signature (e.g., Optical Flow).");
00297                                                         bundleAdjustment_ = 0;
00298                                                 }
00299                                                 else
00300                                                 {
00301                                                         UASSERT(bundlePoses_.size());
00302                                                         UASSERT_MSG(bundlePoses_.size()-1 == bundleLinks_.size(), uFormat("poses=%d links=%d", (int)bundlePoses_.size(), (int)bundleLinks_.size()).c_str());
00303                                                         UASSERT(bundlePoses_.size() == bundleModels_.size());
00304 
00305                                                         bundlePoses = bundlePoses_;
00306                                                         bundleLinks = bundleLinks_;
00307                                                         bundleModels = bundleModels_;
00308 
00309                                                         UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
00310                                                                         uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
00311                                                         cv::Mat var = regInfo.covariance;//cv::Mat::eye(6,6,CV_64FC1); //regInfo.covariance.inv()
00312                                                         //var(cv::Range(0,3), cv::Range(0,3)) *= 0.001;
00313                                                         //var(cv::Range(3,6), cv::Range(3,6)) *= 0.001;
00314                                                         bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
00315                                                         bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
00316 
00317                                                         CameraModel model;
00318                                                         if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
00319                                                         {
00320                                                                 model = lastFrame_->sensorData().cameraModels()[0];
00321                                                         }
00322                                                         else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
00323                                                         {
00324                                                                 model = lastFrame_->sensorData().stereoCameraModel().left();
00325                                                                 // Set Tx for stereo BA
00326                                                                 model = CameraModel(model.fx(),
00327                                                                                 model.fy(),
00328                                                                                 model.cx(),
00329                                                                                 model.cy(),
00330                                                                                 model.localTransform(),
00331                                                                                 -lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
00332                                                         }
00333                                                         else
00334                                                         {
00335                                                                 UFATAL("no valid camera model to do odometry bundle adjustment!");
00336                                                         }
00337                                                         bundleModels.insert(std::make_pair(lastFrame_->id(), model));
00338                                                         Transform invLocalTransform = model.localTransform().inverse();
00339 
00340                                                         UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
00341                                                         std::map<int, std::map<int, cv::Point3f> > wordReferences;
00342                                                         for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
00343                                                         {
00344                                                                 int wordId =regInfo.inliersIDs[i];
00345 
00346                                                                 // 3D point
00347                                                                 std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(wordId);
00348                                                                 UASSERT(iter3D!=tmpMap.getWords3().end());
00349                                                                 points3DMap.insert(*iter3D);
00350 
00351                                                                 std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
00352 
00353                                                                 // all other references
00354                                                                 std::map<int, std::map<int, cv::Point3f> >::iterator refIter = bundleWordReferences_.find(wordId);
00355                                                                 UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
00356 
00357                                                                 std::map<int, cv::Point3f> references;
00358                                                                 int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
00359                                                                 if(step == 0)
00360                                                                 {
00361                                                                         step = 1;
00362                                                                 }
00363                                                                 int oi=0;
00364                                                                 for(std::map<int, cv::Point3f>::iterator jter=refIter->second.begin(); jter!=refIter->second.end(); ++jter)
00365                                                                 {
00366                                                                         if(oi++ % step == 0 && bundlePoses.find(jter->first)!=bundlePoses.end())
00367                                                                         {
00368                                                                                 references.insert(*jter);
00369                                                                                 ++totalBundleWordReferencesUsed;
00370                                                                         }
00371                                                                 }
00372                                                                 //make sure the last reference is here
00373                                                                 if(refIter->second.size() > 1)
00374                                                                 {
00375                                                                         if(references.insert(*refIter->second.rbegin()).second)
00376                                                                         {
00377                                                                                 ++totalBundleWordReferencesUsed;
00378                                                                         }
00379                                                                 }
00380 
00381                                                                 if(iter2D!=lastFrame_->getWords().end())
00382                                                                 {
00383                                                                         UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
00384                                                                         //move back point in camera frame (to get depth along z)
00385                                                                         cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform);
00386                                                                         references.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
00387                                                                 }
00388                                                                 wordReferences.insert(std::make_pair(wordId, references));
00389 
00390                                                                 //UDEBUG("%d (%f,%f,%f)", iter3D->first, iter3D->second.x, iter3D->second.y, iter3D->second.z);
00391                                                                 //for(std::map<int, cv::Point2f>::iterator iter=inserted.first->second.begin(); iter!=inserted.first->second.end(); ++iter)
00392                                                                 //{
00393                                                                 //      UDEBUG("%d (%f,%f)", iter->first, iter->second.x, iter->second.y);
00394                                                                 //}
00395                                                         }
00396 
00397                                                         UDEBUG("sba...start");
00398                                                         // set root negative to fix all other poses
00399                                                         std::set<int> sbaOutliers;
00400                                                         UTimer bundleTimer;
00401                                                         bundlePoses = sba_->optimizeBA(-lastFrame_->id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
00402                                                         bundleTime = bundleTimer.ticks();
00403                                                         UDEBUG("sba...end");
00404                                                         totalBundleOutliers = (int)sbaOutliers.size();
00405 
00406                                                         UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), (int)sbaOutliers.size());
00407                                                         if(info)
00408                                                         {
00409                                                                 info->localBundlePoses = bundlePoses;
00410                                                                 info->localBundleModels = bundleModels;
00411                                                         }
00412 
00413                                                         UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
00414                                                         if(bundlePoses.size() == bundlePoses_.size()+1)
00415                                                         {
00416                                                                 if(!bundlePoses.rbegin()->second.isNull())
00417                                                                 {
00418                                                                         if(sbaOutliers.size())
00419                                                                         {
00420                                                                                 std::vector<int> newInliers(regInfo.inliersIDs.size());
00421                                                                                 int oi=0;
00422                                                                                 for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
00423                                                                                 {
00424                                                                                         if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
00425                                                                                         {
00426                                                                                                 newInliers[oi++] = regInfo.inliersIDs[i];
00427                                                                                         }
00428                                                                                 }
00429                                                                                 newInliers.resize(oi);
00430                                                                                 UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(regInfo.inliersIDs.size()));
00431                                                                                 regInfo.inliers = (int)newInliers.size();
00432                                                                                 regInfo.inliersIDs = newInliers;
00433                                                                         }
00434                                                                         if(regInfo.inliers < regPipeline_->getMinVisualCorrespondences())
00435                                                                         {
00436                                                                                 regInfo.rejectedMsg = uFormat("Too low inliers after bundle adjustment: %d<%d", regInfo.inliers, regPipeline_->getMinVisualCorrespondences());
00437                                                                                 transform.setNull();
00438                                                                         }
00439                                                                         else
00440                                                                         {
00441                                                                                 transform = bundlePoses.rbegin()->second;
00442                                                                                 bundleLinks.find(bundlePoses_.rbegin()->first)->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
00443                                                                         }
00444                                                                 }
00445                                                                 UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
00446                                                         }
00447                                                         else
00448                                                         {
00449                                                                 UWARN("Local bundle adjustment failed! transform is not refined.");
00450                                                         }
00451                                                 }
00452                                         }
00453 
00454                                         if(!transform.isNull())
00455                                         {
00456                                                 // make it incremental
00457                                                 transform = this->getPose().inverse() * transform;
00458                                         }
00459                                 }
00460 
00461                                 if(transform.isNull())
00462                                 {
00463                                         if(guessIteration == 1)
00464                                         {
00465                                                 UWARN("Trial with no guess still fail.");
00466                                         }
00467                                         if(!regInfo.rejectedMsg.empty())
00468                                         {
00469                                                 UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
00470                                         }
00471                                         else
00472                                         {
00473                                                 UWARN("Unknown registration error");
00474                                         }
00475                                 }
00476                                 else if(guessIteration == 1)
00477                                 {
00478                                         UWARN("Trial with no guess succeeded!");
00479                                 }
00480                         }
00481 
00482                         if(!transform.isNull())
00483                         {
00484                                 output = transform;
00485 
00486                                 bool modified = false;
00487                                 Transform newFramePose = this->getPose()*output;
00488 
00489                                 // fields to update
00490                                 LaserScan mapScan = tmpMap.sensorData().laserScanRaw();
00491                                 std::multimap<int, cv::KeyPoint> mapWords = tmpMap.getWords();
00492                                 std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
00493                                 std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
00494 
00495                                 bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
00496                                                  (keyFrameThr_ == 0.0f ||
00497                                                   visKeyFrameThr_ == 0 ||
00498                                                   float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
00499                                                   regInfo.inliers <= visKeyFrameThr_);
00500                                 bool addGeometricKeyFrame = regPipeline_->isScanRequired() && (scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
00501 
00502                                 addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
00503                                 addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
00504 
00505                                 UDEBUG("keyframeThr=%f visKeyFrameThr_=%d matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, visKeyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size());
00506                                 if(addKeyFrame)
00507                                 {
00508                                         //Visual
00509                                         int added = 0;
00510                                         int removed = 0;
00511                                         UTimer tmpTimer;
00512 
00513                                         UDEBUG("Update local map");
00514 
00515                                         // update local map
00516                                         UASSERT(mapWords.size() == mapPoints.size());
00517                                         UASSERT(mapPoints.size() == mapDescriptors.size());
00518                                         UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
00519 
00520                                         std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
00521                                         if(bundleAdjustment_>0)
00522                                         {
00523                                                 bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
00524                                                 UASSERT(graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false) != bundleLinks.end());
00525                                                 bundleLinks_.insert(*bundleLinks.find(bundlePoses_.rbegin()->first));
00526                                                 uInsert(bundlePoses_, bundlePoses);
00527                                                 UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
00528                                                 bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
00529                                                 iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
00530 
00531                                                 // update local map 3D points (if bundle adjustment was done)
00532                                                 for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
00533                                                 {
00534                                                         UASSERT(mapPoints.count(iter->first) == 1);
00535                                                         //UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints.find(origin)->second.x, mapPoints.find(origin)->second.y, mapPoints.find(origin)->second.z, iter->second.x, iter->second.y, iter->second.z);
00536                                                         mapPoints.find(iter->first)->second = iter->second;
00537                                                 }
00538                                         }
00539 
00540                                         // sort by feature response
00541                                         std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
00542                                         UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
00543                                         std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
00544                                         std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
00545                                         UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
00546                                         std::set<int> seenStatusUpdated;
00547                                         Transform invLocalTransform;
00548                                         if(bundleAdjustment_>0)
00549                                         {
00550                                                 if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
00551                                                 {
00552                                                         invLocalTransform = lastFrame_->sensorData().cameraModels()[0].localTransform().inverse();
00553                                                 }
00554                                                 else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
00555                                                 {
00556                                                         invLocalTransform = lastFrame_->sensorData().stereoCameraModel().left().localTransform().inverse();
00557                                                 }
00558                                                 else
00559                                                 {
00560                                                         UFATAL("no valid camera model!");
00561                                                 }
00562                                         }
00563                                         for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
00564                                         {
00565                                                 if(util3d::isFinite(iter->second))
00566                                                 {
00567                                                         if(mapPoints.find(iter->first) == mapPoints.end()) // Point not in map
00568                                                         {
00569                                                                 newIds.insert(
00570                                                                                 std::make_pair(iter2D->second.response>0?1.0f/iter2D->second.response:0.0f,
00571                                                                                                 std::make_pair(iter->first,
00572                                                                                                                 std::make_pair(iter2D->second,
00573                                                                                                                                 std::make_pair(iter->second, iterDesc->second)))));
00574                                                         }
00575                                                         else if(bundleAdjustment_>0)
00576                                                         {
00577                                                                 if(lastFrame_->getWords().count(iter->first) == 1)
00578                                                                 {
00579                                                                         std::multimap<int, cv::KeyPoint>::iterator iterKpts = mapWords.find(iter->first);
00580                                                                         if(iterKpts!=mapWords.end())
00581                                                                         {
00582                                                                                 iterKpts->second.octave = iter2D->second.octave;
00583                                                                         }
00584 
00585                                                                         UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
00586                                                                         iterBundlePosesRef->second += 1;
00587 
00588                                                                         //move back point in camera frame (to get depth along z)
00589                                                                         cv::Point3f pt3d = util3d::transformPoint(iter->second, invLocalTransform);
00590                                                                         if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
00591                                                                         {
00592                                                                                 std::map<int, cv::Point3f> framePt;
00593                                                                                 framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
00594                                                                                 bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
00595                                                                         }
00596                                                                         else
00597                                                                         {
00598                                                                                 bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
00599                                                                         }
00600                                                                 }
00601                                                         }
00602                                                 }
00603                                         }
00604                                         UDEBUG("newIds=%d", (int)newIds.size());
00605 
00606                                         int lastFrameOldestNewId = lastFrameOldestNewId_;
00607                                         lastFrameOldestNewId_ = lastFrame_->getWords().size()?lastFrame_->getWords().rbegin()->first:0;
00608                                         for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::reverse_iterator iter=newIds.rbegin();
00609                                                 iter!=newIds.rend();
00610                                                 ++iter)
00611                                         {
00612                                                 if(maxNewFeatures_ == 0  || added < maxNewFeatures_)
00613                                                 {
00614                                                         if(bundleAdjustment_>0)
00615                                                         {
00616                                                                 if(lastFrame_->getWords().count(iter->second.first) == 1)
00617                                                                 {
00618                                                                         UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
00619                                                                         iterBundlePosesRef->second += 1;
00620 
00621                                                                         //move back point in camera frame (to get depth along z)
00622                                                                         cv::Point3f pt3d = util3d::transformPoint(iter->second.second.second.first, invLocalTransform);
00623                                                                         if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
00624                                                                         {
00625                                                                                 std::map<int, cv::Point3f> framePt;
00626                                                                                 framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, pt3d.z)));
00627                                                                                 bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
00628                                                                         }
00629                                                                         else
00630                                                                         {
00631                                                                                 bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, pt3d.z)));
00632                                                                         }
00633                                                                 }
00634                                                         }
00635 
00636                                                         mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
00637                                                         mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second.second.first, newFramePose)));
00638                                                         mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
00639                                                         if(lastFrameOldestNewId_ > iter->second.first)
00640                                                         {
00641                                                                 lastFrameOldestNewId_ = iter->second.first;
00642                                                         }
00643                                                         ++added;
00644                                                 }
00645                                         }
00646 
00647                                         // remove words in map if max size is reached
00648                                         if((int)mapPoints.size() > maximumMapSize_)
00649                                         {
00650                                                 // remove oldest outliers first
00651                                                 std::set<int> inliers(regInfo.inliersIDs.begin(), regInfo.inliersIDs.end());
00652                                                 std::vector<int> ids = regInfo.matchesIDs;
00653                                                 if(regInfo.projectedIDs.size())
00654                                                 {
00655                                                         ids.resize(ids.size() + regInfo.projectedIDs.size());
00656                                                         int oi=0;
00657                                                         for(unsigned int i=0; i<regInfo.projectedIDs.size(); ++i)
00658                                                         {
00659                                                                 if(regInfo.projectedIDs[i]>=lastFrameOldestNewId)
00660                                                                 {
00661                                                                         ids[regInfo.matchesIDs.size()+oi++] = regInfo.projectedIDs[i];
00662                                                                 }
00663                                                         }
00664                                                         ids.resize(regInfo.matchesIDs.size()+oi);
00665                                                         UDEBUG("projected added=%d/%d minLastFrameId=%d", oi, (int)regInfo.projectedIDs.size(), lastFrameOldestNewId);
00666                                                 }
00667                                                 for(unsigned int i=0; i<ids.size() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size(); ++i)
00668                                                 {
00669                                                         int id = ids.at(i);
00670                                                         if(inliers.find(id) == inliers.end())
00671                                                         {
00672                                                                 std::map<int, std::map<int, cv::Point3f> >::iterator iterRef = bundleWordReferences_.find(id);
00673                                                                 if(iterRef != bundleWordReferences_.end())
00674                                                                 {
00675                                                                         for(std::map<int, cv::Point3f>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
00676                                                                         {
00677                                                                                 if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
00678                                                                                 {
00679                                                                                         bundlePoseReferences_.at(iterFrame->first) -= 1;
00680                                                                                 }
00681                                                                         }
00682                                                                         bundleWordReferences_.erase(iterRef);
00683                                                                 }
00684 
00685                                                                 mapPoints.erase(id);
00686                                                                 mapDescriptors.erase(id);
00687                                                                 mapWords.erase(id);
00688                                                                 ++removed;
00689                                                         }
00690                                                 }
00691 
00692                                                 // remove oldest first
00693                                                 std::multimap<int, cv::Mat>::iterator iterMapDescriptors = mapDescriptors.begin();
00694                                                 std::multimap<int, cv::KeyPoint>::iterator iterMapWords = mapWords.begin();
00695                                                 for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin();
00696                                                         iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();)
00697                                                 {
00698                                                         if(inliers.find(iter->first) == inliers.end())
00699                                                         {
00700                                                                 std::map<int, std::map<int, cv::Point3f> >::iterator iterRef = bundleWordReferences_.find(iter->first);
00701                                                                 if(iterRef != bundleWordReferences_.end())
00702                                                                 {
00703                                                                         for(std::map<int, cv::Point3f>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
00704                                                                         {
00705                                                                                 if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
00706                                                                                 {
00707                                                                                         bundlePoseReferences_.at(iterFrame->first) -= 1;
00708                                                                                 }
00709                                                                         }
00710                                                                         bundleWordReferences_.erase(iterRef);
00711                                                                 }
00712 
00713                                                                 mapPoints.erase(iter++);
00714                                                                 mapDescriptors.erase(iterMapDescriptors++);
00715                                                                 mapWords.erase(iterMapWords++);
00716                                                                 ++removed;
00717                                                         }
00718                                                         else
00719                                                         {
00720                                                                 ++iter;
00721                                                                 ++iterMapDescriptors;
00722                                                                 ++iterMapWords;
00723                                                         }
00724                                                 }
00725 
00726                                                 Link * previousLink = 0;
00727                                                 for(std::map<int, int>::iterator iter=bundlePoseReferences_.begin(); iter!=bundlePoseReferences_.end();)
00728                                                 {
00729                                                         if(iter->second <= 0)
00730                                                         {
00731                                                                 if(previousLink == 0 || bundleLinks_.find(iter->first) != bundleLinks_.end())
00732                                                                 {
00733                                                                         if(previousLink)
00734                                                                         {
00735                                                                                 UASSERT(previousLink->to() == iter->first);
00736                                                                                 *previousLink = previousLink->merge(bundleLinks_.find(iter->first)->second, previousLink->type());
00737                                                                         }
00738                                                                         UASSERT(bundlePoses_.erase(iter->first) == 1);
00739                                                                         bundleLinks_.erase(iter->first);
00740                                                                         bundleModels_.erase(iter->first);
00741                                                                         bundlePoseReferences_.erase(iter++);
00742                                                                 }
00743                                                         }
00744                                                         else
00745                                                         {
00746                                                                 previousLink=0;
00747                                                                 if(bundleLinks_.find(iter->first) != bundleLinks_.end())
00748                                                                 {
00749                                                                         previousLink = &bundleLinks_.find(iter->first)->second;
00750                                                                 }
00751                                                                 ++iter;
00752                                                         }
00753                                                 }
00754                                         }
00755 
00756                                         if(added || removed)
00757                                         {
00758                                                 modified = true;
00759                                         }
00760                                         UDEBUG("Update local features map = %fs", tmpTimer.ticks());
00761 
00762                                         // Geometric
00763                                         UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
00764                                         UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
00765 
00766                                         if(lastFrame_->sensorData().laserScanRaw().size())
00767                                         {
00768                                                 pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanRaw().localTransform());
00769                                                 pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
00770 
00771                                                 pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
00772                                                 int newPoints;
00773                                                 if(mapCloudNormals->size() && scanSubtractRadius_ > 0.0f)
00774                                                 {
00775                                                         frameCloudNormalsIndices = util3d::subtractFiltering(
00776                                                                         frameCloudNormals,
00777                                                                         pcl::IndicesPtr(new std::vector<int>),
00778                                                                         mapCloudNormals,
00779                                                                         pcl::IndicesPtr(new std::vector<int>),
00780                                                                         scanSubtractRadius_,
00781                                                                         scanSubtractAngle_);
00782                                                         newPoints = frameCloudNormalsIndices->size();
00783                                                 }
00784                                                 else
00785                                                 {
00786                                                         newPoints = mapCloudNormals->size();
00787                                                 }
00788 
00789                                                 if(newPoints)
00790                                                 {
00791                                                         scansBuffer_.push_back(std::make_pair(frameCloudNormals, frameCloudNormalsIndices));
00792 
00793                                                         //remove points if too big
00794                                                         UDEBUG("scansBuffer=%d, mapSize=%d newPoints=%d maxPoints=%d",
00795                                                                         (int)scansBuffer_.size(),
00796                                                                         int(mapCloudNormals->size()),
00797                                                                         newPoints,
00798                                                                         scanMaximumMapSize_);
00799 
00800                                                         if(scansBuffer_.size() > 1 &&
00801                                                                 int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_)
00802                                                         {
00803                                                                 //regenerate the local map
00804                                                                 mapCloudNormals->clear();
00805                                                                 std::list<int> toRemove;
00806                                                                 int i = int(scansBuffer_.size())-1;
00807                                                                 for(; i>=0; --i)
00808                                                                 {
00809                                                                         int pointsToAdd = scansBuffer_[i].second->size()?scansBuffer_[i].second->size():scansBuffer_[i].first->size();
00810                                                                         if((int)mapCloudNormals->size() + pointsToAdd > scanMaximumMapSize_ ||
00811                                                                                 i == 0)
00812                                                                         {
00813                                                                                 *mapCloudNormals += *scansBuffer_[i].first;
00814                                                                                 break;
00815                                                                         }
00816                                                                         else
00817                                                                         {
00818                                                                                 if(scansBuffer_[i].second->size())
00819                                                                                 {
00820                                                                                         pcl::PointCloud<pcl::PointNormal> tmp;
00821                                                                                         pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
00822                                                                                         *mapCloudNormals += tmp;
00823                                                                                 }
00824                                                                                 else
00825                                                                                 {
00826                                                                                         *mapCloudNormals += *scansBuffer_[i].first;
00827                                                                                 }
00828                                                                         }
00829                                                                 }
00830                                                                 // remove old clouds
00831                                                                 if(i > 0)
00832                                                                 {
00833                                                                         std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
00834                                                                         int oi = 0;
00835                                                                         for(; i<(int)scansBuffer_.size(); ++i)
00836                                                                         {
00837                                                                                 UASSERT(oi < (int)scansTmp.size());
00838                                                                                 scansTmp[oi++] = scansBuffer_[i];
00839                                                                         }
00840                                                                         scansBuffer_ = scansTmp;
00841                                                                 }
00842                                                         }
00843                                                         else
00844                                                         {
00845                                                                 // just append the last cloud
00846                                                                 if(scansBuffer_.back().second->size())
00847                                                                 {
00848                                                                         pcl::PointCloud<pcl::PointNormal> tmp;
00849                                                                         pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
00850                                                                         *mapCloudNormals += tmp;
00851                                                                 }
00852                                                                 else
00853                                                                 {
00854                                                                         *mapCloudNormals += *scansBuffer_.back().first;
00855                                                                 }
00856                                                         }
00857                                                         if(mapScan.is2d())
00858                                                         {
00859                                                                 Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
00860                                                                 mapScan = LaserScan(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYNormal);
00861                                                         }
00862                                                         else
00863                                                         {
00864                                                                 Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
00865                                                                 mapScan = LaserScan(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYZNormal);
00866                                                         }
00867                                                         modified=true;
00868                                                 }
00869                                         }
00870                                         UDEBUG("Update local scan map = %fs", tmpTimer.ticks());
00871                                 }
00872 
00873                                 if(modified)
00874                                 {
00875                                         *map_ = tmpMap;
00876 
00877                                         if(mapScan.is2d())
00878                                         {
00879 
00880                                                 map_->sensorData().setLaserScanRaw(
00881                                                                 LaserScan(
00882                                                                                 mapScan.data(),
00883                                                                                 0,
00884                                                                                 0.0f,
00885                                                                                 mapScan.format(),
00886                                                                                 Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
00887                                         }
00888                                         else
00889                                         {
00890                                                 map_->sensorData().setLaserScanRaw(
00891                                                                 LaserScan(
00892                                                                                 mapScan.data(),
00893                                                                                 0,
00894                                                                                 0.0f,
00895                                                                                 mapScan.format(),
00896                                                                                 newFramePose.translation()));
00897                                         }
00898 
00899                                         map_->setWords(mapWords);
00900                                         map_->setWords3(mapPoints);
00901                                         map_->setWordsDescriptors(mapDescriptors);
00902                                 }
00903                         }
00904 
00905                         if(info)
00906                         {
00907                                 // use tmpMap instead of map_ to make sure that correspondences with the new frame matches
00908                                 info->localMapSize = (int)tmpMap.getWords3().size();
00909                                 info->localScanMapSize = tmpMap.sensorData().laserScanRaw().size();
00910                                 if(this->isInfoDataFilled())
00911                                 {
00912                                         info->localMap = uMultimapToMap(tmpMap.getWords3());
00913                                         info->localScanMap = tmpMap.sensorData().laserScanRaw();
00914                                 }
00915                         }
00916                 }
00917                 else
00918                 {
00919                         // just generate keypoints for the new signature
00920                         if(regPipeline_->isImageRequired())
00921                         {
00922                                 Signature dummy;
00923                                 regPipeline_->computeTransformationMod(
00924                                                 *lastFrame_,
00925                                                 dummy);
00926                         }
00927 
00928                         data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
00929 
00930                         // a very high variance tells that the new pose is not linked with the previous one
00931                         regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
00932 
00933                         bool frameValid = false;
00934                         Transform newFramePose = this->getPose(); // initial pose may be not identity...
00935                         if(regPipeline_->isImageRequired())
00936                         {
00937                                 if ((int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
00938                                 {
00939                                         frameValid = true;
00940                                         // update local map
00941                                         UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
00942                                         UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
00943 
00944                                         std::multimap<int, cv::KeyPoint> words;
00945                                         std::multimap<int, cv::Point3f> transformedPoints;
00946                                         std::multimap<int, int> mapPointWeights;
00947                                         std::multimap<int, cv::Mat> descriptors;
00948                                         UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
00949                                         std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
00950                                         std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
00951                                         for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
00952                                                 iter != lastFrame_->getWords3().end();
00953                                                 ++iter, ++descIter, ++wordsIter)
00954                                         {
00955                                                 if (util3d::isFinite(iter->second))
00956                                                 {
00957                                                         words.insert(*wordsIter);
00958                                                         transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
00959                                                         mapPointWeights.insert(std::make_pair(iter->first, 0));
00960                                                         descriptors.insert(*descIter);
00961                                                 }
00962                                         }
00963 
00964                                         if(bundleAdjustment_>0)
00965                                         {
00966                                                 Transform invLocalTransform;
00967                                                 if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
00968                                                 {
00969                                                         invLocalTransform = lastFrame_->sensorData().cameraModels()[0].localTransform().inverse();
00970                                                 }
00971                                                 else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
00972                                                 {
00973                                                         invLocalTransform = lastFrame_->sensorData().stereoCameraModel().left().localTransform().inverse();
00974                                                 }
00975                                                 else
00976                                                 {
00977                                                         UFATAL("no valid camera model!");
00978                                                 }
00979 
00980                                                 // update bundleWordReferences_: used for bundle adjustment
00981                                                 for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
00982                                                 {
00983                                                         if(words.count(iter->first) == 1)
00984                                                         {
00985                                                                 UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
00986                                                                 std::map<int, cv::Point3f> framePt;
00987 
00988                                                                 //get depth
00989                                                                 float d = 0.0f;
00990                                                                 if(lastFrame_->getWords3().count(iter->first) == 1)
00991                                                                 {
00992                                                                         //move back point in camera frame (to get depth along z)
00993                                                                         cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(iter->first)->second, invLocalTransform);
00994                                                                         d = pt3d.z;
00995                                                                 }
00996 
00997 
00998                                                                 framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.pt.x, iter->second.pt.y, d)));
00999                                                                 bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
01000                                                         }
01001                                                 }
01002 
01003                                                 bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));
01004 
01005                                                 CameraModel model;
01006                                                 if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
01007                                                 {
01008                                                         model = lastFrame_->sensorData().cameraModels()[0];
01009                                                 }
01010                                                 else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
01011                                                 {
01012                                                         model = lastFrame_->sensorData().stereoCameraModel().left();
01013                                                         // Set Tx for stereo BA
01014                                                         model = CameraModel(model.fx(),
01015                                                                         model.fy(),
01016                                                                         model.cx(),
01017                                                                         model.cy(),
01018                                                                         model.localTransform(),
01019                                                                         -lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
01020                                                 }
01021                                                 else
01022                                                 {
01023                                                         UFATAL("invalid camera model!");
01024                                                 }
01025                                                 bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
01026                                                 bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
01027                                         }
01028 
01029                                         map_->setWords(words);
01030                                         map_->setWords3(transformedPoints);
01031                                         map_->setWordsDescriptors(descriptors);
01032                                         addKeyFrame = true;
01033                                 }
01034                                 else
01035                                 {
01036                                         UWARN("%d visual features required to initialize the odometry (only %d extracted).", regPipeline_->getMinVisualCorrespondences(), (int)lastFrame_->getWords3().size());
01037                                 }
01038                         }
01039                         if(regPipeline_->isScanRequired())
01040                         {
01041                                 if (lastFrame_->sensorData().laserScanRaw().size())
01042                                 {
01043                                         frameValid = true;
01044                                         pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
01045                                         scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
01046                                         if(lastFrame_->sensorData().laserScanRaw().is2d())
01047                                         {
01048                                                 Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
01049                                                 map_->sensorData().setLaserScanRaw(
01050                                                                 LaserScan(
01051                                                                                 util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
01052                                                                                 0,
01053                                                                                 0.0f,
01054                                                                                 LaserScan::kXYNormal,
01055                                                                                 Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
01056                                         }
01057                                         else
01058                                         {
01059                                                 Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
01060                                                 map_->sensorData().setLaserScanRaw(
01061                                                                 LaserScan(
01062                                                                                 util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
01063                                                                                 0,
01064                                                                                 0.0f,
01065                                                                                 LaserScan::kXYZNormal,
01066                                                                                 newFramePose.translation()));
01067                                         }
01068                                         addKeyFrame = true;
01069                                 }
01070                                 else
01071                                 {
01072                                         UWARN("Missing scan to initialize odometry.");
01073                                 }
01074                         }
01075 
01076                         if (frameValid)
01077                         {
01078                                 // We initialized the local map
01079                                 output.setIdentity();
01080                         }
01081 
01082                         if(info)
01083                         {
01084                                 info->localMapSize = (int)map_->getWords3().size();
01085                                 info->localScanMapSize = map_->sensorData().laserScanRaw().size();
01086 
01087                                 if(this->isInfoDataFilled())
01088                                 {
01089                                         info->localMap = uMultimapToMap(map_->getWords3());
01090                                         info->localScanMap = map_->sensorData().laserScanRaw();
01091                                 }
01092                         }
01093                 }
01094 
01095                 map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat()); // clear sensorData features
01096 
01097                 nFeatures = lastFrame_->getWords().size();
01098                 if(this->isInfoDataFilled() && info)
01099                 {
01100                         if(regPipeline_->isImageRequired())
01101                         {
01102                                 info->words = lastFrame_->getWords();
01103                         }
01104                 }
01105         }
01106 
01107         if(info)
01108         {
01109                 info->features = nFeatures;
01110                 info->localKeyFrames = (int)bundlePoses_.size();
01111                 info->keyFrameAdded = addKeyFrame;
01112                 info->localBundleOutliers = totalBundleOutliers;
01113                 info->localBundleConstraints = totalBundleWordReferencesUsed;
01114                 info->localBundleTime = bundleTime;
01115 
01116                 if(this->isInfoDataFilled())
01117                 {
01118                         info->reg = regInfo;
01119                 }
01120                 else
01121                 {
01122                         info->reg = regInfo.copyWithoutData();
01123                 }
01124         }
01125 
01126         UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance:lin=%f, ang=%f local_map=%d local_scan_map=%d",
01127                         timer.elapsed(),
01128                         output.isNull()?"true":"false",
01129                         nFeatures,
01130                         regInfo.inliers,
01131                         regInfo.matches,
01132                         regInfo.covariance.at<double>(0,0),
01133                         regInfo.covariance.at<double>(5,5),
01134                         regPipeline_->isImageRequired()?(int)map_->getWords3().size():0,
01135                         regPipeline_->isScanRequired()?(int)map_->sensorData().laserScanRaw().size():0);
01136 
01137         return output;
01138 }
01139 
01140 } // namespace rtabmap