OptimizerG2O.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <rtabmap/utilite/ULogger.h>
00029 #include <rtabmap/utilite/UStl.h>
00030 #include <rtabmap/utilite/UMath.h>
00031 #include <rtabmap/utilite/UConversion.h>
00032 #include <rtabmap/utilite/UTimer.h>
00033 #include <set>
00034 
00035 #include <rtabmap/core/Version.h>
00036 #include <rtabmap/core/OptimizerG2O.h>
00037 #include <rtabmap/core/util3d_transforms.h>
00038 #include <rtabmap/core/util3d_motion_estimation.h>
00039 #include <rtabmap/core/util3d.h>
00040 
00041 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
00042 #include "g2o/core/sparse_optimizer.h"
00043 #include "g2o/core/block_solver.h"
00044 #include "g2o/core/factory.h"
00045 #include "g2o/core/optimization_algorithm_factory.h"
00046 #include "g2o/core/optimization_algorithm_gauss_newton.h"
00047 #include "g2o/core/optimization_algorithm_levenberg.h"
00048 #include "g2o/core/robust_kernel_impl.h"
00049 namespace g2o {
00050 // bug #include "g2o/core/eigen_types.h" not found on Indigo
00051 typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixXD;
00052 }
00053 
00054 #ifdef RTABMAP_G2O
00055 #include "g2o/types/sba/types_sba.h"
00056 #include "g2o/solvers/eigen/linear_solver_eigen.h"
00057 #include "g2o/config.h"
00058 #include "g2o/types/slam2d/types_slam2d.h"
00059 #include "g2o/types/slam3d/types_slam3d.h"
00060 #ifdef G2O_HAVE_CSPARSE
00061 #include "g2o/solvers/csparse/linear_solver_csparse.h"
00062 #endif
00063 #include "g2o/solvers/pcg/linear_solver_pcg.h"
00064 #ifdef G2O_HAVE_CHOLMOD
00065 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
00066 #endif
00067 enum {
00068     PARAM_OFFSET=0,
00069 };
00070 #endif // RTABMAP_G2O
00071 
00072 #ifdef RTABMAP_ORB_SLAM2
00073 #include "g2o/types/types_sba.h"
00074 #include "g2o/types/types_six_dof_expmap.h"
00075 #include "g2o/solvers/linear_solver_eigen.h"
00076 #endif
00077 
00078 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
00079 typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver;
00080 #ifdef RTABMAP_G2O
00081 typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
00082 #ifdef G2O_HAVE_CSPARSE
00083 typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
00084 #endif
00085 #ifdef G2O_HAVE_CHOLMOD
00086 typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearCholmodSolver;
00087 #endif
00088 
00089 #if defined(RTABMAP_VERTIGO)
00090 #include "vertigo/g2o/edge_switchPrior.h"
00091 #include "vertigo/g2o/edge_se2Switchable.h"
00092 #include "vertigo/g2o/edge_se3Switchable.h"
00093 #include "vertigo/g2o/vertex_switchLinear.h"
00094 #endif
00095 #endif
00096 
00097 #endif // end defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
00098 
00099 namespace rtabmap {
00100 
00101 bool OptimizerG2O::available()
00102 {
00103 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
00104         return true;
00105 #else
00106         return false;
00107 #endif
00108 }
00109 
00110 bool OptimizerG2O::isCSparseAvailable()
00111 {
00112 #ifdef G2O_HAVE_CSPARSE
00113         return true;
00114 #else
00115         return false;
00116 #endif
00117 }
00118 
00119 bool OptimizerG2O::isCholmodAvailable()
00120 {
00121 #ifdef G2O_HAVE_CHOLMOD
00122         return true;
00123 #else
00124         return false;
00125 #endif
00126 }
00127 
00128 void OptimizerG2O::parseParameters(const ParametersMap & parameters)
00129 {
00130         Optimizer::parseParameters(parameters);
00131 
00132         Parameters::parse(parameters, Parameters::kg2oSolver(), solver_);
00133         Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_);
00134         Parameters::parse(parameters, Parameters::kg2oPixelVariance(), pixelVariance_);
00135         Parameters::parse(parameters, Parameters::kg2oRobustKernelDelta(), robustKernelDelta_);
00136         Parameters::parse(parameters, Parameters::kg2oBaseline(), baseline_);
00137         UASSERT(pixelVariance_ > 0.0);
00138         UASSERT(baseline_ >= 0.0);
00139 
00140 #ifdef RTABMAP_ORB_SLAM2
00141         if(solver_ != 3)
00142         {
00143                 UWARN("g2o built with ORB_SLAM2 has only Eigen solver available, using Eigen=3 instead of %d.", solver_);
00144                 solver_ = 3;
00145         }
00146 #else
00147 #ifndef G2O_HAVE_CHOLMOD
00148         if(solver_ == 2)
00149         {
00150                 UWARN("g2o is not built with chmold, so it cannot be used as solver. Using CSparse instead.");
00151                 solver_ = 0;
00152         }
00153 #endif
00154 
00155 #ifndef G2O_HAVE_CSPARSE
00156         if(solver_ == 0)
00157         {
00158                 UWARN("g2o is not built with csparse, so it cannot be used as solver. Using PCG instead.");
00159                 solver_ = 1;
00160         }
00161 #endif
00162 #endif
00163 
00164 }
00165 
00166 std::map<int, Transform> OptimizerG2O::optimize(
00167                 int rootId,
00168                 const std::map<int, Transform> & poses,
00169                 const std::multimap<int, Link> & edgeConstraints,
00170                 cv::Mat & outputCovariance,
00171                 std::list<std::map<int, Transform> > * intermediateGraphes,
00172                 double * finalError,
00173                 int * iterationsDone)
00174 {
00175         outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
00176         std::map<int, Transform> optimizedPoses;
00177 #ifdef RTABMAP_G2O
00178         UDEBUG("Optimizing graph...");
00179 
00180 #ifndef RTABMAP_VERTIGO
00181         if(this->isRobust())
00182         {
00183                 UWARN("Vertigo robust optimization is not available! Robust optimization is now disabled.");
00184                 setRobust(false);
00185         }
00186 #endif
00187 
00188         optimizedPoses.clear();
00189         if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
00190         {
00191                 // Apply g2o optimization
00192 
00193                 g2o::SparseOptimizer optimizer;
00194                 optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
00195                 g2o::ParameterSE3Offset* odomOffset = new g2o::ParameterSE3Offset();
00196                 odomOffset->setId(PARAM_OFFSET);
00197                 optimizer.addParameter(odomOffset);
00198 
00199 #ifdef RTABMAP_G2O_CPP11
00200 
00201                 std::unique_ptr<SlamBlockSolver> blockSolver;
00202 
00203                 if(solver_ == 3)
00204                 {
00205                         //eigen
00206                         auto linearSolver = g2o::make_unique<SlamLinearEigenSolver>();
00207                         linearSolver->setBlockOrdering(false);
00208                         blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
00209                 }
00210 #ifdef G2O_HAVE_CHOLMOD
00211                 else if(solver_ == 2)
00212                 {
00213                         //chmold
00214                         auto linearSolver = g2o::make_unique<SlamLinearCholmodSolver>();
00215                         linearSolver->setBlockOrdering(false);
00216                         blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
00217                 }
00218 #endif
00219 #ifdef G2O_HAVE_CSPARSE
00220                 else if(solver_ == 0)
00221                 {
00222 
00223                         //csparse
00224                         auto linearSolver = g2o::make_unique<SlamLinearCSparseSolver>();
00225                         linearSolver->setBlockOrdering(false);
00226                         blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
00227                 }
00228 #endif
00229                 else
00230                 {
00231                         //pcg
00232                         auto linearSolver = g2o::make_unique<SlamLinearPCGSolver>();
00233                         blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
00234                 }
00235 
00236                 if(optimizer_ == 1)
00237                 {
00238 
00239                         optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(std::move(blockSolver)));
00240                 }
00241                 else
00242                 {
00243                         optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver)));
00244                 }
00245 
00246 #else
00247 
00248                 SlamBlockSolver * blockSolver = 0;
00249 
00250                 if(solver_ == 3)
00251                 {
00252                         //eigen
00253                         SlamLinearEigenSolver * linearSolver = new SlamLinearEigenSolver();
00254                         linearSolver->setBlockOrdering(false);
00255                         blockSolver = new SlamBlockSolver(linearSolver);
00256                 }
00257 #ifdef G2O_HAVE_CHOLMOD
00258                 else if(solver_ == 2)
00259                 {
00260                         //chmold
00261                         SlamLinearCholmodSolver * linearSolver = new SlamLinearCholmodSolver();
00262                         linearSolver->setBlockOrdering(false);
00263                         blockSolver = new SlamBlockSolver(linearSolver);
00264                 }
00265 #endif
00266 #ifdef G2O_HAVE_CSPARSE
00267                 else if(solver_ == 0)
00268                 {
00269                         //csparse
00270                         SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver();
00271                         linearSolver->setBlockOrdering(false);
00272                         blockSolver = new SlamBlockSolver(linearSolver);
00273                 }
00274 #endif
00275                 else
00276                 {
00277                         //pcg
00278                         SlamLinearPCGSolver * linearSolver = new SlamLinearPCGSolver();
00279                         blockSolver = new SlamBlockSolver(linearSolver);
00280                 }
00281 
00282                 if(optimizer_ == 1)
00283                 {
00284                         optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver));
00285                 }
00286                 else
00287                 {
00288                         optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver));
00289                 }
00290 #endif
00291                 // detect if there is a global pose prior set, if so remove rootId
00292                 if(!priorsIgnored())
00293                 {
00294                         for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
00295                         {
00296                                 if(iter->second.from() == iter->second.to())
00297                                 {
00298                                         rootId = 0;
00299                                         break;
00300                                 }
00301                         }
00302                 }
00303 
00304                 UDEBUG("fill poses to g2o...");
00305                 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00306                 {
00307                         UASSERT(!iter->second.isNull());
00308                         g2o::HyperGraph::Vertex * vertex = 0;
00309                         if(isSlam2d())
00310                         {
00311                                 g2o::VertexSE2 * v2 = new g2o::VertexSE2();
00312                                 v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
00313                                 if(iter->first == rootId)
00314                                 {
00315                                         v2->setFixed(true);
00316                                 }
00317                                 vertex = v2;
00318                         }
00319                         else
00320                         {
00321                                 g2o::VertexSE3 * v3 = new g2o::VertexSE3();
00322 
00323                                 Eigen::Affine3d a = iter->second.toEigen3d();
00324                                 Eigen::Isometry3d pose;
00325                                 pose = a.linear();
00326                                 pose.translation() = a.translation();
00327                                 v3->setEstimate(pose);
00328                                 if(iter->first == rootId)
00329                                 {
00330                                         v3->setFixed(true);
00331                                 }
00332                                 vertex = v3;
00333                         }
00334                         vertex->setId(iter->first);
00335                         UASSERT_MSG(optimizer.addVertex(vertex), uFormat("cannot insert vertex %d!?", iter->first).c_str());
00336                 }
00337 
00338                 UDEBUG("fill edges to g2o...");
00339 #if defined(RTABMAP_VERTIGO)
00340                 int vertigoVertexId = poses.rbegin()->first+1;
00341 #endif
00342                 for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
00343                 {
00344                         int id1 = iter->second.from();
00345                         int id2 = iter->second.to();
00346 
00347                         UASSERT(!iter->second.transform().isNull());
00348 
00349                         g2o::HyperGraph::Edge * edge = 0;
00350 
00351                         if(id1 == id2)
00352                         {
00353                                 if(!priorsIgnored())
00354                                 {
00355                                         if(isSlam2d())
00356                                         {
00357                                                 g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
00358                                                 g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
00359                                                 priorEdge->setVertex(0, v1);
00360                                                 priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
00361                                                 priorEdge->setParameterId(0, PARAM_OFFSET);
00362                                                 Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
00363                                                 if(!isCovarianceIgnored())
00364                                                 {
00365                                                         information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
00366                                                         information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
00367                                                         information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
00368                                                         information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
00369                                                         information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
00370                                                         information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
00371                                                         information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
00372                                                         information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
00373                                                         information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
00374                                                 }
00375                                                 priorEdge->setInformation(information);
00376                                                 edge = priorEdge;
00377                                         }
00378                                         else
00379                                         {
00380                                                 g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
00381                                                 g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
00382                                                 priorEdge->setVertex(0, v1);
00383                                                 Eigen::Affine3d a = iter->second.transform().toEigen3d();
00384                                                 Eigen::Isometry3d pose;
00385                                                 pose = a.linear();
00386                                                 pose.translation() = a.translation();
00387                                                 priorEdge->setMeasurement(pose);
00388                                                 priorEdge->setParameterId(0, PARAM_OFFSET);
00389                                                 Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
00390                                                 if(!isCovarianceIgnored())
00391                                                 {
00392                                                         memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
00393                                                 }
00394                                                 priorEdge->setInformation(information);
00395                                                 edge = priorEdge;
00396                                         }
00397                                 }
00398                         }
00399                         else
00400                         {
00401 #if defined(RTABMAP_VERTIGO)
00402                                 VertexSwitchLinear * v = 0;
00403                                 if(this->isRobust() &&
00404                                    iter->second.type() != Link::kNeighbor &&
00405                                    iter->second.type() != Link::kNeighborMerged)
00406                                 {
00407                                         // For loop closure links, add switchable edges
00408 
00409                                         // create new switch variable
00410                                         // Sunderhauf IROS 2012:
00411                                         // "Since it is reasonable to initially accept all loop closure constraints,
00412                                         //  a proper and convenient initial value for all switch variables would be
00413                                         //  sij = 1 when using the linear switch function"
00414                                         v = new VertexSwitchLinear();
00415                                         v->setEstimate(1.0);
00416                                         v->setId(vertigoVertexId++);
00417                                         UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str());
00418 
00419                                         // create switch prior factor
00420                                         // "If the front-end is not able to assign sound individual values
00421                                         //  for Ξij , it is save to set all Ξij = 1, since this value is close
00422                                         //  to the individual optimal choice of Ξij for a large range of
00423                                         //  outliers."
00424                                         EdgeSwitchPrior * prior = new EdgeSwitchPrior();
00425                                         prior->setMeasurement(1.0);
00426                                         prior->setVertex(0, v);
00427                                         UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
00428                                 }
00429 #endif
00430 
00431                                 if(isSlam2d())
00432                                 {
00433                                         Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
00434                                         if(!isCovarianceIgnored())
00435                                         {
00436                                                 information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
00437                                                 information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
00438                                                 information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
00439                                                 information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
00440                                                 information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
00441                                                 information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
00442                                                 information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
00443                                                 information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
00444                                                 information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
00445                                         }
00446 
00447 #if defined(RTABMAP_VERTIGO)
00448                                         if(this->isRobust() &&
00449                                            iter->second.type() != Link::kNeighbor  &&
00450                                            iter->second.type() != Link::kNeighborMerged)
00451                                         {
00452                                                 EdgeSE2Switchable * e = new EdgeSE2Switchable();
00453                                                 g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
00454                                                 g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
00455                                                 UASSERT(v1 != 0);
00456                                                 UASSERT(v2 != 0);
00457                                                 e->setVertex(0, v1);
00458                                                 e->setVertex(1, v2);
00459                                                 e->setVertex(2, v);
00460                                                 e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
00461                                                 e->setInformation(information);
00462                                                 edge = e;
00463                                         }
00464                                         else
00465 #endif
00466                                         {
00467                                                 g2o::EdgeSE2 * e = new g2o::EdgeSE2();
00468                                                 g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
00469                                                 g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
00470                                                 UASSERT(v1 != 0);
00471                                                 UASSERT(v2 != 0);
00472                                                 e->setVertex(0, v1);
00473                                                 e->setVertex(1, v2);
00474                                                 e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
00475                                                 e->setInformation(information);
00476                                                 edge = e;
00477                                         }
00478                                 }
00479                                 else
00480                                 {
00481                                         Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
00482                                         if(!isCovarianceIgnored())
00483                                         {
00484                                                 memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
00485                                         }
00486 
00487                                         Eigen::Affine3d a = iter->second.transform().toEigen3d();
00488                                         Eigen::Isometry3d constraint;
00489                                         constraint = a.linear();
00490                                         constraint.translation() = a.translation();
00491 
00492 #if defined(RTABMAP_VERTIGO)
00493                                         if(this->isRobust() &&
00494                                            iter->second.type() != Link::kNeighbor &&
00495                                            iter->second.type() != Link::kNeighborMerged)
00496                                         {
00497                                                 EdgeSE3Switchable * e = new EdgeSE3Switchable();
00498                                                 g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
00499                                                 g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
00500                                                 UASSERT(v1 != 0);
00501                                                 UASSERT(v2 != 0);
00502                                                 e->setVertex(0, v1);
00503                                                 e->setVertex(1, v2);
00504                                                 e->setVertex(2, v);
00505                                                 e->setMeasurement(constraint);
00506                                                 e->setInformation(information);
00507                                                 edge = e;
00508                                         }
00509                                         else
00510 #endif
00511                                         {
00512                                                 g2o::EdgeSE3 * e = new g2o::EdgeSE3();
00513                                                 g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
00514                                                 g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
00515                                                 UASSERT(v1 != 0);
00516                                                 UASSERT(v2 != 0);
00517                                                 e->setVertex(0, v1);
00518                                                 e->setVertex(1, v2);
00519                                                 e->setMeasurement(constraint);
00520                                                 e->setInformation(information);
00521                                                 edge = e;
00522                                         }
00523                                 }
00524                         }
00525 
00526                         if (edge && !optimizer.addEdge(edge))
00527                         {
00528                                 delete edge;
00529                                 UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
00530                         }
00531 
00532 
00533                 }
00534 
00535                 UDEBUG("Initial optimization...");
00536                 optimizer.initializeOptimization();
00537 
00538                 UASSERT_MSG(optimizer.verifyInformationMatrices(true),
00539                                 "This error can be caused by (1) bad covariance matrix "
00540                                 "set in odometry messages "
00541                                 "(see requirements in g2o::OptimizableGraph::verifyInformationMatrices() function) "
00542                                 "or that (2) PCL and g2o hadn't "
00543                                 "been built both with or without \"-march=native\" compilation "
00544                                 "flag (if one library is built with this flag and not the other, "
00545                                 "this is causing Eigen to not work properly, resulting in segmentation faults).");
00546 
00547                 UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
00548                 int it = 0;
00549                 UTimer timer;
00550                 double lastError = 0.0;
00551                 if(intermediateGraphes || this->epsilon() > 0.0)
00552                 {
00553                         for(int i=0; i<iterations(); ++i)
00554                         {
00555                                 if(intermediateGraphes)
00556                                 {
00557                                         if(i > 0)
00558                                         {
00559                                                 std::map<int, Transform> tmpPoses;
00560                                                 if(isSlam2d())
00561                                                 {
00562                                                         for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00563                                                         {
00564                                                                 const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(iter->first);
00565                                                                 if(v)
00566                                                                 {
00567                                                                         float roll, pitch, yaw;
00568                                                                         iter->second.getEulerAngles(roll, pitch, yaw);
00569                                                                         Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
00570                                                                         tmpPoses.insert(std::pair<int, Transform>(iter->first, t));
00571                                                                         UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
00572                                                                 }
00573                                                                 else
00574                                                                 {
00575                                                                         UERROR("Vertex %d not found!?", iter->first);
00576                                                                 }
00577                                                         }
00578                                                 }
00579                                                 else
00580                                                 {
00581                                                         for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00582                                                         {
00583                                                                 const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(iter->first);
00584                                                                 if(v)
00585                                                                 {
00586                                                                         Transform t = Transform::fromEigen3d(v->estimate());
00587                                                                         tmpPoses.insert(std::pair<int, Transform>(iter->first, t));
00588                                                                         UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
00589                                                                 }
00590                                                                 else
00591                                                                 {
00592                                                                         UERROR("Vertex %d not found!?", iter->first);
00593                                                                 }
00594                                                         }
00595                                                 }
00596                                                 intermediateGraphes->push_back(tmpPoses);
00597                                         }
00598                                 }
00599 
00600                                 it += optimizer.optimize(1);
00601 
00602                                 // early stop condition
00603                                 optimizer.computeActiveErrors();
00604                                 double chi2 = optimizer.activeRobustChi2();
00605                                 UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
00606 
00607                                 if(i>0 && optimizer.activeRobustChi2() > 1000000000000.0)
00608                                 {
00609                                         UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
00610                                         return optimizedPoses;
00611                                 }
00612 
00613                                 double errorDelta = lastError - chi2;
00614                                 if(i>0 && errorDelta < this->epsilon())
00615                                 {
00616                                         if(errorDelta < 0)
00617                                         {
00618                                                 UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
00619                                         }
00620                                         else
00621                                         {
00622                                                 UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
00623                                                 break;
00624                                         }
00625                                 }
00626                                 else if(i==0 && chi2 < this->epsilon())
00627                                 {
00628                                         UINFO("Stop optimizing, error is already under epsilon (%f < %f)", chi2, this->epsilon());
00629                                         break;
00630                                 }
00631                                 lastError = chi2;
00632                         }
00633                 }
00634                 else
00635                 {
00636                         it = optimizer.optimize(iterations());
00637                         optimizer.computeActiveErrors();
00638                         UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
00639                 }
00640                 if(finalError)
00641                 {
00642                         *finalError = lastError;
00643                 }
00644                 if(iterationsDone)
00645                 {
00646                         *iterationsDone = it;
00647                 }
00648                 UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
00649 
00650                 if(optimizer.activeRobustChi2() > 1000000000000.0)
00651                 {
00652                         UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
00653                         return optimizedPoses;
00654                 }
00655 
00656                 if(isSlam2d())
00657                 {
00658                         for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00659                         {
00660                                 const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(iter->first);
00661                                 if(v)
00662                                 {
00663                                         float roll, pitch, yaw;
00664                                         iter->second.getEulerAngles(roll, pitch, yaw);
00665                                         Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
00666                                         optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
00667                                         UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
00668                                 }
00669                                 else
00670                                 {
00671                                         UERROR("Vertex %d not found!?", iter->first);
00672                                 }
00673                         }
00674 
00675                         g2o::VertexSE2* v = (g2o::VertexSE2*)optimizer.vertex(poses.rbegin()->first);
00676                         if(v)
00677                         {
00678                                 UTimer t;
00679                                 g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
00680                                 optimizer.computeMarginals(spinv, v);
00681                                 UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
00682                                 if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
00683                                 {
00684                                         g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
00685                                         UASSERT(block && block->cols() == 3 && block->cols() == 3);
00686                                         outputCovariance.at<double>(0,0) = (*block)(0,0); // x-x
00687                                         outputCovariance.at<double>(0,1) = (*block)(0,1); // x-y
00688                                         outputCovariance.at<double>(0,5) = (*block)(0,2); // x-theta
00689                                         outputCovariance.at<double>(1,0) = (*block)(1,0); // y-x
00690                                         outputCovariance.at<double>(1,1) = (*block)(1,1); // y-y
00691                                         outputCovariance.at<double>(1,5) = (*block)(1,2); // y-theta
00692                                         outputCovariance.at<double>(5,0) = (*block)(2,0); // theta-x
00693                                         outputCovariance.at<double>(5,1) = (*block)(2,1); // theta-y
00694                                         outputCovariance.at<double>(5,5) = (*block)(2,2); // theta-theta
00695                                 }
00696                                 else if(v->hessianIndex() < 0)
00697                                 {
00698                                         UWARN("Computing marginals: vertex %d has negative hessian index (%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex());
00699                                 }
00700                                 else
00701                                 {
00702                                         UWARN("Computing marginals: vertex %d has hessian not valid (%d > block size=%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex(), (int)spinv.blockCols().size());
00703                                 }
00704                         }
00705                         else
00706                         {
00707                                 UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
00708                         }
00709                 }
00710                 else
00711                 {
00712                         for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00713                         {
00714                                 const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(iter->first);
00715                                 if(v)
00716                                 {
00717                                         Transform t = Transform::fromEigen3d(v->estimate());
00718                                         optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
00719                                         UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
00720                                 }
00721                                 else
00722                                 {
00723                                         UERROR("Vertex %d not found!?", iter->first);
00724                                 }
00725                         }
00726 
00727                         g2o::VertexSE3* v = (g2o::VertexSE3*)optimizer.vertex(poses.rbegin()->first);
00728                         if(v)
00729                         {
00730                                 UTimer t;
00731                                 g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
00732                                 optimizer.computeMarginals(spinv, v);
00733                                 UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
00734                                 if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
00735                                 {
00736                                         g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
00737                                         UASSERT(block && block->cols() == 6 && block->cols() == 6);
00738                                         memcpy(outputCovariance.data, block->data(), outputCovariance.total()*sizeof(double));
00739                                 }
00740                                 else if(v->hessianIndex() < 0)
00741                                 {
00742                                         UWARN("Computing marginals: vertex %d has negative hessian index (%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex());
00743                                 }
00744 #ifdef RTABMAP_G2O_CPP11
00745                                 else
00746                                 {
00747                                         UWARN("Computing marginals: vertex %d has hessian not valid (%d > block size=%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex(), (int)spinv.blockCols().size());
00748                                 }
00749 #endif
00750                         }
00751                         else
00752                         {
00753                                 UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
00754                         }
00755                 }
00756         }
00757         else if(poses.size() == 1 || iterations() <= 0)
00758         {
00759                 optimizedPoses = poses;
00760         }
00761         else
00762         {
00763                 UWARN("This method should be called at least with 1 pose!");
00764         }
00765         UDEBUG("Optimizing graph...end!");
00766 #else
00767 #ifdef RTABMAP_ORB_SLAM2
00768         UERROR("G2O graph optimization cannot be used with g2o built from ORB_SLAM2, only SBA is available.");
00769 #else
00770         UERROR("Not built with G2O support!");
00771 #endif
00772 #endif
00773         return optimizedPoses;
00774 }
00775 
00776 #ifdef RTABMAP_ORB_SLAM2
00777 
00780  class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>
00781 {
00782   public:
00783     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00784     EdgeSE3Expmap():  BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){}
00785     bool read(std::istream& is)
00786       {
00787         return false;
00788       }
00789 
00790       bool write(std::ostream& os) const
00791       {
00792         return false;
00793       }
00794 
00795     void computeError()
00796     {
00797       const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
00798       const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
00799       g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
00800       _error[0]=delta.translation().x();
00801       _error[1]=delta.translation().y();
00802       _error[2]=delta.translation().z();
00803       _error[3]=delta.rotation().x();
00804       _error[4]=delta.rotation().y();
00805       _error[5]=delta.rotation().z();
00806     }
00807 
00808     virtual void setMeasurement(const g2o::SE3Quat& meas){
00809       _measurement=meas;
00810       _inverseMeasurement=meas.inverse();
00811     }
00812 
00813     virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;}
00814     virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){
00815         g2o::VertexSE3Expmap* from = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
00816         g2o::VertexSE3Expmap* to = static_cast<g2o::VertexSE3Expmap*>(_vertices[1]);
00817                 if (from_.count(from) > 0)
00818                   to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement);
00819                 else
00820                   from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement);
00821     }
00822 
00823     virtual bool setMeasurementData(const double* d){
00824       Eigen::Map<const g2o::Vector7d> v(d);
00825       _measurement.fromVector(v);
00826       _inverseMeasurement = _measurement.inverse();
00827       return true;
00828     }
00829 
00830     virtual bool getMeasurementData(double* d) const{
00831       Eigen::Map<g2o::Vector7d> v(d);
00832       v = _measurement.toVector();
00833       return true;
00834     }
00835 
00836     virtual int measurementDimension() const {return 7;}
00837 
00838     virtual bool setMeasurementFromState() {
00839         const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
00840                 const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
00841                 _measurement = (v1->estimate().inverse()*v2->estimate());
00842                 _inverseMeasurement = _measurement.inverse();
00843                 return true;
00844     }
00845 
00846   protected:
00847     g2o::SE3Quat _inverseMeasurement;
00848 };
00849 #endif
00850 
00851 std::map<int, Transform> OptimizerG2O::optimizeBA(
00852                 int rootId,
00853                 const std::map<int, Transform> & poses,
00854                 const std::multimap<int, Link> & links,
00855                 const std::map<int, CameraModel> & models,
00856                 std::map<int, cv::Point3f> & points3DMap,
00857                 const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
00858                 std::set<int> * outliers)
00859 {
00860         std::map<int, Transform> optimizedPoses;
00861 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
00862         UDEBUG("Optimizing graph...");
00863 
00864         optimizedPoses.clear();
00865         if(poses.size()>=2 && iterations() > 0 && models.size() == poses.size())
00866         {
00867                 g2o::SparseOptimizer optimizer;
00868                 optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
00869 #if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM2)
00870                 std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
00871 #else
00872                 g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
00873 #endif
00874 
00875 #ifdef RTABMAP_ORB_SLAM2
00876                 linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
00877 #else
00878                 if(solver_ == 3)
00879                 {
00880                         //eigen
00881 #ifdef RTABMAP_G2O_CPP11
00882                         linearSolver = g2o::make_unique<g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType> >();
00883 #else
00884                         linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
00885 #endif
00886                 }
00887 #ifdef G2O_HAVE_CHOLMOD
00888                 else if(solver_ == 2)
00889                 {
00890                         //chmold
00891 #ifdef RTABMAP_G2O_CPP11
00892                         linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> >();
00893 #else
00894                         linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
00895 #endif
00896                 }
00897 #endif
00898 #ifdef G2O_HAVE_CSPARSE
00899                 else if(solver_ == 0)
00900                 {
00901                         //csparse
00902 #ifdef RTABMAP_G2O_CPP11
00903                         linearSolver = g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType> >();
00904 #else
00905                         linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
00906 #endif
00907                 }
00908 #endif
00909                 else
00910                 {
00911                         //pcg
00912 #ifdef RTABMAP_G2O_CPP11
00913                         linearSolver = g2o::make_unique<g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType> >();
00914 #else
00915                         linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
00916 #endif
00917                 }
00918 #endif // RTABMAP_ORB_SLAM2
00919 
00920 #ifndef RTABMAP_ORB_SLAM2
00921                 if(optimizer_ == 1)
00922                 {
00923 #ifdef RTABMAP_G2O_CPP11
00924                         optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(
00925                                         g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
00926 #else
00927                         optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(new g2o::BlockSolver_6_3(linearSolver)));
00928 #endif
00929                 }
00930                 else
00931 #endif
00932                 {
00933 #if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM2)
00934                         optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
00935                                         g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
00936 #else
00937                         optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(new g2o::BlockSolver_6_3(linearSolver)));
00938 #endif
00939                 }
00940 
00941 
00942                 UDEBUG("fill poses to g2o...");
00943                 for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); )
00944                 {
00945                         // Get camera model
00946                         std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
00947                         UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
00948 
00949                         Transform camPose = iter->second * iterModel->second.localTransform();
00950 
00951                         // Add node's pose
00952                         UASSERT(!camPose.isNull());
00953 #ifdef RTABMAP_ORB_SLAM2
00954                         g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
00955 #else
00956                         g2o::VertexCam * vCam = new g2o::VertexCam();
00957 #endif
00958 
00959                         Eigen::Affine3d a = camPose.toEigen3d();
00960 #ifdef RTABMAP_ORB_SLAM2
00961                         a = a.inverse();
00962                         vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
00963 #else
00964                         g2o::SBACam cam(Eigen::Quaterniond(a.linear()), a.translation());
00965                         cam.setKcam(
00966                                         iterModel->second.fx(),
00967                                         iterModel->second.fy(),
00968                                         iterModel->second.cx(),
00969                                         iterModel->second.cy(),
00970                                         iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters
00971                         vCam->setEstimate(cam);
00972 #endif
00973                         vCam->setId(iter->first);
00974 
00975                         // negative root means that all other poses should be fixed instead of the root
00976                         vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
00977 
00978                         UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
00979                                         iter->first,
00980                                         vCam->fixed()?1:0,
00981                                         iterModel->second.fx(),
00982                                         iterModel->second.fy(),
00983                                         iterModel->second.cx(),
00984                                         iterModel->second.cy(),
00985                                         iterModel->second.Tx(),
00986                                         iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_,
00987                                         camPose.prettyPrint().c_str());
00988 
00989                         UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
00990 
00991                         ++iter;
00992                 }
00993 
00994                 UDEBUG("fill edges to g2o...");
00995                 for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
00996                 {
00997                         if(uContains(poses, iter->second.from()) &&
00998                            uContains(poses, iter->second.to()))
00999                         {
01000                                 // add edge
01001                                 int id1 = iter->second.from();
01002                                 int id2 = iter->second.to();
01003 
01004                                 if(id1 != id2) // not supporting prior
01005                                 {
01006                                         UASSERT(!iter->second.transform().isNull());
01007 
01008                                         Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
01009                                         if(!isCovarianceIgnored())
01010                                         {
01011                                                 memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
01012                                         }
01013 
01014                                         // between cameras, not base_link
01015                                         Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
01016                                         //UDEBUG("added edge %d->%d (in cam frame=%s)",
01017                                         //              id1,
01018                                         //              id2,
01019                                         //              camLink.prettyPrint().c_str());
01020 #ifdef RTABMAP_ORB_SLAM2
01021                                         EdgeSE3Expmap * e = new EdgeSE3Expmap();
01022                                         g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id1);
01023                                         g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id2);
01024 
01025                                         Transform camPose1 = Transform::fromEigen3d(v1->estimate()).inverse();
01026                                         Transform camPose2Inv = Transform::fromEigen3d(v2->estimate());
01027 
01028                                         camLink =  camPose1 * camPose1 * camLink * camPose2Inv * camPose2Inv;
01029 #else
01030                                         g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
01031                                         g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
01032                                         g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2);
01033 #endif
01034                                         UASSERT(v1 != 0);
01035                                         UASSERT(v2 != 0);
01036                                         e->setVertex(0, v1);
01037                                         e->setVertex(1, v2);
01038                                         Eigen::Affine3d a = camLink.toEigen3d();
01039                                         e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
01040                                         e->setInformation(information);
01041 
01042                                         if (!optimizer.addEdge(e))
01043                                         {
01044                                                 delete e;
01045                                                 UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
01046                                                 return optimizedPoses;
01047                                         }
01048                                 }
01049                         }
01050                 }
01051 
01052                 UDEBUG("fill 3D points to g2o...");
01053                 const int stepVertexId = poses.rbegin()->first+1;
01054                 int negVertexOffset = stepVertexId;
01055                 if(wordReferences.size() && wordReferences.rbegin()->first>0)
01056                 {
01057                         negVertexOffset += wordReferences.rbegin()->first;
01058                 }
01059                 UDEBUG("stepVertexId=%d, negVertexOffset=%d", stepVertexId, negVertexOffset);
01060                 std::list<g2o::OptimizableGraph::Edge*> edges;
01061                 for(std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
01062                 {
01063                         int id = iter->first;
01064                         if(points3DMap.find(id) != points3DMap.end())
01065                         {
01066                                 cv::Point3f pt3d = points3DMap.at(id);
01067                                 g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
01068 
01069                                 vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
01070                                 if(id<0)
01071                                 {
01072                                         vpt3d->setId(negVertexOffset + id*-1);
01073                                 }
01074                                 else
01075                                 {
01076                                         vpt3d->setId(stepVertexId + id);
01077                                 }
01078                                 UASSERT(vpt3d->id() > 0);
01079                                 vpt3d->setMarginalized(true);
01080                                 optimizer.addVertex(vpt3d);
01081 
01082                                 //UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
01083 
01084                                 // set observations
01085                                 for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
01086                                 {
01087                                         int camId = jter->first;
01088                                         if(poses.find(camId) != poses.end() && optimizer.vertex(camId) != 0)
01089                                         {
01090                                                 const cv::Point3f & pt = jter->second;
01091                                                 double depth = pt.z;
01092 
01093                                                 //UDEBUG("Added observation pt=%d to cam=%d (%f,%f) depth=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
01094 
01095                                                 g2o::OptimizableGraph::Edge * e;
01096                                                 double baseline = 0.0;
01097 #ifdef RTABMAP_ORB_SLAM2
01098                                                 g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
01099                                                 std::map<int, CameraModel>::const_iterator iterModel = models.find(camId);
01100 
01101                                                 UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
01102                                                 baseline = iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_;
01103 #else
01104                                                 g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
01105                                                 baseline = vcam->estimate().baseline;
01106 #endif
01107                                                 double variance = pixelVariance_;
01108                                                 if(uIsFinite(depth) && depth > 0.0 && baseline > 0.0)
01109                                                 {
01110                                                         // stereo edge
01111 #ifdef RTABMAP_ORB_SLAM2
01112                                                         g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
01113                                                         float disparity = baseline * iterModel->second.fx() / depth;
01114                                                         Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity);
01115                                                         es->setMeasurement(obs);
01116                                                         //variance *= log(exp(1)+disparity);
01117                                                         es->setInformation(Eigen::Matrix3d::Identity() / variance);
01118                                                         es->fx = iterModel->second.fx();
01119                                                         es->fy = iterModel->second.fy();
01120                                                         es->cx = iterModel->second.cx();
01121                                                         es->cy = iterModel->second.cy();
01122                                                         es->bf = baseline*es->fx;
01123                                                         e = es;
01124 #else
01125                                                         g2o::EdgeProjectP2SC* es = new g2o::EdgeProjectP2SC();
01126                                                         float disparity = baseline * vcam->estimate().Kcam(0,0) / depth;
01127                                                         Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity);
01128                                                         es->setMeasurement(obs);
01129                                                         //variance *= log(exp(1)+disparity);
01130                                                         es->setInformation(Eigen::Matrix3d::Identity() / variance);
01131                                                         e = es;
01132 #endif
01133                                                 }
01134                                                 else
01135                                                 {
01136                                                         if(baseline > 0.0)
01137                                                         {
01138                                                                 UDEBUG("Stereo camera model detected but current "
01139                                                                                 "observation (pt=%d to cam=%d) has null depth (%f m), adding "
01140                                                                                 "mono observation instead.",
01141                                                                                 vpt3d->id()-stepVertexId, camId, depth);
01142                                                         }
01143                                                         // mono edge
01144 #ifdef RTABMAP_ORB_SLAM2
01145                                                         g2o::EdgeSE3ProjectXYZ* em = new g2o::EdgeSE3ProjectXYZ();
01146                                                         Eigen::Vector2d obs( pt.x, pt.y);
01147                                                         em->setMeasurement(obs);
01148                                                         em->setInformation(Eigen::Matrix2d::Identity() / variance);
01149                                                         em->fx = iterModel->second.fx();
01150                                                         em->fy = iterModel->second.fy();
01151                                                         em->cx = iterModel->second.cx();
01152                                                         em->cy = iterModel->second.cy();
01153                                                         e = em;
01154 
01155 #else
01156                                                         g2o::EdgeProjectP2MC* em = new g2o::EdgeProjectP2MC();
01157                                                         Eigen::Vector2d obs( pt.x, pt.y);
01158                                                         em->setMeasurement(obs);
01159                                                         em->setInformation(Eigen::Matrix2d::Identity() / variance);
01160                                                         e = em;
01161 #endif
01162                                                 }
01163                                                 e->setVertex(0, vpt3d);
01164                                                 e->setVertex(1, vcam);
01165 
01166                                                 if(robustKernelDelta_ > 0.0)
01167                                                 {
01168                                                         g2o::RobustKernelHuber* kernel = new g2o::RobustKernelHuber;
01169                                                         kernel->setDelta(robustKernelDelta_);
01170                                                         e->setRobustKernel(kernel);
01171                                                 }
01172 
01173                                                 optimizer.addEdge(e);
01174                                                 edges.push_back(e);
01175                                         }
01176                                 }
01177                         }
01178                 }
01179 
01180                 UDEBUG("Initial optimization...");
01181                 optimizer.initializeOptimization();
01182 
01183                 UASSERT(optimizer.verifyInformationMatrices());
01184 
01185                 UINFO("g2o optimizing begin (max iterations=%d, robustKernel=%f)", iterations(), robustKernelDelta_);
01186 
01187                 int it = 0;
01188                 UTimer timer;
01189                 int outliersCount = 0;
01190                 int outliersCountFar = 0;
01191 
01192                 for(int i=0; i<(robustKernelDelta_>0.0?2:1); ++i)
01193                 {
01194                         it += optimizer.optimize(i==0&&robustKernelDelta_>0.0?5:iterations());
01195 
01196                         // early stop condition
01197                         optimizer.computeActiveErrors();
01198                         double chi2 = optimizer.activeRobustChi2();
01199 
01200                         if(uIsNan(chi2))
01201                         {
01202                                 UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
01203                                 return optimizedPoses;
01204                         }
01205                         UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
01206 
01207                         if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
01208                         {
01209                                 UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
01210                                 return optimizedPoses;
01211                         }
01212 
01213                         if(robustKernelDelta_>0.0)
01214                         {
01215                                 for(std::list<g2o::OptimizableGraph::Edge*>::iterator iter=edges.begin(); iter!=edges.end();++iter)
01216                                 {
01217                                         if((*iter)->level() == 0 && (*iter)->chi2() > (*iter)->robustKernel()->delta())
01218                                         {
01219                                                 (*iter)->setLevel(1);
01220                                                 ++outliersCount;
01221                                                 double d = 0.0;
01222 #ifdef RTABMAP_ORB_SLAM2
01223                                                 if(dynamic_cast<g2o::EdgeStereoSE3ProjectXYZ*>(*iter) != 0)
01224                                                 {
01225                                                         d = ((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[0]-((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[2];
01226                                                 }
01227                                                 UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
01228 #else
01229                                                 if(dynamic_cast<g2o::EdgeProjectP2SC*>(*iter) != 0)
01230                                                 {
01231                                                         d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
01232                                                 }
01233                                                 UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
01234 #endif
01235 
01236                                                 cv::Point3f pt3d;
01237                                                 if((*iter)->vertex(0)->id() > negVertexOffset)
01238                                                 {
01239                                                         pt3d = points3DMap.at(negVertexOffset - (*iter)->vertex(0)->id());
01240                                                 }
01241                                                 else
01242                                                 {
01243                                                         pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
01244                                                 }
01245                                                 ((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
01246 
01247                                                 if(outliers)
01248                                                 {
01249                                                         outliers->insert((*iter)->vertex(0)->id()-stepVertexId);
01250                                                 }
01251                                                 if(d < 5.0)
01252                                                 {
01253                                                         outliersCountFar++;
01254                                                 }
01255                                         }
01256                                         //(*iter)->setRobustKernel(0);
01257                                 }
01258                                 if(i==0)
01259                                         optimizer.initializeOptimization(0);
01260                                 UDEBUG("outliers=%d outliersCountFar=%d", outliersCount, outliersCountFar);
01261                         }
01262                 }
01263                 UINFO("g2o optimizing end (%d iterations done, error=%f, outliers=%d/%d (delta=%f) time = %f s)", it, optimizer.activeRobustChi2(), outliersCount, (int)edges.size(), robustKernelDelta_, timer.ticks());
01264 
01265                 if(optimizer.activeRobustChi2() > 1000000000000.0)
01266                 {
01267                         UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
01268                         return optimizedPoses;
01269                 }
01270 
01271                 // update poses
01272                 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
01273                 {
01274 #ifdef RTABMAP_ORB_SLAM2
01275                         const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(iter->first);
01276 #else
01277                         const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
01278 #endif
01279                         if(v)
01280                         {
01281                                 Transform t = Transform::fromEigen3d(v->estimate());
01282 
01283 #ifdef RTABMAP_ORB_SLAM2
01284                                 t=t.inverse();
01285 #endif
01286 
01287                                 // remove model local transform
01288                                 t *= models.at(iter->first).localTransform().inverse();
01289 
01290                                 UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
01291                                 if(t.isNull())
01292                                 {
01293                                         UERROR("Optimized pose %d is null!?!?", iter->first);
01294                                         optimizedPoses.clear();
01295                                         return optimizedPoses;
01296                                 }
01297 
01298                                 // FIXME: is there a way that we can add the 2D constraint directly in SBA?
01299                                 if(this->isSlam2d())
01300                                 {
01301                                         // get transform between old and new pose
01302                                         t = iter->second.inverse() * t;
01303                                         optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
01304                                 }
01305                                 else
01306                                 {
01307                                         optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
01308                                 }
01309                         }
01310                         else
01311                         {
01312                                 UERROR("Vertex (pose) %d not found!?", iter->first);
01313                         }
01314                 }
01315 
01316                 //update points3D
01317 
01318                 for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter!=points3DMap.end(); ++iter)
01319                 {
01320                         const g2o::VertexSBAPointXYZ* v;
01321                         int id = iter->first;
01322                         if(id<0)
01323                         {
01324                                 v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(negVertexOffset + id*-1);
01325                         }
01326                         else
01327                         {
01328                                 v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + id);
01329                         }
01330 
01331                         if(v)
01332                         {
01333                                 cv::Point3f p(v->estimate()[0], v->estimate()[1], v->estimate()[2]);
01334                                 //UDEBUG("%d from=%f,%f,%f to=%f,%f,%f", iter->first, iter->second.x, iter->second.y, iter->second.z, p.x, p.y, p.z);
01335                                 iter->second = p;
01336                         }
01337                         else
01338                         {
01339                                 iter->second.x = iter->second.y = iter->second.z = std::numeric_limits<float>::quiet_NaN();
01340                         }
01341                 }
01342         }
01343         else if(poses.size() > 1 && poses.size() != models.size())
01344         {
01345                 UERROR("This method should be called with size of poses = size camera models!");
01346         }
01347         else if(poses.size() == 1 || iterations() <= 0)
01348         {
01349                 optimizedPoses = poses;
01350         }
01351         else
01352         {
01353                 UWARN("This method should be called at least with 1 pose!");
01354         }
01355         UDEBUG("Optimizing graph...end!");
01356 #else
01357         UERROR("Not built with G2O support!");
01358 #endif
01359         return optimizedPoses;
01360 }
01361 
01362 bool OptimizerG2O::saveGraph(
01363                 const std::string & fileName,
01364                 const std::map<int, Transform> & poses,
01365                 const std::multimap<int, Link> & edgeConstraints,
01366                 bool useRobustConstraints)
01367 {
01368         FILE * file = 0;
01369 
01370 #ifdef _MSC_VER
01371         fopen_s(&file, fileName.c_str(), "w");
01372 #else
01373         file = fopen(fileName.c_str(), "w");
01374 #endif
01375 
01376         if(file)
01377         {
01378                 // VERTEX_SE3 id x y z qw qx qy qz
01379                 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
01380                 {
01381                         Eigen::Quaternionf q = iter->second.getQuaternionf();
01382                         fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n",
01383                                         iter->first,
01384                                         iter->second.x(),
01385                                         iter->second.y(),
01386                                         iter->second.z(),
01387                                         q.x(),
01388                                         q.y(),
01389                                         q.z(),
01390                                         q.w());
01391                 }
01392 
01393                 //EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
01394                 int virtualVertexId = poses.size()?poses.rbegin()->first+1:0;
01395                 for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
01396                 {
01397                         std::string prefix = "EDGE_SE3:QUAT";
01398                         std::string suffix = "";
01399 
01400                         if(useRobustConstraints &&
01401                            iter->second.type() != Link::kNeighbor &&
01402                            iter->second.type() != Link::kNeighborMerged)
01403                         {
01404                                 prefix = "EDGE_SE3_SWITCHABLE";
01405                                 fprintf(file, "VERTEX_SWITCH %d 1\n", virtualVertexId);
01406                                 fprintf(file, "EDGE_SWITCH_PRIOR %d 1 1.0\n", virtualVertexId);
01407                                 suffix = uFormat(" %d", virtualVertexId++);
01408                         }
01409 
01410                         Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
01411                         fprintf(file, "%s %d %d%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
01412                                         prefix.c_str(),
01413                                         iter->second.from(),
01414                                         iter->second.to(),
01415                                         suffix.c_str(),
01416                                         iter->second.transform().x(),
01417                                         iter->second.transform().y(),
01418                                         iter->second.transform().z(),
01419                                         q.x(),
01420                                         q.y(),
01421                                         q.z(),
01422                                         q.w(),
01423                                         iter->second.infMatrix().at<double>(0,0),
01424                                         iter->second.infMatrix().at<double>(0,1),
01425                                         iter->second.infMatrix().at<double>(0,2),
01426                                         iter->second.infMatrix().at<double>(0,3),
01427                                         iter->second.infMatrix().at<double>(0,4),
01428                                         iter->second.infMatrix().at<double>(0,5),
01429                                         iter->second.infMatrix().at<double>(1,1),
01430                                         iter->second.infMatrix().at<double>(1,2),
01431                                         iter->second.infMatrix().at<double>(1,3),
01432                                         iter->second.infMatrix().at<double>(1,4),
01433                                         iter->second.infMatrix().at<double>(1,5),
01434                                         iter->second.infMatrix().at<double>(2,2),
01435                                         iter->second.infMatrix().at<double>(2,3),
01436                                         iter->second.infMatrix().at<double>(2,4),
01437                                         iter->second.infMatrix().at<double>(2,5),
01438                                         iter->second.infMatrix().at<double>(3,3),
01439                                         iter->second.infMatrix().at<double>(3,4),
01440                                         iter->second.infMatrix().at<double>(3,5),
01441                                         iter->second.infMatrix().at<double>(4,4),
01442                                         iter->second.infMatrix().at<double>(4,5),
01443                                         iter->second.infMatrix().at<double>(5,5));
01444                 }
01445                 UINFO("Graph saved to %s", fileName.c_str());
01446                 fclose(file);
01447         }
01448         else
01449         {
01450                 UERROR("Cannot save to file %s", fileName.c_str());
01451                 return false;
01452         }
01453         return true;
01454 }
01455 
01456 } /* namespace rtabmap */

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