OptimizerG2O.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/utilite/ULogger.h>
29 #include <rtabmap/utilite/UStl.h>
30 #include <rtabmap/utilite/UMath.h>
31 #include <rtabmap/utilite/UConversion.h>
32 #include <rtabmap/utilite/UTimer.h>
33 #include <set>
34 
35 #include <rtabmap/core/Version.h>
36 #include <rtabmap/core/OptimizerG2O.h>
37 #include <rtabmap/core/util3d_transforms.h>
38 #include <rtabmap/core/util3d_motion_estimation.h>
39 #include <rtabmap/core/util3d.h>
40 
41 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
42 #include "g2o/core/sparse_optimizer.h"
43 #include "g2o/core/block_solver.h"
44 #include "g2o/core/factory.h"
45 #include "g2o/core/optimization_algorithm_factory.h"
46 #include "g2o/core/optimization_algorithm_gauss_newton.h"
47 #include "g2o/core/optimization_algorithm_levenberg.h"
48 #include "g2o/core/robust_kernel_impl.h"
49 namespace g2o {
50 // bug #include "g2o/core/eigen_types.h" not found on Indigo
51 typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixXD;
52 }
53 
54 #ifdef RTABMAP_G2O
55 #include "g2o/types/sba/types_sba.h"
56 #include "g2o/solvers/eigen/linear_solver_eigen.h"
57 #include "g2o/config.h"
58 #include "g2o/types/slam2d/types_slam2d.h"
59 #include "g2o/types/slam3d/types_slam3d.h"
60 #ifdef G2O_HAVE_CSPARSE
61 #include "g2o/solvers/csparse/linear_solver_csparse.h"
62 #endif
63 #include "g2o/solvers/pcg/linear_solver_pcg.h"
64 #ifdef G2O_HAVE_CHOLMOD
65 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
66 #endif
67 enum {
68  PARAM_OFFSET=0,
69 };
70 #endif // RTABMAP_G2O
71 
72 #ifdef RTABMAP_ORB_SLAM2
73 #include "g2o/types/types_sba.h"
74 #include "g2o/types/types_six_dof_expmap.h"
75 #include "g2o/solvers/linear_solver_eigen.h"
76 #endif
77 
78 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
79 typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver;
80 #ifdef RTABMAP_G2O
81 typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
82 #ifdef G2O_HAVE_CSPARSE
83 typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
84 #endif
85 #ifdef G2O_HAVE_CHOLMOD
86 typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearCholmodSolver;
87 #endif
88 
89 #if defined(RTABMAP_VERTIGO)
90 #include "vertigo/g2o/edge_switchPrior.h"
91 #include "vertigo/g2o/edge_se2Switchable.h"
92 #include "vertigo/g2o/edge_se3Switchable.h"
93 #include "vertigo/g2o/vertex_switchLinear.h"
94 #endif
95 #endif
96 
97 #endif // end defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
98 
99 namespace rtabmap {
100 
101 bool OptimizerG2O::available()
102 {
103 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
104  return true;
105 #else
106  return false;
107 #endif
108 }
109 
110 bool OptimizerG2O::isCSparseAvailable()
111 {
112 #ifdef G2O_HAVE_CSPARSE
113  return true;
114 #else
115  return false;
116 #endif
117 }
118 
119 bool OptimizerG2O::isCholmodAvailable()
120 {
121 #ifdef G2O_HAVE_CHOLMOD
122  return true;
123 #else
124  return false;
125 #endif
126 }
127 
128 void OptimizerG2O::parseParameters(const ParametersMap & parameters)
129 {
130  Optimizer::parseParameters(parameters);
131 
132  Parameters::parse(parameters, Parameters::kg2oSolver(), solver_);
133  Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_);
134  Parameters::parse(parameters, Parameters::kg2oPixelVariance(), pixelVariance_);
135  Parameters::parse(parameters, Parameters::kg2oRobustKernelDelta(), robustKernelDelta_);
136  Parameters::parse(parameters, Parameters::kg2oBaseline(), baseline_);
137  UASSERT(pixelVariance_ > 0.0);
138  UASSERT(baseline_ >= 0.0);
139 
140 #ifdef RTABMAP_ORB_SLAM2
141  if(solver_ != 3)
142  {
143  UWARN("g2o built with ORB_SLAM2 has only Eigen solver available, using Eigen=3 instead of %d.", solver_);
144  solver_ = 3;
145  }
146 #else
147 #ifndef G2O_HAVE_CHOLMOD
148  if(solver_ == 2)
149  {
150  UWARN("g2o is not built with chmold, so it cannot be used as solver. Using CSparse instead.");
151  solver_ = 0;
152  }
153 #endif
154 
155 #ifndef G2O_HAVE_CSPARSE
156  if(solver_ == 0)
157  {
158  UWARN("g2o is not built with csparse, so it cannot be used as solver. Using PCG instead.");
159  solver_ = 1;
160  }
161 #endif
162 #endif
163 
164 }
165 
166 std::map<int, Transform> OptimizerG2O::optimize(
167  int rootId,
168  const std::map<int, Transform> & poses,
169  const std::multimap<int, Link> & edgeConstraints,
170  cv::Mat & outputCovariance,
171  std::list<std::map<int, Transform> > * intermediateGraphes,
172  double * finalError,
173  int * iterationsDone)
174 {
175  outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
176  std::map<int, Transform> optimizedPoses;
177 #ifdef RTABMAP_G2O
178  UDEBUG("Optimizing graph...");
179 
180 #ifndef RTABMAP_VERTIGO
181  if(this->isRobust())
182  {
183  UWARN("Vertigo robust optimization is not available! Robust optimization is now disabled.");
184  setRobust(false);
185  }
186 #endif
187 
188  optimizedPoses.clear();
189  if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
190  {
191  // Apply g2o optimization
192 
193  g2o::SparseOptimizer optimizer;
194  optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
195  g2o::ParameterSE3Offset* odomOffset = new g2o::ParameterSE3Offset();
196  odomOffset->setId(PARAM_OFFSET);
197  optimizer.addParameter(odomOffset);
198 
199 #ifdef RTABMAP_G2O_CPP11
200 
201  std::unique_ptr<SlamBlockSolver> blockSolver;
202 
203  if(solver_ == 3)
204  {
205  //eigen
206  auto linearSolver = g2o::make_unique<SlamLinearEigenSolver>();
207  linearSolver->setBlockOrdering(false);
208  blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
209  }
210 #ifdef G2O_HAVE_CHOLMOD
211  else if(solver_ == 2)
212  {
213  //chmold
214  auto linearSolver = g2o::make_unique<SlamLinearCholmodSolver>();
215  linearSolver->setBlockOrdering(false);
216  blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
217  }
218 #endif
219 #ifdef G2O_HAVE_CSPARSE
220  else if(solver_ == 0)
221  {
222 
223  //csparse
224  auto linearSolver = g2o::make_unique<SlamLinearCSparseSolver>();
225  linearSolver->setBlockOrdering(false);
226  blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
227  }
228 #endif
229  else
230  {
231  //pcg
232  auto linearSolver = g2o::make_unique<SlamLinearPCGSolver>();
233  blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
234  }
235 
236  if(optimizer_ == 1)
237  {
238 
239  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(std::move(blockSolver)));
240  }
241  else
242  {
243  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver)));
244  }
245 
246 #else
247 
248  SlamBlockSolver * blockSolver = 0;
249 
250  if(solver_ == 3)
251  {
252  //eigen
253  SlamLinearEigenSolver * linearSolver = new SlamLinearEigenSolver();
254  linearSolver->setBlockOrdering(false);
255  blockSolver = new SlamBlockSolver(linearSolver);
256  }
257 #ifdef G2O_HAVE_CHOLMOD
258  else if(solver_ == 2)
259  {
260  //chmold
261  SlamLinearCholmodSolver * linearSolver = new SlamLinearCholmodSolver();
262  linearSolver->setBlockOrdering(false);
263  blockSolver = new SlamBlockSolver(linearSolver);
264  }
265 #endif
266 #ifdef G2O_HAVE_CSPARSE
267  else if(solver_ == 0)
268  {
269  //csparse
270  SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver();
271  linearSolver->setBlockOrdering(false);
272  blockSolver = new SlamBlockSolver(linearSolver);
273  }
274 #endif
275  else
276  {
277  //pcg
278  SlamLinearPCGSolver * linearSolver = new SlamLinearPCGSolver();
279  blockSolver = new SlamBlockSolver(linearSolver);
280  }
281 
282  if(optimizer_ == 1)
283  {
284  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver));
285  }
286  else
287  {
288  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver));
289  }
290 #endif
291  // detect if there is a global pose prior set, if so remove rootId
292  if(!priorsIgnored())
293  {
294  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
295  {
296  if(iter->second.from() == iter->second.to())
297  {
298  rootId = 0;
299  break;
300  }
301  }
302  }
303 
304  UDEBUG("fill poses to g2o...");
305  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
306  {
307  UASSERT(!iter->second.isNull());
308  g2o::HyperGraph::Vertex * vertex = 0;
309  if(isSlam2d())
310  {
311  g2o::VertexSE2 * v2 = new g2o::VertexSE2();
312  v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
313  if(iter->first == rootId)
314  {
315  v2->setFixed(true);
316  }
317  vertex = v2;
318  }
319  else
320  {
321  g2o::VertexSE3 * v3 = new g2o::VertexSE3();
322 
323  Eigen::Affine3d a = iter->second.toEigen3d();
324  Eigen::Isometry3d pose;
325  pose = a.linear();
326  pose.translation() = a.translation();
327  v3->setEstimate(pose);
328  if(iter->first == rootId)
329  {
330  v3->setFixed(true);
331  }
332  vertex = v3;
333  }
334  vertex->setId(iter->first);
335  UASSERT_MSG(optimizer.addVertex(vertex), uFormat("cannot insert vertex %d!?", iter->first).c_str());
336  }
337 
338  UDEBUG("fill edges to g2o...");
339 #if defined(RTABMAP_VERTIGO)
340  int vertigoVertexId = poses.rbegin()->first+1;
341 #endif
342  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
343  {
344  int id1 = iter->second.from();
345  int id2 = iter->second.to();
346 
347  UASSERT(!iter->second.transform().isNull());
348 
349  g2o::HyperGraph::Edge * edge = 0;
350 
351  if(id1 == id2)
352  {
353  if(!priorsIgnored())
354  {
355  if(isSlam2d())
356  {
357  g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
358  g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
359  priorEdge->setVertex(0, v1);
360  priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
361  priorEdge->setParameterId(0, PARAM_OFFSET);
362  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
363  if(!isCovarianceIgnored())
364  {
365  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
366  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
367  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
368  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
369  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
370  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
371  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
372  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
373  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
374  }
375  priorEdge->setInformation(information);
376  edge = priorEdge;
377  }
378  else
379  {
380  g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
381  g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
382  priorEdge->setVertex(0, v1);
383  Eigen::Affine3d a = iter->second.transform().toEigen3d();
384  Eigen::Isometry3d pose;
385  pose = a.linear();
386  pose.translation() = a.translation();
387  priorEdge->setMeasurement(pose);
388  priorEdge->setParameterId(0, PARAM_OFFSET);
389  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
390  if(!isCovarianceIgnored())
391  {
392  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
393  }
394  priorEdge->setInformation(information);
395  edge = priorEdge;
396  }
397  }
398  }
399  else
400  {
401 #if defined(RTABMAP_VERTIGO)
402  VertexSwitchLinear * v = 0;
403  if(this->isRobust() &&
404  iter->second.type() != Link::kNeighbor &&
405  iter->second.type() != Link::kNeighborMerged)
406  {
407  // For loop closure links, add switchable edges
408 
409  // create new switch variable
410  // Sunderhauf IROS 2012:
411  // "Since it is reasonable to initially accept all loop closure constraints,
412  // a proper and convenient initial value for all switch variables would be
413  // sij = 1 when using the linear switch function"
414  v = new VertexSwitchLinear();
415  v->setEstimate(1.0);
416  v->setId(vertigoVertexId++);
417  UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str());
418 
419  // create switch prior factor
420  // "If the front-end is not able to assign sound individual values
421  // for Ξij , it is save to set all Ξij = 1, since this value is close
422  // to the individual optimal choice of Ξij for a large range of
423  // outliers."
424  EdgeSwitchPrior * prior = new EdgeSwitchPrior();
425  prior->setMeasurement(1.0);
426  prior->setVertex(0, v);
427  UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
428  }
429 #endif
430 
431  if(isSlam2d())
432  {
433  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
434  if(!isCovarianceIgnored())
435  {
436  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
437  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
438  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
439  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
440  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
441  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
442  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
443  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
444  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
445  }
446 
447 #if defined(RTABMAP_VERTIGO)
448  if(this->isRobust() &&
449  iter->second.type() != Link::kNeighbor &&
450  iter->second.type() != Link::kNeighborMerged)
451  {
452  EdgeSE2Switchable * e = new EdgeSE2Switchable();
453  g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
454  g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
455  UASSERT(v1 != 0);
456  UASSERT(v2 != 0);
457  e->setVertex(0, v1);
458  e->setVertex(1, v2);
459  e->setVertex(2, v);
460  e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
461  e->setInformation(information);
462  edge = e;
463  }
464  else
465 #endif
466  {
467  g2o::EdgeSE2 * e = new g2o::EdgeSE2();
468  g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
469  g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
470  UASSERT(v1 != 0);
471  UASSERT(v2 != 0);
472  e->setVertex(0, v1);
473  e->setVertex(1, v2);
474  e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
475  e->setInformation(information);
476  edge = e;
477  }
478  }
479  else
480  {
481  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
482  if(!isCovarianceIgnored())
483  {
484  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
485  }
486 
487  Eigen::Affine3d a = iter->second.transform().toEigen3d();
488  Eigen::Isometry3d constraint;
489  constraint = a.linear();
490  constraint.translation() = a.translation();
491 
492 #if defined(RTABMAP_VERTIGO)
493  if(this->isRobust() &&
494  iter->second.type() != Link::kNeighbor &&
495  iter->second.type() != Link::kNeighborMerged)
496  {
497  EdgeSE3Switchable * e = new EdgeSE3Switchable();
498  g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
499  g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
500  UASSERT(v1 != 0);
501  UASSERT(v2 != 0);
502  e->setVertex(0, v1);
503  e->setVertex(1, v2);
504  e->setVertex(2, v);
505  e->setMeasurement(constraint);
506  e->setInformation(information);
507  edge = e;
508  }
509  else
510 #endif
511  {
512  g2o::EdgeSE3 * e = new g2o::EdgeSE3();
513  g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
514  g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
515  UASSERT(v1 != 0);
516  UASSERT(v2 != 0);
517  e->setVertex(0, v1);
518  e->setVertex(1, v2);
519  e->setMeasurement(constraint);
520  e->setInformation(information);
521  edge = e;
522  }
523  }
524  }
525 
526  if (edge && !optimizer.addEdge(edge))
527  {
528  delete edge;
529  UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
530  }
531 
532 
533  }
534 
535  UDEBUG("Initial optimization...");
536  optimizer.initializeOptimization();
537 
538  UASSERT_MSG(optimizer.verifyInformationMatrices(true),
539  "This error can be caused by (1) bad covariance matrix "
540  "set in odometry messages "
541  "(see requirements in g2o::OptimizableGraph::verifyInformationMatrices() function) "
542  "or that (2) PCL and g2o hadn't "
543  "been built both with or without \"-march=native\" compilation "
544  "flag (if one library is built with this flag and not the other, "
545  "this is causing Eigen to not work properly, resulting in segmentation faults).");
546 
547  UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
548  int it = 0;
549  UTimer timer;
550  double lastError = 0.0;
551  if(intermediateGraphes || this->epsilon() > 0.0)
552  {
553  for(int i=0; i<iterations(); ++i)
554  {
555  if(intermediateGraphes)
556  {
557  if(i > 0)
558  {
559  std::map<int, Transform> tmpPoses;
560  if(isSlam2d())
561  {
562  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
563  {
564  const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(iter->first);
565  if(v)
566  {
567  float roll, pitch, yaw;
568  iter->second.getEulerAngles(roll, pitch, yaw);
569  Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
570  tmpPoses.insert(std::pair<int, Transform>(iter->first, t));
571  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
572  }
573  else
574  {
575  UERROR("Vertex %d not found!?", iter->first);
576  }
577  }
578  }
579  else
580  {
581  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
582  {
583  const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(iter->first);
584  if(v)
585  {
586  Transform t = Transform::fromEigen3d(v->estimate());
587  tmpPoses.insert(std::pair<int, Transform>(iter->first, t));
588  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
589  }
590  else
591  {
592  UERROR("Vertex %d not found!?", iter->first);
593  }
594  }
595  }
596  intermediateGraphes->push_back(tmpPoses);
597  }
598  }
599 
600  it += optimizer.optimize(1);
601 
602  // early stop condition
603  optimizer.computeActiveErrors();
604  double chi2 = optimizer.activeRobustChi2();
605  UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
606 
607  if(i>0 && optimizer.activeRobustChi2() > 1000000000000.0)
608  {
609  UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
610  return optimizedPoses;
611  }
612 
613  double errorDelta = lastError - chi2;
614  if(i>0 && errorDelta < this->epsilon())
615  {
616  if(errorDelta < 0)
617  {
618  UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
619  }
620  else
621  {
622  UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
623  break;
624  }
625  }
626  else if(i==0 && chi2 < this->epsilon())
627  {
628  UINFO("Stop optimizing, error is already under epsilon (%f < %f)", chi2, this->epsilon());
629  break;
630  }
631  lastError = chi2;
632  }
633  }
634  else
635  {
636  it = optimizer.optimize(iterations());
637  optimizer.computeActiveErrors();
638  UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
639  }
640  if(finalError)
641  {
642  *finalError = lastError;
643  }
644  if(iterationsDone)
645  {
646  *iterationsDone = it;
647  }
648  UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
649 
650  if(optimizer.activeRobustChi2() > 1000000000000.0)
651  {
652  UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
653  return optimizedPoses;
654  }
655 
656  if(isSlam2d())
657  {
658  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
659  {
660  const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(iter->first);
661  if(v)
662  {
663  float roll, pitch, yaw;
664  iter->second.getEulerAngles(roll, pitch, yaw);
665  Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
666  optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
667  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
668  }
669  else
670  {
671  UERROR("Vertex %d not found!?", iter->first);
672  }
673  }
674 
675  g2o::VertexSE2* v = (g2o::VertexSE2*)optimizer.vertex(poses.rbegin()->first);
676  if(v)
677  {
678  UTimer t;
679  g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
680  optimizer.computeMarginals(spinv, v);
681  UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
682  if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
683  {
684  g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
685  UASSERT(block && block->cols() == 3 && block->cols() == 3);
686  outputCovariance.at<double>(0,0) = (*block)(0,0); // x-x
687  outputCovariance.at<double>(0,1) = (*block)(0,1); // x-y
688  outputCovariance.at<double>(0,5) = (*block)(0,2); // x-theta
689  outputCovariance.at<double>(1,0) = (*block)(1,0); // y-x
690  outputCovariance.at<double>(1,1) = (*block)(1,1); // y-y
691  outputCovariance.at<double>(1,5) = (*block)(1,2); // y-theta
692  outputCovariance.at<double>(5,0) = (*block)(2,0); // theta-x
693  outputCovariance.at<double>(5,1) = (*block)(2,1); // theta-y
694  outputCovariance.at<double>(5,5) = (*block)(2,2); // theta-theta
695  }
696  else if(v->hessianIndex() < 0)
697  {
698  UWARN("Computing marginals: vertex %d has negative hessian index (%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex());
699  }
700  else
701  {
702  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());
703  }
704  }
705  else
706  {
707  UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
708  }
709  }
710  else
711  {
712  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
713  {
714  const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(iter->first);
715  if(v)
716  {
717  Transform t = Transform::fromEigen3d(v->estimate());
718  optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
719  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
720  }
721  else
722  {
723  UERROR("Vertex %d not found!?", iter->first);
724  }
725  }
726 
727  g2o::VertexSE3* v = (g2o::VertexSE3*)optimizer.vertex(poses.rbegin()->first);
728  if(v)
729  {
730  UTimer t;
731  g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
732  optimizer.computeMarginals(spinv, v);
733  UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
734  if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
735  {
736  g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
737  UASSERT(block && block->cols() == 6 && block->cols() == 6);
738  memcpy(outputCovariance.data, block->data(), outputCovariance.total()*sizeof(double));
739  }
740  else if(v->hessianIndex() < 0)
741  {
742  UWARN("Computing marginals: vertex %d has negative hessian index (%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex());
743  }
744 #ifdef RTABMAP_G2O_CPP11
745  else
746  {
747  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());
748  }
749 #endif
750  }
751  else
752  {
753  UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
754  }
755  }
756  }
757  else if(poses.size() == 1 || iterations() <= 0)
758  {
759  optimizedPoses = poses;
760  }
761  else
762  {
763  UWARN("This method should be called at least with 1 pose!");
764  }
765  UDEBUG("Optimizing graph...end!");
766 #else
767 #ifdef RTABMAP_ORB_SLAM2
768  UERROR("G2O graph optimization cannot be used with g2o built from ORB_SLAM2, only SBA is available.");
769 #else
770  UERROR("Not built with G2O support!");
771 #endif
772 #endif
773  return optimizedPoses;
774 }
775 
776 #ifdef RTABMAP_ORB_SLAM2
777 
780  class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>
781 {
782  public:
783  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
784  EdgeSE3Expmap(): BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){}
785  bool read(std::istream& is)
786  {
787  return false;
788  }
789 
790  bool write(std::ostream& os) const
791  {
792  return false;
793  }
794 
795  void computeError()
796  {
797  const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
798  const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
799  g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
800  _error[0]=delta.translation().x();
801  _error[1]=delta.translation().y();
802  _error[2]=delta.translation().z();
803  _error[3]=delta.rotation().x();
804  _error[4]=delta.rotation().y();
805  _error[5]=delta.rotation().z();
806  }
807 
808  virtual void setMeasurement(const g2o::SE3Quat& meas){
809  _measurement=meas;
810  _inverseMeasurement=meas.inverse();
811  }
812 
813  virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;}
814  virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){
815  g2o::VertexSE3Expmap* from = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
816  g2o::VertexSE3Expmap* to = static_cast<g2o::VertexSE3Expmap*>(_vertices[1]);
817  if (from_.count(from) > 0)
818  to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement);
819  else
820  from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement);
821  }
822 
823  virtual bool setMeasurementData(const double* d){
824  Eigen::Map<const g2o::Vector7d> v(d);
825  _measurement.fromVector(v);
826  _inverseMeasurement = _measurement.inverse();
827  return true;
828  }
829 
830  virtual bool getMeasurementData(double* d) const{
831  Eigen::Map<g2o::Vector7d> v(d);
832  v = _measurement.toVector();
833  return true;
834  }
835 
836  virtual int measurementDimension() const {return 7;}
837 
838  virtual bool setMeasurementFromState() {
839  const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
840  const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
841  _measurement = (v1->estimate().inverse()*v2->estimate());
842  _inverseMeasurement = _measurement.inverse();
843  return true;
844  }
845 
846  protected:
847  g2o::SE3Quat _inverseMeasurement;
848 };
849 #endif
850 
851 std::map<int, Transform> OptimizerG2O::optimizeBA(
852  int rootId,
853  const std::map<int, Transform> & poses,
854  const std::multimap<int, Link> & links,
855  const std::map<int, CameraModel> & models,
856  std::map<int, cv::Point3f> & points3DMap,
857  const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
858  std::set<int> * outliers)
859 {
860  std::map<int, Transform> optimizedPoses;
861 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
862  UDEBUG("Optimizing graph...");
863 
864  optimizedPoses.clear();
865  if(poses.size()>=2 && iterations() > 0 && models.size() == poses.size())
866  {
867  g2o::SparseOptimizer optimizer;
868  optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
869 #if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM2)
870  std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
871 #else
872  g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
873 #endif
874 
875 #ifdef RTABMAP_ORB_SLAM2
876  linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
877 #else
878  if(solver_ == 3)
879  {
880  //eigen
881 #ifdef RTABMAP_G2O_CPP11
882  linearSolver = g2o::make_unique<g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType> >();
883 #else
884  linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
885 #endif
886  }
887 #ifdef G2O_HAVE_CHOLMOD
888  else if(solver_ == 2)
889  {
890  //chmold
891 #ifdef RTABMAP_G2O_CPP11
892  linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> >();
893 #else
894  linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
895 #endif
896  }
897 #endif
898 #ifdef G2O_HAVE_CSPARSE
899  else if(solver_ == 0)
900  {
901  //csparse
902 #ifdef RTABMAP_G2O_CPP11
903  linearSolver = g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType> >();
904 #else
905  linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
906 #endif
907  }
908 #endif
909  else
910  {
911  //pcg
912 #ifdef RTABMAP_G2O_CPP11
913  linearSolver = g2o::make_unique<g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType> >();
914 #else
915  linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
916 #endif
917  }
918 #endif // RTABMAP_ORB_SLAM2
919 
920 #ifndef RTABMAP_ORB_SLAM2
921  if(optimizer_ == 1)
922  {
923 #ifdef RTABMAP_G2O_CPP11
924  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(
925  g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
926 #else
927  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(new g2o::BlockSolver_6_3(linearSolver)));
928 #endif
929  }
930  else
931 #endif
932  {
933 #if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM2)
934  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
935  g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
936 #else
937  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(new g2o::BlockSolver_6_3(linearSolver)));
938 #endif
939  }
940 
941 
942  UDEBUG("fill poses to g2o...");
943  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); )
944  {
945  // Get camera model
946  std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
947  UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
948 
949  Transform camPose = iter->second * iterModel->second.localTransform();
950 
951  // Add node's pose
952  UASSERT(!camPose.isNull());
953 #ifdef RTABMAP_ORB_SLAM2
954  g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
955 #else
956  g2o::VertexCam * vCam = new g2o::VertexCam();
957 #endif
958 
959  Eigen::Affine3d a = camPose.toEigen3d();
960 #ifdef RTABMAP_ORB_SLAM2
961  a = a.inverse();
962  vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
963 #else
964  g2o::SBACam cam(Eigen::Quaterniond(a.linear()), a.translation());
965  cam.setKcam(
966  iterModel->second.fx(),
967  iterModel->second.fy(),
968  iterModel->second.cx(),
969  iterModel->second.cy(),
970  iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters
971  vCam->setEstimate(cam);
972 #endif
973  vCam->setId(iter->first);
974 
975  // negative root means that all other poses should be fixed instead of the root
976  vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
977 
978  UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
979  iter->first,
980  vCam->fixed()?1:0,
981  iterModel->second.fx(),
982  iterModel->second.fy(),
983  iterModel->second.cx(),
984  iterModel->second.cy(),
985  iterModel->second.Tx(),
986  iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_,
987  camPose.prettyPrint().c_str());
988 
989  UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
990 
991  ++iter;
992  }
993 
994  UDEBUG("fill edges to g2o...");
995  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
996  {
997  if(uContains(poses, iter->second.from()) &&
998  uContains(poses, iter->second.to()))
999  {
1000  // add edge
1001  int id1 = iter->second.from();
1002  int id2 = iter->second.to();
1003 
1004  if(id1 != id2) // not supporting prior
1005  {
1006  UASSERT(!iter->second.transform().isNull());
1007 
1008  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
1009  if(!isCovarianceIgnored())
1010  {
1011  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
1012  }
1013 
1014  // between cameras, not base_link
1015  Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
1016  //UDEBUG("added edge %d->%d (in cam frame=%s)",
1017  // id1,
1018  // id2,
1019  // camLink.prettyPrint().c_str());
1020 #ifdef RTABMAP_ORB_SLAM2
1021  EdgeSE3Expmap * e = new EdgeSE3Expmap();
1022  g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id1);
1023  g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id2);
1024 
1025  Transform camPose1 = Transform::fromEigen3d(v1->estimate()).inverse();
1026  Transform camPose2Inv = Transform::fromEigen3d(v2->estimate());
1027 
1028  camLink = camPose1 * camPose1 * camLink * camPose2Inv * camPose2Inv;
1029 #else
1030  g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
1031  g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
1032  g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2);
1033 #endif
1034  UASSERT(v1 != 0);
1035  UASSERT(v2 != 0);
1036  e->setVertex(0, v1);
1037  e->setVertex(1, v2);
1038  Eigen::Affine3d a = camLink.toEigen3d();
1039  e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
1040  e->setInformation(information);
1041 
1042  if (!optimizer.addEdge(e))
1043  {
1044  delete e;
1045  UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
1046  return optimizedPoses;
1047  }
1048  }
1049  }
1050  }
1051 
1052  UDEBUG("fill 3D points to g2o...");
1053  const int stepVertexId = poses.rbegin()->first+1;
1054  int negVertexOffset = stepVertexId;
1055  if(wordReferences.size() && wordReferences.rbegin()->first>0)
1056  {
1057  negVertexOffset += wordReferences.rbegin()->first;
1058  }
1059  UDEBUG("stepVertexId=%d, negVertexOffset=%d", stepVertexId, negVertexOffset);
1060  std::list<g2o::OptimizableGraph::Edge*> edges;
1061  for(std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
1062  {
1063  int id = iter->first;
1064  if(points3DMap.find(id) != points3DMap.end())
1065  {
1066  cv::Point3f pt3d = points3DMap.at(id);
1067  g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
1068 
1069  vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
1070  if(id<0)
1071  {
1072  vpt3d->setId(negVertexOffset + id*-1);
1073  }
1074  else
1075  {
1076  vpt3d->setId(stepVertexId + id);
1077  }
1078  UASSERT(vpt3d->id() > 0);
1079  vpt3d->setMarginalized(true);
1080  optimizer.addVertex(vpt3d);
1081 
1082  //UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
1083 
1084  // set observations
1085  for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
1086  {
1087  int camId = jter->first;
1088  if(poses.find(camId) != poses.end() && optimizer.vertex(camId) != 0)
1089  {
1090  const cv::Point3f & pt = jter->second;
1091  double depth = pt.z;
1092 
1093  //UDEBUG("Added observation pt=%d to cam=%d (%f,%f) depth=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
1094 
1095  g2o::OptimizableGraph::Edge * e;
1096  double baseline = 0.0;
1097 #ifdef RTABMAP_ORB_SLAM2
1098  g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
1099  std::map<int, CameraModel>::const_iterator iterModel = models.find(camId);
1100 
1101  UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
1102  baseline = iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_;
1103 #else
1104  g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
1105  baseline = vcam->estimate().baseline;
1106 #endif
1107  double variance = pixelVariance_;
1108  if(uIsFinite(depth) && depth > 0.0 && baseline > 0.0)
1109  {
1110  // stereo edge
1111 #ifdef RTABMAP_ORB_SLAM2
1112  g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
1113  float disparity = baseline * iterModel->second.fx() / depth;
1114  Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity);
1115  es->setMeasurement(obs);
1116  //variance *= log(exp(1)+disparity);
1117  es->setInformation(Eigen::Matrix3d::Identity() / variance);
1118  es->fx = iterModel->second.fx();
1119  es->fy = iterModel->second.fy();
1120  es->cx = iterModel->second.cx();
1121  es->cy = iterModel->second.cy();
1122  es->bf = baseline*es->fx;
1123  e = es;
1124 #else
1125  g2o::EdgeProjectP2SC* es = new g2o::EdgeProjectP2SC();
1126  float disparity = baseline * vcam->estimate().Kcam(0,0) / depth;
1127  Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity);
1128  es->setMeasurement(obs);
1129  //variance *= log(exp(1)+disparity);
1130  es->setInformation(Eigen::Matrix3d::Identity() / variance);
1131  e = es;
1132 #endif
1133  }
1134  else
1135  {
1136  if(baseline > 0.0)
1137  {
1138  UDEBUG("Stereo camera model detected but current "
1139  "observation (pt=%d to cam=%d) has null depth (%f m), adding "
1140  "mono observation instead.",
1141  vpt3d->id()-stepVertexId, camId, depth);
1142  }
1143  // mono edge
1144 #ifdef RTABMAP_ORB_SLAM2
1145  g2o::EdgeSE3ProjectXYZ* em = new g2o::EdgeSE3ProjectXYZ();
1146  Eigen::Vector2d obs( pt.x, pt.y);
1147  em->setMeasurement(obs);
1148  em->setInformation(Eigen::Matrix2d::Identity() / variance);
1149  em->fx = iterModel->second.fx();
1150  em->fy = iterModel->second.fy();
1151  em->cx = iterModel->second.cx();
1152  em->cy = iterModel->second.cy();
1153  e = em;
1154 
1155 #else
1156  g2o::EdgeProjectP2MC* em = new g2o::EdgeProjectP2MC();
1157  Eigen::Vector2d obs( pt.x, pt.y);
1158  em->setMeasurement(obs);
1159  em->setInformation(Eigen::Matrix2d::Identity() / variance);
1160  e = em;
1161 #endif
1162  }
1163  e->setVertex(0, vpt3d);
1164  e->setVertex(1, vcam);
1165 
1166  if(robustKernelDelta_ > 0.0)
1167  {
1168  g2o::RobustKernelHuber* kernel = new g2o::RobustKernelHuber;
1169  kernel->setDelta(robustKernelDelta_);
1170  e->setRobustKernel(kernel);
1171  }
1172 
1173  optimizer.addEdge(e);
1174  edges.push_back(e);
1175  }
1176  }
1177  }
1178  }
1179 
1180  UDEBUG("Initial optimization...");
1181  optimizer.initializeOptimization();
1182 
1183  UASSERT(optimizer.verifyInformationMatrices());
1184 
1185  UINFO("g2o optimizing begin (max iterations=%d, robustKernel=%f)", iterations(), robustKernelDelta_);
1186 
1187  int it = 0;
1188  UTimer timer;
1189  int outliersCount = 0;
1190  int outliersCountFar = 0;
1191 
1192  for(int i=0; i<(robustKernelDelta_>0.0?2:1); ++i)
1193  {
1194  it += optimizer.optimize(i==0&&robustKernelDelta_>0.0?5:iterations());
1195 
1196  // early stop condition
1197  optimizer.computeActiveErrors();
1198  double chi2 = optimizer.activeRobustChi2();
1199 
1200  if(uIsNan(chi2))
1201  {
1202  UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
1203  return optimizedPoses;
1204  }
1205  UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
1206 
1207  if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
1208  {
1209  UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
1210  return optimizedPoses;
1211  }
1212 
1213  if(robustKernelDelta_>0.0)
1214  {
1215  for(std::list<g2o::OptimizableGraph::Edge*>::iterator iter=edges.begin(); iter!=edges.end();++iter)
1216  {
1217  if((*iter)->level() == 0 && (*iter)->chi2() > (*iter)->robustKernel()->delta())
1218  {
1219  (*iter)->setLevel(1);
1220  ++outliersCount;
1221  double d = 0.0;
1222 #ifdef RTABMAP_ORB_SLAM2
1223  if(dynamic_cast<g2o::EdgeStereoSE3ProjectXYZ*>(*iter) != 0)
1224  {
1225  d = ((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[0]-((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[2];
1226  }
1227  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());
1228 #else
1229  if(dynamic_cast<g2o::EdgeProjectP2SC*>(*iter) != 0)
1230  {
1231  d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
1232  }
1233  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());
1234 #endif
1235 
1236  cv::Point3f pt3d;
1237  if((*iter)->vertex(0)->id() > negVertexOffset)
1238  {
1239  pt3d = points3DMap.at(negVertexOffset - (*iter)->vertex(0)->id());
1240  }
1241  else
1242  {
1243  pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
1244  }
1245  ((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
1246 
1247  if(outliers)
1248  {
1249  outliers->insert((*iter)->vertex(0)->id()-stepVertexId);
1250  }
1251  if(d < 5.0)
1252  {
1253  outliersCountFar++;
1254  }
1255  }
1256  //(*iter)->setRobustKernel(0);
1257  }
1258  if(i==0)
1259  optimizer.initializeOptimization(0);
1260  UDEBUG("outliers=%d outliersCountFar=%d", outliersCount, outliersCountFar);
1261  }
1262  }
1263  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());
1264 
1265  if(optimizer.activeRobustChi2() > 1000000000000.0)
1266  {
1267  UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
1268  return optimizedPoses;
1269  }
1270 
1271  // update poses
1272  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
1273  {
1274 #ifdef RTABMAP_ORB_SLAM2
1275  const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(iter->first);
1276 #else
1277  const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
1278 #endif
1279  if(v)
1280  {
1281  Transform t = Transform::fromEigen3d(v->estimate());
1282 
1283 #ifdef RTABMAP_ORB_SLAM2
1284  t=t.inverse();
1285 #endif
1286 
1287  // remove model local transform
1288  t *= models.at(iter->first).localTransform().inverse();
1289 
1290  UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
1291  if(t.isNull())
1292  {
1293  UERROR("Optimized pose %d is null!?!?", iter->first);
1294  optimizedPoses.clear();
1295  return optimizedPoses;
1296  }
1297 
1298  // FIXME: is there a way that we can add the 2D constraint directly in SBA?
1299  if(this->isSlam2d())
1300  {
1301  // get transform between old and new pose
1302  t = iter->second.inverse() * t;
1303  optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
1304  }
1305  else
1306  {
1307  optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
1308  }
1309  }
1310  else
1311  {
1312  UERROR("Vertex (pose) %d not found!?", iter->first);
1313  }
1314  }
1315 
1316  //update points3D
1317 
1318  for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter!=points3DMap.end(); ++iter)
1319  {
1320  const g2o::VertexSBAPointXYZ* v;
1321  int id = iter->first;
1322  if(id<0)
1323  {
1324  v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(negVertexOffset + id*-1);
1325  }
1326  else
1327  {
1328  v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + id);
1329  }
1330 
1331  if(v)
1332  {
1333  cv::Point3f p(v->estimate()[0], v->estimate()[1], v->estimate()[2]);
1334  //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);
1335  iter->second = p;
1336  }
1337  else
1338  {
1339  iter->second.x = iter->second.y = iter->second.z = std::numeric_limits<float>::quiet_NaN();
1340  }
1341  }
1342  }
1343  else if(poses.size() > 1 && poses.size() != models.size())
1344  {
1345  UERROR("This method should be called with size of poses = size camera models!");
1346  }
1347  else if(poses.size() == 1 || iterations() <= 0)
1348  {
1349  optimizedPoses = poses;
1350  }
1351  else
1352  {
1353  UWARN("This method should be called at least with 1 pose!");
1354  }
1355  UDEBUG("Optimizing graph...end!");
1356 #else
1357  UERROR("Not built with G2O support!");
1358 #endif
1359  return optimizedPoses;
1360 }
1361 
1362 bool OptimizerG2O::saveGraph(
1363  const std::string & fileName,
1364  const std::map<int, Transform> & poses,
1365  const std::multimap<int, Link> & edgeConstraints,
1366  bool useRobustConstraints)
1367 {
1368  FILE * file = 0;
1369 
1370 #ifdef _MSC_VER
1371  fopen_s(&file, fileName.c_str(), "w");
1372 #else
1373  file = fopen(fileName.c_str(), "w");
1374 #endif
1375 
1376  if(file)
1377  {
1378  // VERTEX_SE3 id x y z qw qx qy qz
1379  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
1380  {
1381  Eigen::Quaternionf q = iter->second.getQuaternionf();
1382  fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n",
1383  iter->first,
1384  iter->second.x(),
1385  iter->second.y(),
1386  iter->second.z(),
1387  q.x(),
1388  q.y(),
1389  q.z(),
1390  q.w());
1391  }
1392 
1393  //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
1394  int virtualVertexId = poses.size()?poses.rbegin()->first+1:0;
1395  for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
1396  {
1397  std::string prefix = "EDGE_SE3:QUAT";
1398  std::string suffix = "";
1399 
1400  if(useRobustConstraints &&
1401  iter->second.type() != Link::kNeighbor &&
1402  iter->second.type() != Link::kNeighborMerged)
1403  {
1404  prefix = "EDGE_SE3_SWITCHABLE";
1405  fprintf(file, "VERTEX_SWITCH %d 1\n", virtualVertexId);
1406  fprintf(file, "EDGE_SWITCH_PRIOR %d 1 1.0\n", virtualVertexId);
1407  suffix = uFormat(" %d", virtualVertexId++);
1408  }
1409 
1410  Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
1411  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",
1412  prefix.c_str(),
1413  iter->second.from(),
1414  iter->second.to(),
1415  suffix.c_str(),
1416  iter->second.transform().x(),
1417  iter->second.transform().y(),
1418  iter->second.transform().z(),
1419  q.x(),
1420  q.y(),
1421  q.z(),
1422  q.w(),
1423  iter->second.infMatrix().at<double>(0,0),
1424  iter->second.infMatrix().at<double>(0,1),
1425  iter->second.infMatrix().at<double>(0,2),
1426  iter->second.infMatrix().at<double>(0,3),
1427  iter->second.infMatrix().at<double>(0,4),
1428  iter->second.infMatrix().at<double>(0,5),
1429  iter->second.infMatrix().at<double>(1,1),
1430  iter->second.infMatrix().at<double>(1,2),
1431  iter->second.infMatrix().at<double>(1,3),
1432  iter->second.infMatrix().at<double>(1,4),
1433  iter->second.infMatrix().at<double>(1,5),
1434  iter->second.infMatrix().at<double>(2,2),
1435  iter->second.infMatrix().at<double>(2,3),
1436  iter->second.infMatrix().at<double>(2,4),
1437  iter->second.infMatrix().at<double>(2,5),
1438  iter->second.infMatrix().at<double>(3,3),
1439  iter->second.infMatrix().at<double>(3,4),
1440  iter->second.infMatrix().at<double>(3,5),
1441  iter->second.infMatrix().at<double>(4,4),
1442  iter->second.infMatrix().at<double>(4,5),
1443  iter->second.infMatrix().at<double>(5,5));
1444  }
1445  UINFO("Graph saved to %s", fileName.c_str());
1446  fclose(file);
1447  }
1448  else
1449  {
1450  UERROR("Cannot save to file %s", fileName.c_str());
1451  return false;
1452  }
1453  return true;
1454 }
1455 
1456 } /* namespace rtabmap */
d
d
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
cam
rtabmap::CameraThread * cam
Definition: tools/DataRecorder/main.cpp:63
epsilon
double epsilon
UTimer
Definition: UTimer.h:46
edge_switchPrior.h
util3d_motion_estimation.h
glm::e
GLM_FUNC_DECL genType e()
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
UMath.h
Basic mathematics functions.
UConversion.h
Some conversion functions.
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
edge_se2Switchable.h
EdgeSE3Switchable::setMeasurement
virtual void setMeasurement(const Eigen::Isometry3d &m)
Definition: edge_se3Switchable.h:28
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
UASSERT
#define UASSERT(condition)
vertex_switchLinear.h
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
edge_se3Switchable.h
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
util3d_transforms.h
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
EdgeSE2Switchable::setMeasurement
virtual void setMeasurement(const g2o::SE2 &m)
Definition: edge_se2Switchable.h:29
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
uIsNan
bool uIsNan(const T &value)
Definition: UMath.h:42
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:169
VertexSwitchLinear::setEstimate
virtual void setEstimate(const double &et)
Definition: vertex_switchLinear.cpp:44
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:32