rtabmap: OptimizerGTSAM.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 #include "rtabmap/core/Graph.h"
00028 
00029 #include <rtabmap/utilite/ULogger.h>
00030 #include <rtabmap/utilite/UStl.h>
00031 #include <rtabmap/utilite/UMath.h>
00032 #include <rtabmap/utilite/UConversion.h>
00033 #include <rtabmap/utilite/UTimer.h>
00034 #include <set>
00035 
00036 #include <rtabmap/core/OptimizerGTSAM.h>
00037 
00038 #ifdef RTABMAP_GTSAM
00039 #include <gtsam/geometry/Pose2.h>
00040 #include <gtsam/geometry/Pose3.h>
00041 #include <gtsam/inference/Key.h>
00042 #include <gtsam/inference/Symbol.h>
00043 #include <gtsam/slam/PriorFactor.h>
00044 #include <gtsam/slam/BetweenFactor.h>
00045 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
00046 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
00047 #include <gtsam/nonlinear/DoglegOptimizer.h>
00048 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
00049 #include <gtsam/nonlinear/NonlinearOptimizer.h>
00050 #include <gtsam/nonlinear/Marginals.h>
00051 #include <gtsam/nonlinear/Values.h>
00052 
00053 #ifdef RTABMAP_VERTIGO
00054 #include "vertigo/gtsam/betweenFactorMaxMix.h"
00055 #include "vertigo/gtsam/betweenFactorSwitchable.h"
00056 #include "vertigo/gtsam/switchVariableLinear.h"
00057 #include "vertigo/gtsam/switchVariableSigmoid.h"
00058 #endif
00059 #endif // end RTABMAP_GTSAM
00060 
00061 namespace rtabmap {
00062 
00063 bool OptimizerGTSAM::available()
00064 {
00065 #ifdef RTABMAP_GTSAM
00066         return true;
00067 #else
00068         return false;
00069 #endif
00070 }
00071 
00072 void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
00073 {
00074         Optimizer::parseParameters(parameters);
00075         Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), optimizer_);
00076 }
00077 
00078 std::map<int, Transform> OptimizerGTSAM::optimize(
00079                 int rootId,
00080                 const std::map<int, Transform> & poses,
00081                 const std::multimap<int, Link> & edgeConstraints,
00082                 cv::Mat & outputCovariance,
00083                 std::list<std::map<int, Transform> > * intermediateGraphes,
00084                 double * finalError,
00085                 int * iterationsDone)
00086 {
00087         outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
00088         std::map<int, Transform> optimizedPoses;
00089 #ifdef RTABMAP_GTSAM
00090 
00091 #ifndef RTABMAP_VERTIGO
00092         if(this->isRobust())
00093         {
00094                 UWARN("Vertigo robust optimization is not available! Robust optimization is now disabled.");
00095                 setRobust(false);
00096         }
00097 #endif
00098 
00099         UDEBUG("Optimizing graph...");
00100         if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
00101         {
00102                 gtsam::NonlinearFactorGraph graph;
00103 
00104                 // detect if there is a global pose prior set, if so remove rootId
00105                 if(!priorsIgnored())
00106                 {
00107                         for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
00108                         {
00109                                 if(iter->second.from() == iter->second.to())
00110                                 {
00111                                         rootId = 0;
00112                                         break;
00113                                 }
00114                         }
00115                 }
00116 
00117                 //prior first pose
00118                 if(rootId != 0)
00119                 {
00120                         UASSERT(uContains(poses, rootId));
00121                         const Transform & initialPose = poses.at(rootId);
00122                         if(isSlam2d())
00123                         {
00124                                 gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, 0.01));
00125                                 graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
00126                         }
00127                         else
00128                         {
00129                                 gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
00130                                 graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
00131                         }
00132                 }
00133 
00134                 UDEBUG("fill poses to gtsam...");
00135                 gtsam::Values initialEstimate;
00136                 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00137                 {
00138                         UASSERT(!iter->second.isNull());
00139                         if(isSlam2d())
00140                         {
00141                                 initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
00142                         }
00143                         else
00144                         {
00145                                 initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
00146                         }
00147                 }
00148 
00149                 UDEBUG("fill edges to gtsam...");
00150                 int switchCounter = poses.rbegin()->first+1;
00151                 for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
00152                 {
00153                         int id1 = iter->second.from();
00154                         int id2 = iter->second.to();
00155                         UASSERT(!iter->second.transform().isNull());
00156                         if(id1 == id2)
00157                         {
00158                                 if(!priorsIgnored())
00159                                 {
00160                                         if(isSlam2d())
00161                                         {
00162                                                 Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
00163                                                 if(!isCovarianceIgnored())
00164                                                 {
00165                                                         information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
00166                                                         information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
00167                                                         information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
00168                                                         information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
00169                                                         information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
00170                                                         information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
00171                                                         information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
00172                                                         information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
00173                                                         information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
00174                                                 }
00175 
00176                                                 gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
00177                                                 graph.add(gtsam::PriorFactor<gtsam::Pose2>(id1, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
00178                                         }
00179                                         else
00180                                         {
00181                                                 Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
00182                                                 if(!isCovarianceIgnored())
00183                                                 {
00184                                                         memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
00185                                                 }
00186 
00187                                                 Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
00188                                                 mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
00189                                                 mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
00190                                                 mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
00191                                                 mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
00192                                                 gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
00193 
00194                                                 graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
00195                                         }
00196                                 }
00197                         }
00198                         else
00199                         {
00200 #ifdef RTABMAP_VERTIGO
00201                                 if(this->isRobust() &&
00202                                    iter->second.type() != Link::kNeighbor &&
00203                                    iter->second.type() != Link::kNeighborMerged &&
00204                                    iter->second.type() != Link::kPosePrior)
00205                                 {
00206                                         // create new switch variable
00207                                         // Sunderhauf IROS 2012:
00208                                         // "Since it is reasonable to initially accept all loop closure constraints,
00209                                         //  a proper and convenient initial value for all switch variables would be
00210                                         //  sij = 1 when using the linear switch function"
00211                                         double prior = 1.0;
00212                                         initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior));
00213 
00214                                         // create switch prior factor
00215                                         // "If the front-end is not able to assign sound individual values
00216                                         //  for Ξij , it is save to set all Ξij = 1, since this value is close
00217                                         //  to the individual optimal choice of Ξij for a large range of
00218                                         //  outliers."
00219                                         gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
00220                                         graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
00221                                 }
00222 #endif
00223 
00224                                 if(isSlam2d())
00225                                 {
00226                                         Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
00227                                         if(!isCovarianceIgnored())
00228                                         {
00229                                                 information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
00230                                                 information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
00231                                                 information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
00232                                                 information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
00233                                                 information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
00234                                                 information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
00235                                                 information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
00236                                                 information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
00237                                                 information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
00238                                         }
00239                                         gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
00240 
00241 #ifdef RTABMAP_VERTIGO
00242                                         if(this->isRobust() &&
00243                                            iter->second.type()!=Link::kNeighbor &&
00244                                            iter->second.type() != Link::kNeighborMerged)
00245                                         {
00246                                                 // create switchable edge factor
00247                                                 graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
00248                                         }
00249                                         else
00250 #endif
00251                                         {
00252                                                 graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
00253                                         }
00254                                 }
00255                                 else
00256                                 {
00257                                         Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
00258                                         if(!isCovarianceIgnored())
00259                                         {
00260                                                 memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
00261                                         }
00262 
00263                                         Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
00264                                         mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
00265                                         mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
00266                                         mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
00267                                         mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
00268                                         gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
00269 
00270 #ifdef RTABMAP_VERTIGO
00271                                         if(this->isRobust() &&
00272                                            iter->second.type() != Link::kNeighbor &&
00273                                            iter->second.type() != Link::kNeighborMerged &&
00274                                            iter->second.type() != Link::kPosePrior)
00275                                         {
00276                                                 // create switchable edge factor
00277                                                 graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
00278                                         }
00279                                         else
00280 #endif
00281                                         {
00282                                                 graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
00283                                         }
00284                                 }
00285                         }
00286                 }
00287 
00288                 UDEBUG("create optimizer");
00289                 gtsam::NonlinearOptimizer * optimizer;
00290 
00291                 if(optimizer_ == 2)
00292                 {
00293                         gtsam::DoglegParams parameters;
00294                         parameters.relativeErrorTol = epsilon();
00295                         parameters.maxIterations = iterations();
00296                         optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters);
00297                 }
00298                 else if(optimizer_ == 1)
00299                 {
00300                         gtsam::GaussNewtonParams parameters;
00301                         parameters.relativeErrorTol = epsilon();
00302                         parameters.maxIterations = iterations();
00303                         optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters);
00304                 }
00305                 else
00306                 {
00307                         gtsam::LevenbergMarquardtParams parameters;
00308                         parameters.relativeErrorTol = epsilon();
00309                         parameters.maxIterations = iterations();
00310                         optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);
00311                 }
00312 
00313                 UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
00314                 UTimer timer;
00315                 int it = 0;
00316                 double lastError = optimizer->error();
00317                 for(int i=0; i<iterations(); ++i)
00318                 {
00319                         if(intermediateGraphes && i > 0)
00320                         {
00321                                 std::map<int, Transform> tmpPoses;
00322                                 for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
00323                                 {
00324                                         if(iter->value.dim() > 1)
00325                                         {
00326                                                 if(isSlam2d())
00327                                                 {
00328                                                         gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
00329                                                         tmpPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
00330                                                 }
00331                                                 else
00332                                                 {
00333                                                         gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
00334                                                         tmpPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
00335                                                 }
00336                                         }
00337                                 }
00338                                 intermediateGraphes->push_back(tmpPoses);
00339                         }
00340                         try
00341                         {
00342                                 optimizer->iterate();
00343                                 ++it;
00344                         }
00345                         catch(gtsam::IndeterminantLinearSystemException & e)
00346                         {
00347                                 UERROR("GTSAM exception caught: %s", e.what());
00348                                 delete optimizer;
00349                                 return optimizedPoses;
00350                         }
00351 
00352                         // early stop condition
00353                         double error = optimizer->error();
00354                         UDEBUG("iteration %d error =%f", i+1, error);
00355                         double errorDelta = lastError - error;
00356                         if(i>0 && errorDelta < this->epsilon())
00357                         {
00358                                 if(errorDelta < 0)
00359                                 {
00360                                         UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
00361                                 }
00362                                 else
00363                                 {
00364                                         UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
00365                                         break;
00366                                 }
00367                         }
00368                         else if(i==0 && error < this->epsilon())
00369                         {
00370                                 UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
00371                                 break;
00372                         }
00373                         lastError = error;
00374                 }
00375                 if(finalError)
00376                 {
00377                         *finalError = lastError;
00378                 }
00379                 if(iterationsDone)
00380                 {
00381                         *iterationsDone = it;
00382                 }
00383                 UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)",
00384                                 optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
00385 
00386                 gtsam::Marginals marginals(graph, optimizer->values());
00387                 for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
00388                 {
00389                         if(iter->value.dim() > 1)
00390                         {
00391                                 if(isSlam2d())
00392                                 {
00393                                         gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
00394                                         optimizedPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
00395                                 }
00396                                 else
00397                                 {
00398                                         gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
00399                                         optimizedPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
00400                                 }
00401                         }
00402                 }
00403 
00404                 // compute marginals
00405                 try {
00406                         UTimer t;
00407                         gtsam::Marginals marginals(graph, optimizer->values());
00408                         gtsam::Matrix info = marginals.marginalCovariance(optimizer->values().rbegin()->key);
00409                         UINFO("Computed marginals = %fs (key=%d)", t.ticks(), optimizer->values().rbegin()->key);
00410                         if(isSlam2d())
00411                         {
00412                                 UASSERT(info.cols() == 3 && info.cols() == 3);
00413                                 outputCovariance.at<double>(0,0) = info(0,0); // x-x
00414                                 outputCovariance.at<double>(0,1) = info(0,1); // x-y
00415                                 outputCovariance.at<double>(0,5) = info(0,2); // x-theta
00416                                 outputCovariance.at<double>(1,0) = info(1,0); // y-x
00417                                 outputCovariance.at<double>(1,1) = info(1,1); // y-y
00418                                 outputCovariance.at<double>(1,5) = info(1,2); // y-theta
00419                                 outputCovariance.at<double>(5,0) = info(2,0); // theta-x
00420                                 outputCovariance.at<double>(5,1) = info(2,1); // theta-y
00421                                 outputCovariance.at<double>(5,5) = info(2,2); // theta-theta
00422                         }
00423                         else
00424                         {
00425                                 UASSERT(info.cols() == 6 && info.cols() == 6);
00426                                 Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
00427                                 mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
00428                                 mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
00429                                 mgtsam.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
00430                                 mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
00431                                 memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
00432                         }
00433                 } catch(std::exception& e) {
00434                         cout << e.what() << endl;
00435                 }
00436 
00437                 delete optimizer;
00438         }
00439         else if(poses.size() == 1 || iterations() <= 0)
00440         {
00441                 optimizedPoses = poses;
00442         }
00443         else
00444         {
00445                 UWARN("This method should be called at least with 1 pose!");
00446         }
00447         UDEBUG("Optimizing graph...end!");
00448 #else
00449         UERROR("Not built with GTSAM support!");
00450 #endif
00451         return optimizedPoses;
00452 }
00453 
00454 } /* namespace rtabmap */