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 <locale.h>
34 #include <set>
35 
36 #include <rtabmap/core/Version.h>
37 #include <rtabmap/core/optimizer/OptimizerG2O.h>
38 #include <rtabmap/core/util3d_transforms.h>
39 #include <rtabmap/core/util3d_motion_estimation.h>
40 #include <rtabmap/core/util3d.h>
41 
42 
43 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
44 #include "g2o/core/sparse_optimizer.h"
45 #include "g2o/core/block_solver.h"
46 #include "g2o/core/factory.h"
47 #include "g2o/core/optimization_algorithm_factory.h"
48 #include "g2o/core/optimization_algorithm_gauss_newton.h"
49 #include "g2o/core/optimization_algorithm_levenberg.h"
50 #include "g2o/core/robust_kernel_impl.h"
51 namespace g2o {
52 // bug #include "g2o/core/eigen_types.h" not found on Indigo
53 typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixXD;
54 }
55 
56 #ifdef RTABMAP_G2O
57 #include "g2o/types/sba/types_sba.h"
58 #include "g2o/solvers/eigen/linear_solver_eigen.h"
59 #include "g2o/config.h"
60 #include "g2o/types/slam2d/types_slam2d.h"
61 #include "g2o/types/slam3d/types_slam3d.h"
62 #include "g2o/edge_se3_xyzprior.h" // Include after types_slam3d.h to be ignored on newest g2o versions
63 #include "g2o/edge_se3_gravity.h"
64 #include "g2o/edge_sbacam_gravity.h"
65 #include "g2o/edge_xy_prior.h" // Include after types_slam2d.h to be ignored on newest g2o versions
66 #include "g2o/edge_xyz_prior.h" // Include after types_slam3d.h to be ignored on newest g2o versions
67 #ifdef G2O_HAVE_CSPARSE
68 #include "g2o/solvers/csparse/linear_solver_csparse.h"
69 #endif
70 #include "g2o/solvers/pcg/linear_solver_pcg.h"
71 #ifdef G2O_HAVE_CHOLMOD
72 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
73 #endif
74 #endif // RTABMAP_G2O
75 
76 #ifdef RTABMAP_ORB_SLAM
77 #include "g2o/types/types_sba.h"
78 #include "g2o/types/types_six_dof_expmap.h"
79 #include "g2o/solvers/linear_solver_eigen.h"
80 #endif
81 
82 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
83 typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver;
84 #ifdef RTABMAP_G2O
85 typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
86 #ifdef G2O_HAVE_CSPARSE
87 typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
88 #endif
89 #ifdef G2O_HAVE_CHOLMOD
90 typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearCholmodSolver;
91 #endif
92 
93 #ifdef G2O_SRC_DIR
94 namespace g2o {
95 typedef VertexPointXYZ VertexSBAPointXYZ;
96 }
97 #endif
98 
99 #if defined(RTABMAP_VERTIGO)
100 #include "vertigo/g2o/edge_switchPrior.h"
101 #include "vertigo/g2o/edge_se2Switchable.h"
102 #include "vertigo/g2o/edge_se3Switchable.h"
103 #include "vertigo/g2o/vertex_switchLinear.h"
104 #endif
105 #endif
106 
107 #endif // end defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
108 
109 enum {
110  PARAM_OFFSET=0,
111 };
112 
113 #define MULTICAM_OFFSET 10 // 10 means max 10 cameras per pose
114 
115 namespace rtabmap {
116 
117 bool OptimizerG2O::available()
118 {
119 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
120  return true;
121 #else
122  return false;
123 #endif
124 }
125 
126 bool OptimizerG2O::isCSparseAvailable()
127 {
128 #ifdef G2O_HAVE_CSPARSE
129  return true;
130 #else
131  return false;
132 #endif
133 }
134 
135 bool OptimizerG2O::isCholmodAvailable()
136 {
137 #ifdef G2O_HAVE_CHOLMOD
138  return true;
139 #else
140  return false;
141 #endif
142 }
143 
144 OptimizerG2O::OptimizerG2O(const ParametersMap & parameters) :
145  Optimizer(parameters),
146  solver_(Parameters::defaultg2oSolver()),
147  optimizer_(Parameters::defaultg2oOptimizer()),
148  pixelVariance_(Parameters::defaultg2oPixelVariance()),
149  robustKernelDelta_(Parameters::defaultg2oRobustKernelDelta()),
150  baseline_(Parameters::defaultg2oBaseline())
151 {
152 #ifdef RTABMAP_G2O
153  // Issue on android, have to explicitly register this type when using fixed root prior below
154  if(!g2o::Factory::instance()->knowsTag("CACHE_SE3_OFFSET"))
155  {
156 #if defined(RTABMAP_G2O_CPP11) and RTABMAP_G2O_CPP11 == 1
157  g2o::Factory::instance()->registerType("CACHE_SE3_OFFSET", g2o::make_unique<g2o::HyperGraphElementCreator<g2o::CacheSE3Offset> >());
158 #else
159  g2o::Factory::instance()->registerType("CACHE_SE3_OFFSET", new g2o::HyperGraphElementCreator<g2o::CacheSE3Offset>);
160 #endif
161  }
162 #endif
163  parseParameters(parameters);
164 }
165 
166 void OptimizerG2O::parseParameters(const ParametersMap & parameters)
167 {
168  Optimizer::parseParameters(parameters);
169 
170  Parameters::parse(parameters, Parameters::kg2oSolver(), solver_);
171  Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_);
172  Parameters::parse(parameters, Parameters::kg2oPixelVariance(), pixelVariance_);
173  Parameters::parse(parameters, Parameters::kg2oRobustKernelDelta(), robustKernelDelta_);
174  Parameters::parse(parameters, Parameters::kg2oBaseline(), baseline_);
175  UASSERT(pixelVariance_ > 0.0);
176  UASSERT(baseline_ >= 0.0);
177 
178 #ifdef RTABMAP_ORB_SLAM
179  if(solver_ != 3)
180  {
181  UWARN("g2o built with ORB_SLAM has only Eigen solver available, using Eigen=3 instead of %d.", solver_);
182  solver_ = 3;
183  }
184 #else
185 #ifndef G2O_HAVE_CHOLMOD
186  if(solver_ == 2)
187  {
188  UWARN("g2o is not built with chmold, so it cannot be used as solver. Using CSparse instead.");
189  solver_ = 0;
190  }
191 #endif
192 
193 #ifndef G2O_HAVE_CSPARSE
194  if(solver_ == 0)
195  {
196  UWARN("g2o is not built with csparse, so it cannot be used as solver. Using PCG instead.");
197  solver_ = 1;
198  }
199 #endif
200 #endif
201 
202 }
203 
204 std::map<int, Transform> OptimizerG2O::optimize(
205  int rootId,
206  const std::map<int, Transform> & poses,
207  const std::multimap<int, Link> & edgeConstraints,
208  cv::Mat & outputCovariance,
209  std::list<std::map<int, Transform> > * intermediateGraphes,
210  double * finalError,
211  int * iterationsDone)
212 {
213  outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
214  std::map<int, Transform> optimizedPoses;
215 #ifdef RTABMAP_G2O
216  UDEBUG("Optimizing graph...");
217 
218 #ifndef RTABMAP_VERTIGO
219  if(this->isRobust())
220  {
221  UWARN("Vertigo robust optimization is not available! Robust optimization is now disabled.");
222  setRobust(false);
223  }
224 #endif
225 
226  optimizedPoses.clear();
227  if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0 && poses.rbegin()->first > 0)
228  {
229  // Apply g2o optimization
230 
231  g2o::SparseOptimizer optimizer;
232  //optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
233  if (isSlam2d())
234  {
235  g2o::ParameterSE2Offset* odomOffset = new g2o::ParameterSE2Offset();
236  odomOffset->setId(PARAM_OFFSET);
237  optimizer.addParameter(odomOffset);
238  }
239  else
240  {
241  g2o::ParameterSE3Offset* odomOffset = new g2o::ParameterSE3Offset();
242  odomOffset->setId(PARAM_OFFSET);
243  optimizer.addParameter(odomOffset);
244  }
245 
246 #ifdef RTABMAP_G2O_CPP11
247 
248  std::unique_ptr<SlamBlockSolver> blockSolver;
249 
250  if(solver_ == 3)
251  {
252  //eigen
253  auto linearSolver = g2o::make_unique<SlamLinearEigenSolver>();
254  linearSolver->setBlockOrdering(false);
255  blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
256  }
257 #ifdef G2O_HAVE_CHOLMOD
258  else if(solver_ == 2)
259  {
260  //chmold
261  auto linearSolver = g2o::make_unique<SlamLinearCholmodSolver>();
262  linearSolver->setBlockOrdering(false);
263  blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
264  }
265 #endif
266 #ifdef G2O_HAVE_CSPARSE
267  else if(solver_ == 0)
268  {
269 
270  //csparse
271  auto linearSolver = g2o::make_unique<SlamLinearCSparseSolver>();
272  linearSolver->setBlockOrdering(false);
273  blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
274  }
275 #endif
276  else
277  {
278  //pcg
279  auto linearSolver = g2o::make_unique<SlamLinearPCGSolver>();
280  blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
281  }
282 
283  if(optimizer_ == 1)
284  {
285 
286  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(std::move(blockSolver)));
287  }
288  else
289  {
290  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver)));
291  }
292 
293 #else
294 
295  SlamBlockSolver * blockSolver = 0;
296 
297  if(solver_ == 3)
298  {
299  //eigen
300  SlamLinearEigenSolver * linearSolver = new SlamLinearEigenSolver();
301  linearSolver->setBlockOrdering(false);
302  blockSolver = new SlamBlockSolver(linearSolver);
303  }
304 #ifdef G2O_HAVE_CHOLMOD
305  else if(solver_ == 2)
306  {
307  //chmold
308  SlamLinearCholmodSolver * linearSolver = new SlamLinearCholmodSolver();
309  linearSolver->setBlockOrdering(false);
310  blockSolver = new SlamBlockSolver(linearSolver);
311  }
312 #endif
313 #ifdef G2O_HAVE_CSPARSE
314  else if(solver_ == 0)
315  {
316  //csparse
317  SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver();
318  linearSolver->setBlockOrdering(false);
319  blockSolver = new SlamBlockSolver(linearSolver);
320  }
321 #endif
322  else
323  {
324  //pcg
325  SlamLinearPCGSolver * linearSolver = new SlamLinearPCGSolver();
326  blockSolver = new SlamBlockSolver(linearSolver);
327  }
328 
329  if(optimizer_ == 1)
330  {
331  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver));
332  }
333  else
334  {
335  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver));
336  }
337 #endif
338  // detect if there is a global pose prior set, if so remove rootId
339  bool hasGravityConstraints = false;
340  if(!priorsIgnored() || (!isSlam2d() && gravitySigma() > 0))
341  {
342  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
343  {
344  if(iter->second.from() == iter->second.to())
345  {
346  if(!priorsIgnored() && iter->second.type() == Link::kPosePrior)
347  {
348  rootId = 0;
349  break;
350  }
351  else if(!isSlam2d() &&
352  gravitySigma() > 0 &&
353  iter->second.type() == Link::kGravity)
354  {
355  hasGravityConstraints = true;
356  if(priorsIgnored())
357  {
358  break;
359  }
360  }
361  }
362  }
363  }
364 
365  int landmarkVertexOffset = poses.rbegin()->first+1;
366  std::map<int, bool> isLandmarkWithRotation;
367 
368  UDEBUG("fill poses to g2o... (rootId=%d hasGravityConstraints=%d isSlam2d=%d)", rootId, hasGravityConstraints?1:0, isSlam2d()?1:0);
369  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
370  {
371  UASSERT(!iter->second.isNull());
372  g2o::HyperGraph::Vertex * vertex = 0;
373  int id = iter->first;
374  if(isSlam2d())
375  {
376  if(id > 0)
377  {
378  g2o::VertexSE2 * v2 = new g2o::VertexSE2();
379  v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
380  if(id == rootId)
381  {
382  UDEBUG("Set %d fixed", id);
383  v2->setFixed(true);
384  }
385  vertex = v2;
386  }
387  else if(!landmarksIgnored())
388  {
389  // check if it is SE2 or only PointXY
390  std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(id);
391  UASSERT(jter != edgeConstraints.end());
392 
393  if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
394  {
395  g2o::VertexPointXY * v2 = new g2o::VertexPointXY();
396  v2->setEstimate(Eigen::Vector2d(iter->second.x(), iter->second.y()));
397  vertex = v2;
398  isLandmarkWithRotation.insert(std::make_pair(id, false));
399  id = landmarkVertexOffset - id;
400  }
401  else
402  {
403  g2o::VertexSE2 * v2 = new g2o::VertexSE2();
404  v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
405  if(id == rootId)
406  {
407  UDEBUG("Set %d fixed", id);
408  v2->setFixed(true);
409  }
410  vertex = v2;
411  isLandmarkWithRotation.insert(std::make_pair(id, true));
412  id = landmarkVertexOffset - id;
413  }
414  }
415  else
416  {
417  continue;
418  }
419  }
420  else
421  {
422  if(id > 0)
423  {
424  g2o::VertexSE3 * v3 = new g2o::VertexSE3();
425 
426  Eigen::Affine3d a = iter->second.toEigen3d();
427  Eigen::Isometry3d pose;
428  pose = a.linear();
429  pose.translation() = a.translation();
430  v3->setEstimate(pose);
431  if(id == rootId && !hasGravityConstraints)
432  {
433  UDEBUG("Set %d fixed", id);
434  v3->setFixed(true);
435  }
436  vertex = v3;
437  }
438  else if(!landmarksIgnored())
439  {
440  // check if it is SE3 or only PointXYZ
441  std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(id);
442  UASSERT(jter != edgeConstraints.end());
443 
444  if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
445  1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
446  1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
447  {
448  g2o::VertexPointXYZ * v3 = new g2o::VertexPointXYZ();
449  v3->setEstimate(Eigen::Vector3d(iter->second.x(), iter->second.y(), iter->second.z()));
450  vertex = v3;
451  isLandmarkWithRotation.insert(std::make_pair(id, false));
452  id = landmarkVertexOffset - id;
453  }
454  else
455  {
456  g2o::VertexSE3 * v3 = new g2o::VertexSE3();
457  Eigen::Affine3d a = iter->second.toEigen3d();
458  Eigen::Isometry3d pose;
459  pose = a.linear();
460  pose.translation() = a.translation();
461  v3->setEstimate(pose);
462  if(id == rootId && !hasGravityConstraints)
463  {
464  UDEBUG("Set %d fixed", id);
465  v3->setFixed(true);
466  }
467  vertex = v3;
468  isLandmarkWithRotation.insert(std::make_pair(id, true));
469  id = landmarkVertexOffset - id;
470  }
471  }
472  else
473  {
474  continue;
475  }
476  }
477  if(vertex == 0)
478  {
479  UERROR("Could not create vertex for node %d", id);
480  }
481  else
482  {
483  vertex->setId(id);
484  UASSERT_MSG(optimizer.addVertex(vertex), uFormat("cannot insert vertex %d!?", iter->first).c_str());
485  }
486  }
487 
488  // Setup root prior (fixed x,y,z,yaw)
489  if(!isSlam2d() && rootId !=0 && hasGravityConstraints)
490  {
491  g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(optimizer.vertex(rootId));
492  if(v1)
493  {
494  g2o::EdgeSE3Prior * e = new g2o::EdgeSE3Prior();
495  e->setVertex(0, v1);
496  Eigen::Affine3d a = poses.at(rootId).toEigen3d();
497  Eigen::Isometry3d pose;
498  pose = a.linear();
499  pose.translation() = a.translation();
500  e->setMeasurement(pose);
501  e->setParameterId(0, PARAM_OFFSET);
502  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*10e6;
503  // pitch and roll not fixed
504  information(3,3) = information(4,4) = 1;
505  e->setInformation(information);
506  if (!optimizer.addEdge(e))
507  {
508  delete e;
509  UERROR("Map: Failed adding fixed constraint of rootid %d, set as fixed instead", rootId);
510  v1->setFixed(true);
511  }
512  else
513  {
514  UDEBUG("Set %d fixed with prior (have gravity constraints)", rootId);
515  }
516  }
517  else
518  {
519  UERROR("Map: Failed adding fixed constraint of rootid %d (not found in added vertices)", rootId);
520  }
521  }
522 
523  UDEBUG("fill edges to g2o...");
524 #if defined(RTABMAP_VERTIGO)
525  int vertigoVertexId = landmarkVertexOffset - (poses.begin()->first<0?poses.begin()->first-1:0);
526 #endif
527  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
528  {
529  int id1 = iter->second.from();
530  int id2 = iter->second.to();
531 
532  UASSERT(!iter->second.transform().isNull());
533 
534  g2o::HyperGraph::Edge * edge = 0;
535 
536  if(id1 == id2)
537  {
538  if(iter->second.type() == Link::kPosePrior && !priorsIgnored() &&
539  (!landmarksIgnored() || id1>0))
540  {
541  int idTag= id1;
542  if(id1<0)
543  {
544  // landmark prior, offset ids
545  id1 = landmarkVertexOffset - id1;
546  id2 = landmarkVertexOffset - id2;
547  }
548 
549  if(isSlam2d())
550  {
551  if(idTag < 0 && !isLandmarkWithRotation.at(idTag))
552  {
553  g2o::EdgeXYPrior * priorEdge = new g2o::EdgeXYPrior();
554  g2o::VertexPointXY* v1 = (g2o::VertexPointXY*)optimizer.vertex(id1);
555  priorEdge->setVertex(0, v1);
556  priorEdge->setMeasurement(Eigen::Vector2d(iter->second.transform().x(), iter->second.transform().y()));
557  Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
558  if(!isCovarianceIgnored())
559  {
560  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
561  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
562  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
563  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
564  }
565  priorEdge->setInformation(information);
566  edge = priorEdge;
567  }
568  else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
569  {
570  g2o::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior();
571  g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
572  priorEdge->setVertex(0, v1);
573  priorEdge->setMeasurement(Eigen::Vector2d(iter->second.transform().x(), iter->second.transform().y()));
574  Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
575  if(!isCovarianceIgnored())
576  {
577  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
578  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
579  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
580  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
581  }
582  priorEdge->setInformation(information);
583  edge = priorEdge;
584  }
585  else
586  {
587  g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
588  g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
589  priorEdge->setVertex(0, v1);
590  priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
591  priorEdge->setParameterId(0, PARAM_OFFSET);
592  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
593  if(!isCovarianceIgnored())
594  {
595  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
596  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
597  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
598  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
599  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
600  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
601  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
602  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
603  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
604  }
605  priorEdge->setInformation(information);
606  edge = priorEdge;
607  }
608  }
609  else
610  {
611  if(idTag < 0 && !isLandmarkWithRotation.at(idTag))
612  {
613  //XYZ case
614  g2o::EdgeXYZPrior * priorEdge = new g2o::EdgeXYZPrior();
615  g2o::VertexPointXYZ* v1 = (g2o::VertexPointXYZ*)optimizer.vertex(id1);
616  priorEdge->setVertex(0, v1);
617  priorEdge->setMeasurement(Eigen::Vector3d(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()));
618  priorEdge->setParameterId(0, PARAM_OFFSET);
619  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
620  if(!isCovarianceIgnored())
621  {
622  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
623  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
624  information(0,2) = iter->second.infMatrix().at<double>(0,2); // x-z
625  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
626  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
627  information(1,2) = iter->second.infMatrix().at<double>(1,2); // y-z
628  information(2,0) = iter->second.infMatrix().at<double>(2,0); // z-x
629  information(2,1) = iter->second.infMatrix().at<double>(2,1); // z-y
630  information(2,2) = iter->second.infMatrix().at<double>(2,2); // z-z
631  }
632  priorEdge->setInformation(information);
633  edge = priorEdge;
634  }
635  else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
636  1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
637  1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
638  {
639  //GPS XYZ case
640  g2o::EdgeSE3XYZPrior * priorEdge = new g2o::EdgeSE3XYZPrior();
641  g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
642  priorEdge->setVertex(0, v1);
643  priorEdge->setMeasurement(Eigen::Vector3d(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()));
644  priorEdge->setParameterId(0, PARAM_OFFSET);
645  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
646  if(!isCovarianceIgnored())
647  {
648  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
649  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
650  information(0,2) = iter->second.infMatrix().at<double>(0,2); // x-z
651  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
652  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
653  information(1,2) = iter->second.infMatrix().at<double>(1,2); // y-z
654  information(2,0) = iter->second.infMatrix().at<double>(2,0); // z-x
655  information(2,1) = iter->second.infMatrix().at<double>(2,1); // z-y
656  information(2,2) = iter->second.infMatrix().at<double>(2,2); // z-z
657  }
658  priorEdge->setInformation(information);
659  edge = priorEdge;
660  }
661  else
662  {
663  // XYZ+RPY case
664  g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
665  g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
666  priorEdge->setVertex(0, v1);
667  Eigen::Affine3d a = iter->second.transform().toEigen3d();
668  Eigen::Isometry3d pose;
669  pose = a.linear();
670  pose.translation() = a.translation();
671  priorEdge->setMeasurement(pose);
672  priorEdge->setParameterId(0, PARAM_OFFSET);
673  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
674  if(!isCovarianceIgnored())
675  {
676  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
677  }
678  priorEdge->setInformation(information);
679  edge = priorEdge;
680  }
681  }
682  }
683  else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
684  {
685  Eigen::Matrix<double, 6, 1> m;
686  // Up vector in robot frame
687  m.head<3>() = Eigen::Vector3d::UnitZ();
688  // Observed Gravity vector in world frame
689  float roll, pitch, yaw;
690  iter->second.transform().getEulerAngles(roll, pitch, yaw);
691  m.tail<3>() = Transform(0,0,0,roll,pitch,0).toEigen3d() * -Eigen::Vector3d::UnitZ();
692 
693  Eigen::MatrixXd information = Eigen::MatrixXd::Identity(3, 3) * 1.0/(gravitySigma()*gravitySigma());
694 
695  g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
696  EdgeSE3Gravity* priorEdge(new EdgeSE3Gravity());
697  priorEdge->setMeasurement(m);
698  priorEdge->setInformation(information);
699  priorEdge->vertices()[0] = v1;
700  edge = priorEdge;
701  }
702  }
703  else if(id1<0 || id2 < 0)
704  {
705  if(!landmarksIgnored())
706  {
707  //landmarks
708  UASSERT((id1 < 0 && id2 > 0) || (id1 > 0 && id2 < 0));
709 
710  Transform t;
711  if(id2 < 0)
712  {
713  t = iter->second.transform();
714  }
715  else
716  {
717  t = iter->second.transform().inverse();
718  std::swap(id1, id2); // should be node -> landmark
719  }
720  int idTag= id2;
721  id2 = landmarkVertexOffset - id2;
722 
723  if(isSlam2d())
724  {
725  if(isLandmarkWithRotation.at(idTag))
726  {
727  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
728  if(!isCovarianceIgnored())
729  {
730  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
731  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
732  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
733  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
734  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
735  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
736  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
737  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
738  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
739  }
740  g2o::EdgeSE2 * e = new g2o::EdgeSE2();
741  g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
742  g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
743  UASSERT(v1 != 0);
744  UASSERT(v2 != 0);
745  e->setVertex(0, v1);
746  e->setVertex(1, v2);
747  e->setMeasurement(g2o::SE2(t.x(), t.y(), t.theta()));
748  e->setInformation(information);
749  edge = e;
750  }
751  else
752  {
753  Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
754  if(!isCovarianceIgnored())
755  {
756  cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,2), cv::Range(0,2)).clone();
757  memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
758  }
759  g2o::EdgeSE2PointXY* e = new g2o::EdgeSE2PointXY;
760  e->vertices()[0] = optimizer.vertex(id1);
761  e->vertices()[1] = optimizer.vertex(id2);
762  e->setMeasurement(Eigen::Vector2d(t.x(), t.y()));
763  e->setInformation(information);
764  e->setParameterId(0, PARAM_OFFSET);
765  edge = e;
766  }
767  }
768  else
769  {
770  if(isLandmarkWithRotation.at(idTag))
771  {
772  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
773  if(!isCovarianceIgnored())
774  {
775  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
776  }
777 
778  Eigen::Affine3d a = t.toEigen3d();
779  Eigen::Isometry3d constraint;
780  constraint = a.linear();
781  constraint.translation() = a.translation();
782 
783  g2o::EdgeSE3 * e = new g2o::EdgeSE3();
784  g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
785  g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
786  UASSERT(v1 != 0);
787  UASSERT(v2 != 0);
788  e->setVertex(0, v1);
789  e->setVertex(1, v2);
790  e->setMeasurement(constraint);
791  e->setInformation(information);
792  edge = e;
793  }
794  else
795  {
796  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
797  if(!isCovarianceIgnored())
798  {
799  cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();
800  memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
801  }
802 
803  g2o::EdgeSE3PointXYZ* e = new g2o::EdgeSE3PointXYZ;
804  e->vertices()[0] = optimizer.vertex(id1);
805  e->vertices()[1] = optimizer.vertex(id2);
806  e->setMeasurement(Eigen::Vector3d(t.x(), t.y(), t.z()));
807  e->setInformation(information);
808  e->setParameterId(0, PARAM_OFFSET);
809  edge = e;
810  }
811  }
812  }
813  }
814  else
815  {
816 #if defined(RTABMAP_VERTIGO)
817  VertexSwitchLinear * v = 0;
818  if(this->isRobust() &&
819  iter->second.type() != Link::kNeighbor &&
820  iter->second.type() != Link::kNeighborMerged)
821  {
822  // For loop closure links, add switchable edges
823 
824  // create new switch variable
825  // Sunderhauf IROS 2012:
826  // "Since it is reasonable to initially accept all loop closure constraints,
827  // a proper and convenient initial value for all switch variables would be
828  // sij = 1 when using the linear switch function"
829  v = new VertexSwitchLinear();
830  v->setEstimate(1.0);
831  v->setId(vertigoVertexId++);
832  UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str());
833 
834  // create switch prior factor
835  // "If the front-end is not able to assign sound individual values
836  // for Ξij , it is save to set all Ξij = 1, since this value is close
837  // to the individual optimal choice of Ξij for a large range of
838  // outliers."
839  EdgeSwitchPrior * prior = new EdgeSwitchPrior();
840  prior->setMeasurement(1.0);
841  prior->setVertex(0, v);
842  UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
843  }
844 #endif
845 
846  if(isSlam2d())
847  {
848  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
849  if(!isCovarianceIgnored())
850  {
851  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
852  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
853  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
854  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
855  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
856  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
857  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
858  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
859  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
860  }
861 
862 #if defined(RTABMAP_VERTIGO)
863  if(this->isRobust() &&
864  iter->second.type() != Link::kNeighbor &&
865  iter->second.type() != Link::kNeighborMerged)
866  {
867  EdgeSE2Switchable * e = new EdgeSE2Switchable();
868  g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
869  g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
870  UASSERT(v1 != 0);
871  UASSERT(v2 != 0);
872  e->setVertex(0, v1);
873  e->setVertex(1, v2);
874  e->setVertex(2, v);
875  e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
876  e->setInformation(information);
877  edge = e;
878  }
879  else
880 #endif
881  {
882  g2o::EdgeSE2 * e = new g2o::EdgeSE2();
883  g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
884  g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
885  UASSERT(v1 != 0);
886  UASSERT(v2 != 0);
887  e->setVertex(0, v1);
888  e->setVertex(1, v2);
889  e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
890  e->setInformation(information);
891  edge = e;
892  }
893  }
894  else
895  {
896  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
897  if(!isCovarianceIgnored())
898  {
899  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
900  }
901 
902  Eigen::Affine3d a = iter->second.transform().toEigen3d();
903  Eigen::Isometry3d constraint;
904  constraint = a.linear();
905  constraint.translation() = a.translation();
906 
907 #if defined(RTABMAP_VERTIGO)
908  if(this->isRobust() &&
909  iter->second.type() != Link::kNeighbor &&
910  iter->second.type() != Link::kNeighborMerged)
911  {
912  EdgeSE3Switchable * e = new EdgeSE3Switchable();
913  g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
914  g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
915  UASSERT(v1 != 0);
916  UASSERT(v2 != 0);
917  e->setVertex(0, v1);
918  e->setVertex(1, v2);
919  e->setVertex(2, v);
920  e->setMeasurement(constraint);
921  e->setInformation(information);
922  edge = e;
923  }
924  else
925 #endif
926  {
927  g2o::EdgeSE3 * e = new g2o::EdgeSE3();
928  g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
929  g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
930  UASSERT(v1 != 0);
931  UASSERT(v2 != 0);
932  e->setVertex(0, v1);
933  e->setVertex(1, v2);
934  e->setMeasurement(constraint);
935  e->setInformation(information);
936  edge = e;
937  }
938  }
939  }
940 
941  if (edge && !optimizer.addEdge(edge))
942  {
943  delete edge;
944  UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
945  }
946 
947 
948  }
949 
950  UDEBUG("Initial optimization...");
951  optimizer.initializeOptimization();
952 
953  if(!optimizer.verifyInformationMatrices(true))
954  {
955  UERROR("This error can be caused by (1) bad covariance matrix "
956  "set in odometry messages "
957  "(see requirements in g2o::OptimizableGraph::verifyInformationMatrices() function) "
958  "or that (2) PCL and g2o hadn't "
959  "been built both with or without \"-march=native\" compilation "
960  "flag (if one library is built with this flag and not the other, "
961  "this is causing Eigen to not work properly, resulting in segmentation faults).");
962  return optimizedPoses;
963  }
964 
965  UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
966  int it = 0;
967  UTimer timer;
968  double lastError = 0.0;
969  if(intermediateGraphes || this->epsilon() > 0.0)
970  {
971  for(int i=0; i<iterations(); ++i)
972  {
973  if(intermediateGraphes)
974  {
975  if(i > 0)
976  {
977  std::map<int, Transform> tmpPoses;
978  if(isSlam2d())
979  {
980  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
981  {
982  int id = iter->first;
983  if(id > 0)
984  {
985  const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(id);
986  if(v)
987  {
988  float roll, pitch, yaw;
989  iter->second.getEulerAngles(roll, pitch, yaw);
990  Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
991  tmpPoses.insert(std::pair<int, Transform>(id, t));
992  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
993  }
994  else
995  {
996  UERROR("Vertex %d not found!?", id);
997  }
998  }
999  else if(!landmarksIgnored())
1000  {
1001  const g2o::OptimizableGraph::Vertex* v = (const g2o::OptimizableGraph::Vertex*)optimizer.vertex(landmarkVertexOffset - id);
1002  if(v)
1003  {
1004  if(isLandmarkWithRotation.at(id))
1005  {
1006  const g2o::VertexSE2* vSE2 = (const g2o::VertexSE2*)v;
1007  float roll, pitch, yaw;
1008  iter->second.getEulerAngles(roll, pitch, yaw);
1009  Transform t(vSE2->estimate().translation()[0], vSE2->estimate().translation()[1], iter->second.z(), roll, pitch, vSE2->estimate().rotation().angle());
1010  tmpPoses.insert(std::pair<int, Transform>(id, t));
1011  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1012  }
1013  else
1014  {
1015  const g2o::VertexPointXY* vP = (const g2o::VertexPointXY*)v;
1016  float roll, pitch, yaw;
1017  iter->second.getEulerAngles(roll, pitch, yaw);
1018  Transform t(vP->estimate()[0], vP->estimate()[1], iter->second.z(), roll, pitch, yaw);
1019  tmpPoses.insert(std::pair<int, Transform>(id, t));
1020  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1021  }
1022  }
1023  else
1024  {
1025  UERROR("Vertex %d not found!?", id);
1026  }
1027  }
1028  }
1029  }
1030  else
1031  {
1032  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
1033  {
1034  int id = iter->first;
1035  if(id > 0)
1036  {
1037  const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(id);
1038  if(v)
1039  {
1040  Transform t = Transform::fromEigen3d(v->estimate());
1041  tmpPoses.insert(std::pair<int, Transform>(id, t));
1042  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1043  }
1044  else
1045  {
1046  UERROR("Vertex %d not found!?", id);
1047  }
1048  }
1049  else if(!landmarksIgnored())
1050  {
1051  const g2o::OptimizableGraph::Vertex* v = (const g2o::OptimizableGraph::Vertex*)optimizer.vertex(landmarkVertexOffset - id);
1052  if(v)
1053  {
1054  if(isLandmarkWithRotation.at(id))
1055  {
1056  const g2o::VertexSE3* vSE3 = (const g2o::VertexSE3*)v;
1057  Transform t = Transform::fromEigen3d(vSE3->estimate());
1058  tmpPoses.insert(std::pair<int, Transform>(id, t));
1059  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1060  }
1061  else
1062  {
1063  const g2o::VertexPointXYZ* vP = (const g2o::VertexPointXYZ*)v;
1064  float roll, pitch, yaw;
1065  iter->second.getEulerAngles(roll, pitch, yaw);
1066  Transform t(vP->estimate()[0], vP->estimate()[1], vP->estimate()[2], roll, pitch, yaw);
1067  tmpPoses.insert(std::pair<int, Transform>(id, t));
1068  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1069  }
1070  }
1071  else
1072  {
1073  UERROR("Vertex %d not found!?", id);
1074  }
1075  }
1076  }
1077  }
1078  intermediateGraphes->push_back(tmpPoses);
1079  }
1080  }
1081 
1082  it += optimizer.optimize(1);
1083 
1084  // early stop condition
1085  optimizer.computeActiveErrors();
1086  double chi2 = optimizer.activeRobustChi2();
1087  UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
1088 
1089  if(i>0 && optimizer.activeRobustChi2() > 1000000000000.0)
1090  {
1091  UERROR("g2o: Large optimimzation error detected (%f), aborting optimization!");
1092  return optimizedPoses;
1093  }
1094 
1095  double errorDelta = lastError - chi2;
1096  if(i>0 && errorDelta < this->epsilon())
1097  {
1098  if(errorDelta < 0)
1099  {
1100  UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
1101  }
1102  else
1103  {
1104  UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
1105  break;
1106  }
1107  }
1108  else if(i==0 && chi2 < this->epsilon())
1109  {
1110  UINFO("Stop optimizing, error is already under epsilon (%f < %f)", chi2, this->epsilon());
1111  break;
1112  }
1113  lastError = chi2;
1114  }
1115  }
1116  else
1117  {
1118  it = optimizer.optimize(iterations());
1119  optimizer.computeActiveErrors();
1120  UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
1121  }
1122  if(finalError)
1123  {
1124  *finalError = lastError;
1125  }
1126  if(iterationsDone)
1127  {
1128  *iterationsDone = it;
1129  }
1130  UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
1131 
1132  if(optimizer.activeRobustChi2() > 1000000000000.0)
1133  {
1134  UERROR("g2o: Large optimimzation error detected (%f), aborting optimization!");
1135  return optimizedPoses;
1136  }
1137 
1138  if(isSlam2d())
1139  {
1140  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
1141  {
1142  int id = iter->first;
1143  if(id > 0)
1144  {
1145  const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(id);
1146  if(v)
1147  {
1148  float roll, pitch, yaw;
1149  iter->second.getEulerAngles(roll, pitch, yaw);
1150  Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
1151  optimizedPoses.insert(std::pair<int, Transform>(id, t));
1152  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1153  }
1154  else
1155  {
1156  UERROR("Vertex %d not found!?", id);
1157  }
1158  }
1159  else if(!landmarksIgnored())
1160  {
1161  const g2o::OptimizableGraph::Vertex* v = (const g2o::OptimizableGraph::Vertex*)optimizer.vertex(landmarkVertexOffset - id);
1162 
1163  if(v)
1164  {
1165  if(isLandmarkWithRotation.at(id))
1166  {
1167  const g2o::VertexSE2* vSE2 = (const g2o::VertexSE2*)v;
1168  float roll, pitch, yaw;
1169  iter->second.getEulerAngles(roll, pitch, yaw);
1170  Transform t(vSE2->estimate().translation()[0], vSE2->estimate().translation()[1], iter->second.z(), roll, pitch, vSE2->estimate().rotation().angle());
1171  optimizedPoses.insert(std::pair<int, Transform>(id, t));
1172  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1173  }
1174  else
1175  {
1176  const g2o::VertexPointXY* vP = (const g2o::VertexPointXY*)v;
1177  float roll, pitch, yaw;
1178  iter->second.getEulerAngles(roll, pitch, yaw);
1179  Transform t(vP->estimate()[0], vP->estimate()[1], iter->second.z(), roll, pitch, yaw);
1180  optimizedPoses.insert(std::pair<int, Transform>(id, t));
1181  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1182  }
1183  }
1184  else
1185  {
1186  UERROR("Vertex %d not found!?", id);
1187  }
1188  }
1189  }
1190 
1191  g2o::VertexSE2* v = (g2o::VertexSE2*)optimizer.vertex(poses.rbegin()->first);
1192  if(v)
1193  {
1194  UTimer t;
1195  g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
1196  optimizer.computeMarginals(spinv, v);
1197  UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
1198  if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
1199  {
1200  g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
1201  UASSERT(block && block->cols() == 3 && block->cols() == 3);
1202  outputCovariance.at<double>(0,0) = (*block)(0,0); // x-x
1203  outputCovariance.at<double>(0,1) = (*block)(0,1); // x-y
1204  outputCovariance.at<double>(0,5) = (*block)(0,2); // x-theta
1205  outputCovariance.at<double>(1,0) = (*block)(1,0); // y-x
1206  outputCovariance.at<double>(1,1) = (*block)(1,1); // y-y
1207  outputCovariance.at<double>(1,5) = (*block)(1,2); // y-theta
1208  outputCovariance.at<double>(5,0) = (*block)(2,0); // theta-x
1209  outputCovariance.at<double>(5,1) = (*block)(2,1); // theta-y
1210  outputCovariance.at<double>(5,5) = (*block)(2,2); // theta-theta
1211  }
1212  else if(v->hessianIndex() < 0)
1213  {
1214  UWARN("Computing marginals: vertex %d has negative hessian index (%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex());
1215  }
1216  else
1217  {
1218  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());
1219  }
1220  }
1221  else
1222  {
1223  UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
1224  }
1225  }
1226  else
1227  {
1228  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
1229  {
1230  int id = iter->first;
1231  if(id > 0)
1232  {
1233  const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(id);
1234  if(v)
1235  {
1236  Transform t = Transform::fromEigen3d(v->estimate());
1237  optimizedPoses.insert(std::pair<int, Transform>(id, t));
1238  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1239  }
1240  else
1241  {
1242  UERROR("Vertex %d not found!?", id);
1243  }
1244  }
1245  else if(!landmarksIgnored())
1246  {
1247  const g2o::OptimizableGraph::Vertex* v = (const g2o::OptimizableGraph::Vertex*)optimizer.vertex(landmarkVertexOffset - id);
1248 
1249  if(v)
1250  {
1251  if(isLandmarkWithRotation.at(id))
1252  {
1253  const g2o::VertexSE3* vSE3 = (const g2o::VertexSE3*)v;
1254  Transform t = Transform::fromEigen3d(vSE3->estimate());
1255  optimizedPoses.insert(std::pair<int, Transform>(id, t));
1256  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1257  }
1258  else
1259  {
1260  const g2o::VertexPointXYZ* vP = (const g2o::VertexPointXYZ*)v;
1261  float roll, pitch, yaw;
1262  iter->second.getEulerAngles(roll, pitch, yaw);
1263  Transform t(vP->estimate()[0], vP->estimate()[1], vP->estimate()[2], roll, pitch, yaw);
1264  optimizedPoses.insert(std::pair<int, Transform>(id, t));
1265  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1266  }
1267  }
1268  else
1269  {
1270  UERROR("Vertex %d not found!?", id);
1271  }
1272  }
1273  }
1274 
1275  g2o::VertexSE3* v = (g2o::VertexSE3*)optimizer.vertex(poses.rbegin()->first);
1276  if(v)
1277  {
1278  UTimer t;
1279  g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
1280  optimizer.computeMarginals(spinv, v);
1281  UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
1282  if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
1283  {
1284  g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
1285  UASSERT(block && block->cols() == 6 && block->cols() == 6);
1286  memcpy(outputCovariance.data, block->data(), outputCovariance.total()*sizeof(double));
1287  }
1288  else if(v->hessianIndex() < 0)
1289  {
1290  UWARN("Computing marginals: vertex %d has negative hessian index (%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex());
1291  }
1292 #ifdef RTABMAP_G2O_CPP11
1293  else
1294  {
1295  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());
1296  }
1297 #endif
1298  }
1299  else
1300  {
1301  UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
1302  }
1303  }
1304  }
1305  else if(poses.size() == 1 || iterations() <= 0)
1306  {
1307  optimizedPoses = poses;
1308  }
1309  else
1310  {
1311  UWARN("This method should be called at least with 1 pose!");
1312  }
1313  UDEBUG("Optimizing graph...end!");
1314 #else
1315 #ifdef RTABMAP_ORB_SLAM
1316  UERROR("G2O graph optimization cannot be used with g2o built from ORB_SLAM, only SBA is available.");
1317 #else
1318  UERROR("Not built with G2O support!");
1319 #endif
1320 #endif
1321  return optimizedPoses;
1322 }
1323 
1324 #ifdef RTABMAP_ORB_SLAM
1325 
1328  class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>
1329 {
1330  public:
1331  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
1332  EdgeSE3Expmap(): BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){}
1333  bool read(std::istream& is)
1334  {
1335  return false;
1336  }
1337 
1338  bool write(std::ostream& os) const
1339  {
1340  return false;
1341  }
1342 
1343  void computeError()
1344  {
1345  const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
1346  const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
1347  g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
1348  _error[0]=delta.translation().x();
1349  _error[1]=delta.translation().y();
1350  _error[2]=delta.translation().z();
1351  _error[3]=delta.rotation().x();
1352  _error[4]=delta.rotation().y();
1353  _error[5]=delta.rotation().z();
1354  }
1355 
1356  virtual void setMeasurement(const g2o::SE3Quat& meas){
1357  _measurement=meas;
1358  _inverseMeasurement=meas.inverse();
1359  }
1360 
1361  virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;}
1362  virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){
1363  g2o::VertexSE3Expmap* from = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
1364  g2o::VertexSE3Expmap* to = static_cast<g2o::VertexSE3Expmap*>(_vertices[1]);
1365  if (from_.count(from) > 0)
1366  to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement);
1367  else
1368  from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement);
1369  }
1370 
1371  virtual bool setMeasurementData(const double* d){
1372  Eigen::Map<const g2o::Vector7d> v(d);
1373  _measurement.fromVector(v);
1374  _inverseMeasurement = _measurement.inverse();
1375  return true;
1376  }
1377 
1378  virtual bool getMeasurementData(double* d) const{
1379  Eigen::Map<g2o::Vector7d> v(d);
1380  v = _measurement.toVector();
1381  return true;
1382  }
1383 
1384  virtual int measurementDimension() const {return 7;}
1385 
1386  virtual bool setMeasurementFromState() {
1387  const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
1388  const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
1389  _measurement = (v1->estimate().inverse()*v2->estimate());
1390  _inverseMeasurement = _measurement.inverse();
1391  return true;
1392  }
1393 
1394  protected:
1395  g2o::SE3Quat _inverseMeasurement;
1396 };
1397 #endif
1398 
1399 std::map<int, Transform> OptimizerG2O::optimizeBA(
1400  int rootId,
1401  const std::map<int, Transform> & poses,
1402  const std::multimap<int, Link> & links,
1403  const std::map<int, std::vector<CameraModel> > & models,
1404  std::map<int, cv::Point3f> & points3DMap,
1405  const std::map<int, std::map<int, FeatureBA> > & wordReferences,
1406  std::set<int> * outliers)
1407 {
1408  std::map<int, Transform> optimizedPoses;
1409 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
1410  UDEBUG("Optimizing graph...");
1411 
1412  optimizedPoses.clear();
1413  if(poses.size()>=2 && iterations() > 0 && (models.size() == poses.size() || poses.begin()->first < 0))
1414  {
1415  g2o::SparseOptimizer optimizer;
1416  //optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
1417 #if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM)
1418  std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
1419 #else
1420  g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
1421 #endif
1422 
1423 #ifdef RTABMAP_ORB_SLAM
1424  linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
1425 #else
1426  if(solver_ == 3)
1427  {
1428  //eigen
1429 #ifdef RTABMAP_G2O_CPP11
1430  linearSolver = g2o::make_unique<g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType> >();
1431 #else
1432  linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
1433 #endif
1434  }
1435 #ifdef G2O_HAVE_CHOLMOD
1436  else if(solver_ == 2)
1437  {
1438  //chmold
1439 #ifdef RTABMAP_G2O_CPP11
1440  linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> >();
1441 #else
1442  linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
1443 #endif
1444  }
1445 #endif
1446 #ifdef G2O_HAVE_CSPARSE
1447  else if(solver_ == 0)
1448  {
1449  //csparse
1450 #ifdef RTABMAP_G2O_CPP11
1451  linearSolver = g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType> >();
1452 #else
1453  linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
1454 #endif
1455  }
1456 #endif
1457  else
1458  {
1459  //pcg
1460 #ifdef RTABMAP_G2O_CPP11
1461  linearSolver = g2o::make_unique<g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType> >();
1462 #else
1463  linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
1464 #endif
1465  }
1466 #endif // RTABMAP_ORB_SLAM
1467 
1468 #ifndef RTABMAP_ORB_SLAM
1469  if(optimizer_ == 1)
1470  {
1471 #ifdef RTABMAP_G2O_CPP11
1472  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(
1473  g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
1474 #else
1475  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(new g2o::BlockSolver_6_3(linearSolver)));
1476 #endif
1477  }
1478  else
1479 #endif
1480  {
1481 #if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM)
1482  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
1483  g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
1484 #else
1485  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(new g2o::BlockSolver_6_3(linearSolver)));
1486 #endif
1487  }
1488 
1489 
1490  UDEBUG("fill poses to g2o...");
1491  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
1492  {
1493  if(iter->first > 0)
1494  {
1495  // Get camera model
1496  std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
1497  UASSERT(iterModel != models.end() && !iterModel->second.empty());
1498  for(size_t i=0; i<iterModel->second.size(); ++i)
1499  {
1500  UASSERT(iterModel->second[i].isValidForProjection());
1501 
1502  Transform camPose = iter->second * iterModel->second[i].localTransform();
1503 
1504  // Add node's pose
1505  UASSERT(!camPose.isNull());
1506 #ifdef RTABMAP_ORB_SLAM
1507  g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
1508 #else
1509  g2o::VertexCam * vCam = new g2o::VertexCam();
1510 #endif
1511 
1512  Eigen::Affine3d a = camPose.toEigen3d();
1513 #ifdef RTABMAP_ORB_SLAM
1514  a = a.inverse();
1515  vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
1516 #else
1517  g2o::SBACam cam(Eigen::Quaterniond(a.linear()), a.translation());
1518  cam.setKcam(
1519  iterModel->second[i].fx(),
1520  iterModel->second[i].fy(),
1521  iterModel->second[i].cx(),
1522  iterModel->second[i].cy(),
1523  iterModel->second[i].Tx()<0.0?-iterModel->second[i].Tx()/iterModel->second[i].fx():baseline_); // baseline in meters
1524  vCam->setEstimate(cam);
1525 #endif
1526  vCam->setId(iter->first*MULTICAM_OFFSET + i);
1527 
1528  // negative root means that all other poses should be fixed instead of the root
1529  vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
1530 
1531  /*UDEBUG("camPose %d (camid=%d) (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
1532  iter->first,
1533  vCam->id(),
1534  vCam->fixed()?1:0,
1535  iterModel->second[i].fx(),
1536  iterModel->second[i].fy(),
1537  iterModel->second[i].cx(),
1538  iterModel->second[i].cy(),
1539  iterModel->second[i].Tx(),
1540  iterModel->second[i].Tx()<0.0?-iterModel->second[i].Tx()/iterModel->second[i].fx():baseline_,
1541  camPose.prettyPrint().c_str());*/
1542 
1543  UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert cam vertex %d (pose=%d)!?", vCam->id(), iter->first).c_str());
1544  }
1545  }
1546  }
1547 
1548  UDEBUG("fill edges to g2o...");
1549  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
1550  {
1551  if(iter->second.from() > 0 &&
1552  iter->second.to() > 0 &&
1553  uContains(poses, iter->second.from()) &&
1554  uContains(poses, iter->second.to()))
1555  {
1556  // add edge
1557  int id1 = iter->second.from();
1558  int id2 = iter->second.to();
1559 
1560  if(id1 == id2)
1561  {
1562 #ifndef RTABMAP_ORB_SLAM
1563  g2o::HyperGraph::Edge * edge = 0;
1564  if(gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
1565  {
1566  Eigen::Matrix<double, 6, 1> m;
1567  // Up vector in robot frame
1568  m.head<3>() = Eigen::Vector3d::UnitZ();
1569  // Observed Gravity vector in world frame
1570  float roll, pitch, yaw;
1571  iter->second.transform().getEulerAngles(roll, pitch, yaw);
1572  m.tail<3>() = Transform(0,0,0,roll,pitch,0).toEigen3d() * -Eigen::Vector3d::UnitZ();
1573 
1574  Eigen::MatrixXd information = Eigen::MatrixXd::Identity(3, 3) * 1.0/(gravitySigma()*gravitySigma());
1575 
1576  g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
1577  EdgeSBACamGravity* priorEdge(new EdgeSBACamGravity());
1578  std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
1579  // Gravity constraint added only to first camera of a pose
1580  UASSERT(iterModel != models.end() && !iterModel->second.empty() && !iterModel->second[0].localTransform().isNull());
1581  priorEdge->setCameraInvLocalTransform(iterModel->second[0].localTransform().inverse().toEigen3d().linear());
1582  priorEdge->setMeasurement(m);
1583  priorEdge->setInformation(information);
1584  priorEdge->vertices()[0] = v1;
1585  edge = priorEdge;
1586  }
1587  if (edge && !optimizer.addEdge(edge))
1588  {
1589  delete edge;
1590  UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
1591  return optimizedPoses;
1592  }
1593 #endif
1594  }
1595  else if(id1>0 && id2>0) // not supporting landmarks
1596  {
1597  UASSERT(!iter->second.transform().isNull());
1598 
1599  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
1600  if(!isCovarianceIgnored())
1601  {
1602  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
1603  }
1604 
1605  // between cameras, not base_link
1606  Transform camLink = models.at(id1)[0].localTransform().inverse()*iter->second.transform()*models.at(id2)[0].localTransform();
1607  /*UDEBUG("added edge %d->%d (camIDs %d->%d) (in cam frame=%s)",
1608  id1,
1609  id2,
1610  id1*MULTICAM_OFFSET,
1611  id2*MULTICAM_OFFSET,
1612  camLink.prettyPrint().c_str());*/
1613 #ifdef RTABMAP_ORB_SLAM
1614  EdgeSE3Expmap * e = new EdgeSE3Expmap();
1615  g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id1*MULTICAM_OFFSET);
1616  g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id2*MULTICAM_OFFSET);
1617 
1618  Transform camPose1 = Transform::fromEigen3d(v1->estimate()).inverse();
1619  Transform camPose2Inv = Transform::fromEigen3d(v2->estimate());
1620 
1621  camLink = camPose1 * camPose1 * camLink * camPose2Inv * camPose2Inv;
1622 #else
1623  g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
1624  g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
1625  g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2*MULTICAM_OFFSET);
1626 #endif
1627  UASSERT(v1 != 0);
1628  UASSERT(v2 != 0);
1629  e->setVertex(0, v1);
1630  e->setVertex(1, v2);
1631  Eigen::Affine3d a = camLink.toEigen3d();
1632  e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
1633  e->setInformation(information);
1634 
1635  if (!optimizer.addEdge(e))
1636  {
1637  delete e;
1638  UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
1639  return optimizedPoses;
1640  }
1641  }
1642  }
1643  }
1644 
1645  UDEBUG("fill hard edges between camera 0 and other cameras (multicam)...");
1646  for(std::map<int, std::vector<CameraModel> >::const_iterator iter=models.begin(); iter!=models.end(); ++iter)
1647  {
1648  int id = iter->first;
1649  if(uContains(poses, id))
1650  {
1651  for(size_t i=1; i<iter->second.size(); ++i)
1652  {
1653  // add edge
1654  // Set large information matrix to keep these links fixed
1655  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*9999999;
1656 
1657  // between cameras, not base_link
1658  Transform camLink = iter->second[0].localTransform().inverse()*iter->second[i].localTransform();
1659 #ifdef RTABMAP_ORB_SLAM
1660  EdgeSE3Expmap * e = new EdgeSE3Expmap();
1661  g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id*MULTICAM_OFFSET);
1662  g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id*MULTICAM_OFFSET+i);
1663 
1664  Transform camPose1 = Transform::fromEigen3d(v1->estimate()).inverse();
1665  Transform camPose2Inv = Transform::fromEigen3d(v2->estimate());
1666 
1667  camLink = camPose1 * camPose1 * camLink * camPose2Inv * camPose2Inv;
1668 #else
1669  g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
1670  g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id*MULTICAM_OFFSET);
1671  g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id*MULTICAM_OFFSET+i);
1672 #endif
1673  /*UDEBUG("added edge between subcam 0->%d id:%d->%d (in cam frame=%s)",
1674  i,
1675  v1->id(),
1676  v2->id(),
1677  camLink.prettyPrint().c_str());*/
1678 
1679  UASSERT(v1 != 0);
1680  UASSERT(v2 != 0);
1681  e->setVertex(0, v1);
1682  e->setVertex(1, v2);
1683  Eigen::Affine3d a = camLink.toEigen3d();
1684  e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
1685  e->setInformation(information);
1686 
1687  if (!optimizer.addEdge(e))
1688  {
1689  delete e;
1690  UERROR("Map: Failed adding constraint between %d and %d, skipping", v1->id(), v2->id());
1691  return optimizedPoses;
1692  }
1693  }
1694  }
1695  }
1696 
1697  UDEBUG("fill 3D points to g2o...");
1698  const int stepVertexId = poses.rbegin()->first*MULTICAM_OFFSET+MULTICAM_OFFSET;
1699  int negVertexOffset = stepVertexId;
1700  if(wordReferences.size() && wordReferences.rbegin()->first>0)
1701  {
1702  negVertexOffset += wordReferences.rbegin()->first;
1703  }
1704  UDEBUG("stepVertexId=%d, negVertexOffset=%d", stepVertexId, negVertexOffset);
1705  std::list<g2o::OptimizableGraph::Edge*> edges;
1706  for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
1707  {
1708  int id = iter->first;
1709  if(points3DMap.find(id) != points3DMap.end())
1710  {
1711  cv::Point3f pt3d = points3DMap.at(id);
1712  if(!util3d::isFinite(pt3d))
1713  {
1714  UWARN("Ignoring 3D point %d because it has nan value(s)!", id);
1715  continue;
1716  }
1717  g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
1718 
1719  vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
1720  if(id<0)
1721  {
1722  vpt3d->setId(negVertexOffset + id*-1);
1723  }
1724  else
1725  {
1726  vpt3d->setId(stepVertexId + id);
1727  }
1728  UASSERT(vpt3d->id() > 0);
1729  vpt3d->setMarginalized(true);
1730  optimizer.addVertex(vpt3d);
1731 
1732  //UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
1733 
1734  // set observations
1735  for(std::map<int, FeatureBA>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
1736  {
1737  int poseId = jter->first;
1738  int camIndex = jter->second.cameraIndex;
1739  int camId = poseId*MULTICAM_OFFSET+camIndex;
1740  if(poses.find(poseId) != poses.end() && optimizer.vertex(camId) != 0)
1741  {
1742  const FeatureBA & pt = jter->second;
1743  double depth = pt.depth;
1744 
1745  //UDEBUG("Added observation pt=%d to cam=%d (%d,%d) depth=%f (camIndex=%d)", vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth, camIndex);
1746 
1747  g2o::OptimizableGraph::Edge * e;
1748  double baseline = 0.0;
1749 #ifdef RTABMAP_ORB_SLAM
1750  g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
1751  std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(poseId);
1752 
1753  UASSERT(iterModel != models.end() && camIndex<iterModel->second.size() && iterModel->second[camIndex].isValidForProjection());
1754  baseline = iterModel->second[camIndex].Tx()<0.0?-iterModel->second[camIndex].Tx()/iterModel->second[camIndex].fx():baseline_;
1755 #else
1756  g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
1757  baseline = vcam->estimate().baseline;
1758 #endif
1759  double variance = pixelVariance_;
1760  if(uIsFinite(depth) && depth > 0.0 && baseline > 0.0)
1761  {
1762  // stereo edge
1763 #ifdef RTABMAP_ORB_SLAM
1764  g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
1765  float disparity = baseline * iterModel->second[camIndex].fx() / depth;
1766  Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
1767  es->setMeasurement(obs);
1768  //variance *= log(exp(1)+disparity);
1769  es->setInformation(Eigen::Matrix3d::Identity() / variance);
1770  es->fx = iterModel->second[camIndex].fx();
1771  es->fy = iterModel->second[camIndex].fy();
1772  es->cx = iterModel->second[camIndex].cx();
1773  es->cy = iterModel->second[camIndex].cy();
1774  es->bf = baseline*es->fx;
1775  e = es;
1776 #else
1777  g2o::EdgeProjectP2SC* es = new g2o::EdgeProjectP2SC();
1778  float disparity = baseline * vcam->estimate().Kcam(0,0) / depth;
1779  Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
1780  es->setMeasurement(obs);
1781  //variance *= log(exp(1)+disparity);
1782  es->setInformation(Eigen::Matrix3d::Identity() / variance);
1783  e = es;
1784 #endif
1785  }
1786  else
1787  {
1788  if(baseline > 0.0)
1789  {
1790  UDEBUG("Stereo camera model detected but current "
1791  "observation (pt=%d to cam=%d, kpt=[%d,%d]) has null depth (%f m), adding "
1792  "mono observation instead.",
1793  vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth);
1794  }
1795  // mono edge
1796 #ifdef RTABMAP_ORB_SLAM
1797  g2o::EdgeSE3ProjectXYZ* em = new g2o::EdgeSE3ProjectXYZ();
1798  Eigen::Vector2d obs( pt.kpt.pt.x, pt.kpt.pt.y);
1799  em->setMeasurement(obs);
1800  em->setInformation(Eigen::Matrix2d::Identity() / variance);
1801  em->fx = iterModel->second[camIndex].fx();
1802  em->fy = iterModel->second[camIndex].fy();
1803  em->cx = iterModel->second[camIndex].cx();
1804  em->cy = iterModel->second[camIndex].cy();
1805  e = em;
1806 
1807 #else
1808  g2o::EdgeProjectP2MC* em = new g2o::EdgeProjectP2MC();
1809  Eigen::Vector2d obs( pt.kpt.pt.x, pt.kpt.pt.y);
1810  em->setMeasurement(obs);
1811  em->setInformation(Eigen::Matrix2d::Identity() / variance);
1812  e = em;
1813 #endif
1814  }
1815  e->setVertex(0, vpt3d);
1816  e->setVertex(1, vcam);
1817 
1818  if(robustKernelDelta_ > 0.0)
1819  {
1820  g2o::RobustKernelHuber* kernel = new g2o::RobustKernelHuber;
1821  kernel->setDelta(robustKernelDelta_);
1822  e->setRobustKernel(kernel);
1823  }
1824 
1825  optimizer.addEdge(e);
1826  edges.push_back(e);
1827  }
1828  }
1829  }
1830  }
1831 
1832  UDEBUG("Initial optimization...");
1833  optimizer.initializeOptimization();
1834 
1835  UASSERT(optimizer.verifyInformationMatrices());
1836 
1837  UDEBUG("g2o optimizing begin (max iterations=%d, robustKernel=%f)", iterations(), robustKernelDelta_);
1838 
1839  int it = 0;
1840  UTimer timer;
1841  int outliersCount = 0;
1842  int outliersCountFar = 0;
1843 
1844  for(int i=0; i<(robustKernelDelta_>0.0?2:1); ++i)
1845  {
1846  it += optimizer.optimize(i==0&&robustKernelDelta_>0.0?5:iterations());
1847 
1848  // early stop condition
1849  optimizer.computeActiveErrors();
1850  double chi2 = optimizer.activeRobustChi2();
1851 
1852  if(uIsNan(chi2))
1853  {
1854  UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
1855  return optimizedPoses;
1856  }
1857  UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
1858 
1859  if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
1860  {
1861  UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
1862  return optimizedPoses;
1863  }
1864 
1865  if(robustKernelDelta_>0.0)
1866  {
1867  for(std::list<g2o::OptimizableGraph::Edge*>::iterator iter=edges.begin(); iter!=edges.end();++iter)
1868  {
1869  if((*iter)->level() == 0 && (*iter)->chi2() > (*iter)->robustKernel()->delta())
1870  {
1871  (*iter)->setLevel(1);
1872  ++outliersCount;
1873  double d = 0.0;
1874 #ifdef RTABMAP_ORB_SLAM
1875  if(dynamic_cast<g2o::EdgeStereoSE3ProjectXYZ*>(*iter) != 0)
1876  {
1877  d = ((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[0]-((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[2];
1878  }
1879  //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());
1880 #else
1881  if(dynamic_cast<g2o::EdgeProjectP2SC*>(*iter) != 0)
1882  {
1883  d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
1884  }
1885  //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());
1886 #endif
1887 
1888  cv::Point3f pt3d;
1889  if((*iter)->vertex(0)->id() > negVertexOffset)
1890  {
1891  pt3d = points3DMap.at(negVertexOffset - (*iter)->vertex(0)->id());
1892  }
1893  else
1894  {
1895  pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
1896  }
1897  ((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
1898 
1899  if(outliers)
1900  {
1901  outliers->insert((*iter)->vertex(0)->id()-stepVertexId);
1902  }
1903  if(d < 5.0)
1904  {
1905  outliersCountFar++;
1906  }
1907  }
1908  //(*iter)->setRobustKernel(0);
1909  }
1910  if(i==0)
1911  optimizer.initializeOptimization(0);
1912  UDEBUG("outliers=%d outliersCountFar=%d", outliersCount, outliersCountFar);
1913  }
1914  }
1915  UDEBUG("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());
1916 
1917  if(optimizer.activeRobustChi2() > 1000000000000.0)
1918  {
1919  UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
1920  return optimizedPoses;
1921  }
1922 
1923  // update poses
1924  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
1925  {
1926  if(iter->first > 0)
1927  {
1928  int camId = iter->first*MULTICAM_OFFSET;
1929 #ifdef RTABMAP_ORB_SLAM
1930  const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(camId);
1931 #else
1932  const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(camId);
1933 #endif
1934  if(v)
1935  {
1936  Transform t = Transform::fromEigen3d(v->estimate());
1937 
1938 #ifdef RTABMAP_ORB_SLAM
1939  t=t.inverse();
1940 #endif
1941 
1942  // remove model local transform
1943  t *= models.at(iter->first)[0].localTransform().inverse();
1944 
1945  //UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
1946  if(t.isNull())
1947  {
1948  UERROR("Optimized pose %d is null!?!?", iter->first);
1949  optimizedPoses.clear();
1950  return optimizedPoses;
1951  }
1952 
1953  // FIXME: is there a way that we can add the 2D constraint directly in SBA?
1954  if(this->isSlam2d())
1955  {
1956  // get transform between old and new pose
1957  t = iter->second.inverse() * t;
1958  optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
1959  }
1960  else
1961  {
1962  optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
1963  }
1964  }
1965  else
1966  {
1967  UERROR("Vertex (pose) %d (cam=%d) not found!?", iter->first, camId);
1968  }
1969  }
1970  }
1971 
1972  //update points3D
1973 
1974  for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter!=points3DMap.end(); ++iter)
1975  {
1976  const g2o::VertexSBAPointXYZ* v;
1977  int id = iter->first;
1978  if(id<0)
1979  {
1980  v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(negVertexOffset + id*-1);
1981  }
1982  else
1983  {
1984  v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + id);
1985  }
1986 
1987  if(v)
1988  {
1989  cv::Point3f p(v->estimate()[0], v->estimate()[1], v->estimate()[2]);
1990  //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);
1991  iter->second = p;
1992  }
1993  else
1994  {
1995  iter->second.x = iter->second.y = iter->second.z = std::numeric_limits<float>::quiet_NaN();
1996  }
1997  }
1998  }
1999  else if(poses.size() > 1 && (poses.size() != models.size() && poses.begin()->first > 0))
2000  {
2001  UERROR("This method should be called with size of poses = size camera models!");
2002  }
2003  else if(poses.size() == 1 || iterations() <= 0)
2004  {
2005  optimizedPoses = poses;
2006  }
2007  else
2008  {
2009  UWARN("This method should be called at least with 1 pose!");
2010  }
2011  UDEBUG("Optimizing graph...end!");
2012 #else
2013  UERROR("Not built with G2O support!");
2014 #endif
2015  return optimizedPoses;
2016 }
2017 
2018 bool OptimizerG2O::saveGraph(
2019  const std::string & fileName,
2020  const std::map<int, Transform> & poses,
2021  const std::multimap<int, Link> & edgeConstraints)
2022 {
2023  FILE * file = 0;
2024 
2025 #ifdef _MSC_VER
2026  fopen_s(&file, fileName.c_str(), "w");
2027 #else
2028  file = fopen(fileName.c_str(), "w");
2029 #endif
2030 
2031  if(file)
2032  {
2033  // force periods to be used instead of commas
2034  setlocale(LC_ALL, "en_US.UTF-8");
2035 
2036  if(isSlam2d())
2037  {
2038  // PARAMS_SE2OFFSET id x y theta (set for priors)
2039  fprintf(file, "PARAMS_SE2OFFSET %d 0 0 0\n", PARAM_OFFSET);
2040  }
2041  else
2042  {
2043  // PARAMS_SE3OFFSET id x y z qw qx qy qz (set for priors)
2044  Eigen::Vector3f v = Eigen::Vector3f::Zero();
2045  Eigen::Quaternionf q = Eigen::Quaternionf::Identity();
2046  fprintf(file, "PARAMS_SE3OFFSET %d %f %f %f %f %f %f %f\n",
2047  PARAM_OFFSET,
2048  v.x(),
2049  v.y(),
2050  v.z(),
2051  q.x(),
2052  q.y(),
2053  q.z(),
2054  q.w());
2055  }
2056 
2057  int landmarkOffset = poses.size()&&poses.rbegin()->first>0?poses.rbegin()->first+1:0;
2058  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2059  {
2060  if (isSlam2d())
2061  {
2062  if(iter->first > 0)
2063  {
2064  // VERTEX_SE2 id x y theta
2065  fprintf(file, "VERTEX_SE2 %d %f %f %f\n",
2066  landmarkOffset-iter->first,
2067  iter->second.x(),
2068  iter->second.y(),
2069  iter->second.theta());
2070  }
2071  else if(!landmarksIgnored())
2072  {
2073  // VERTEX_XY id x y
2074  fprintf(file, "VERTEX_XY %d %f %f\n",
2075  iter->first,
2076  iter->second.x(),
2077  iter->second.y());
2078  }
2079  }
2080  else
2081  {
2082  if(iter->first > 0)
2083  {
2084  // VERTEX_SE3 id x y z qw qx qy qz
2085  Eigen::Quaternionf q = iter->second.getQuaternionf();
2086  fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n",
2087  iter->first,
2088  iter->second.x(),
2089  iter->second.y(),
2090  iter->second.z(),
2091  q.x(),
2092  q.y(),
2093  q.z(),
2094  q.w());
2095  }
2096  else if(!landmarksIgnored())
2097  {
2098  // VERTEX_TRACKXYZ id x y z
2099  fprintf(file, "VERTEX_TRACKXYZ %d %f %f %f\n",
2100  landmarkOffset-iter->first,
2101  iter->second.x(),
2102  iter->second.y(),
2103  iter->second.z());
2104  }
2105  }
2106  }
2107 
2108  int virtualVertexId = landmarkOffset - (poses.size()&&poses.rbegin()->first<0?poses.rbegin()->first:0);
2109  for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
2110  {
2111  if (iter->second.type() == Link::kLandmark)
2112  {
2113  if (this->landmarksIgnored())
2114  {
2115  continue;
2116  }
2117  if(isSlam2d())
2118  {
2119  // EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
2120  fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f\n",
2121  iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
2122  iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
2123  iter->second.transform().x(),
2124  iter->second.transform().y(),
2125  iter->second.infMatrix().at<double>(0, 0),
2126  iter->second.infMatrix().at<double>(0, 1),
2127  iter->second.infMatrix().at<double>(1, 1));
2128  }
2129  else
2130  {
2131  // EDGE_SE3_TRACKXYZ observed_vertex_id observing_vertex_id param_offset x y z inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
2132  fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f\n",
2133  iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
2134  iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
2135  PARAM_OFFSET,
2136  iter->second.transform().x(),
2137  iter->second.transform().y(),
2138  iter->second.transform().z(),
2139  iter->second.infMatrix().at<double>(0, 0),
2140  iter->second.infMatrix().at<double>(0, 1),
2141  iter->second.infMatrix().at<double>(0, 2),
2142  iter->second.infMatrix().at<double>(1, 1),
2143  iter->second.infMatrix().at<double>(1, 2),
2144  iter->second.infMatrix().at<double>(2, 2));
2145  }
2146  continue;
2147  }
2148 
2149  std::string prefix = isSlam2d()? "EDGE_SE2" :"EDGE_SE3:QUAT";
2150  std::string suffix = "";
2151  std::string to = uFormat(" %d", iter->second.to());
2152 
2153  bool isSE2 = true;
2154  bool isSE3 = true;
2155 
2156  if (iter->second.type() == Link::kGravity)
2157  {
2158  continue;
2159  }
2160  else if (iter->second.type() == Link::kPosePrior)
2161  {
2162  if (this->priorsIgnored())
2163  {
2164  continue;
2165  }
2166  if (isSlam2d())
2167  {
2168  if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
2169  {
2170  prefix = "EDGE_PRIOR_SE2_XY";
2171  isSE2 = false;
2172  }
2173  else
2174  {
2175  prefix = "EDGE_PRIOR_SE2";
2176  }
2177 
2178  // based on https://github.com/RainerKuemmerle/g2o/blob/38347944c6ad7a3b31976b97406ff0de20be1530/g2o/types/slam2d/edge_se2_prior.cpp#L42
2179  // there is no pid for the 2d prior case
2180  to = "";
2181  }
2182  else
2183  {
2184  if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
2185  1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
2186  1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
2187  {
2188  to = "";
2189  prefix = "EDGE_POINTXYZ_PRIOR";
2190  isSE3 = false;
2191  }
2192  else
2193  {
2194  to = uFormat(" %d", PARAM_OFFSET);
2195  prefix = "EDGE_SE3_PRIOR";
2196  }
2197  }
2198  }
2199  else if(this->isRobust() &&
2200  iter->second.type() != Link::kNeighbor &&
2201  iter->second.type() != Link::kNeighborMerged)
2202  {
2203  fprintf(file, "VERTEX_SWITCH %d 1\n", virtualVertexId);
2204  fprintf(file, "EDGE_SWITCH_PRIOR %d 1 1.0\n", virtualVertexId);
2205  prefix = isSlam2d() ? "EDGE_SE2_SWITCHABLE" : "EDGE_SE3_SWITCHABLE";
2206  suffix = uFormat(" %d", virtualVertexId++);
2207  }
2208 
2209  if(isSlam2d())
2210  {
2211  if (isSE2)
2212  {
2213  // EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
2214  // EDGE_SE2_PRIOR observed_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
2215  fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
2216  prefix.c_str(),
2217  iter->second.from(),
2218  to.c_str(),
2219  suffix.c_str(),
2220  iter->second.transform().x(),
2221  iter->second.transform().y(),
2222  iter->second.transform().theta(),
2223  iter->second.infMatrix().at<double>(0, 0),
2224  iter->second.infMatrix().at<double>(0, 1),
2225  iter->second.infMatrix().at<double>(0, 5),
2226  iter->second.infMatrix().at<double>(1, 1),
2227  iter->second.infMatrix().at<double>(1, 5),
2228  iter->second.infMatrix().at<double>(5, 5));
2229  }
2230  else
2231  {
2232  // EDGE_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
2233  // EDGE_POINTXY_PRIOR x y inf_11 inf_12 inf_22
2234  fprintf(file, "%s %d%s%s %f %f %f %f %f\n",
2235  prefix.c_str(),
2236  iter->second.from(),
2237  to.c_str(),
2238  suffix.c_str(),
2239  iter->second.transform().x(),
2240  iter->second.transform().y(),
2241  iter->second.infMatrix().at<double>(0, 0),
2242  iter->second.infMatrix().at<double>(0, 1),
2243  iter->second.infMatrix().at<double>(1, 1));
2244  }
2245  }
2246  else
2247  {
2248  if (isSE3)
2249  {
2250  // 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
2251  // EDGE_SE3_PRIOR observed_vertex_id offset_parameter_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
2252  Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
2253  fprintf(file, "%s %d%s%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",
2254  prefix.c_str(),
2255  iter->second.from(),
2256  to.c_str(),
2257  suffix.c_str(),
2258  iter->second.transform().x(),
2259  iter->second.transform().y(),
2260  iter->second.transform().z(),
2261  q.x(),
2262  q.y(),
2263  q.z(),
2264  q.w(),
2265  iter->second.infMatrix().at<double>(0, 0),
2266  iter->second.infMatrix().at<double>(0, 1),
2267  iter->second.infMatrix().at<double>(0, 2),
2268  iter->second.infMatrix().at<double>(0, 3),
2269  iter->second.infMatrix().at<double>(0, 4),
2270  iter->second.infMatrix().at<double>(0, 5),
2271  iter->second.infMatrix().at<double>(1, 1),
2272  iter->second.infMatrix().at<double>(1, 2),
2273  iter->second.infMatrix().at<double>(1, 3),
2274  iter->second.infMatrix().at<double>(1, 4),
2275  iter->second.infMatrix().at<double>(1, 5),
2276  iter->second.infMatrix().at<double>(2, 2),
2277  iter->second.infMatrix().at<double>(2, 3),
2278  iter->second.infMatrix().at<double>(2, 4),
2279  iter->second.infMatrix().at<double>(2, 5),
2280  iter->second.infMatrix().at<double>(3, 3),
2281  iter->second.infMatrix().at<double>(3, 4),
2282  iter->second.infMatrix().at<double>(3, 5),
2283  iter->second.infMatrix().at<double>(4, 4),
2284  iter->second.infMatrix().at<double>(4, 5),
2285  iter->second.infMatrix().at<double>(5, 5));
2286  }
2287  else
2288  {
2289  // EDGE_XYZ observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_13 inf_22 .. inf_33
2290  // EDGE_POINTXYZ_PRIOR observed_vertex_id x y z inf_11 inf_12 .. inf_13 inf_22 .. inf_33
2291  fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
2292  prefix.c_str(),
2293  iter->second.from(),
2294  to.c_str(),
2295  suffix.c_str(),
2296  iter->second.transform().x(),
2297  iter->second.transform().y(),
2298  iter->second.transform().z(),
2299  iter->second.infMatrix().at<double>(0, 0),
2300  iter->second.infMatrix().at<double>(0, 1),
2301  iter->second.infMatrix().at<double>(0, 2),
2302  iter->second.infMatrix().at<double>(1, 1),
2303  iter->second.infMatrix().at<double>(1, 2),
2304  iter->second.infMatrix().at<double>(2, 2));
2305  }
2306  }
2307  }
2308  UINFO("Graph saved to %s", fileName.c_str());
2309 
2310  fclose(file);
2311  }
2312  else
2313  {
2314  UERROR("Cannot save to file %s", fileName.c_str());
2315  return false;
2316  }
2317  return true;
2318 }
2319 
2320 } /* namespace rtabmap */
d
d
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3194
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
cam
rtabmap::CameraThread * cam
Definition: tools/DataRecorder/main.cpp:58
UTimer
Definition: UTimer.h:46
g2o::EdgeSE3XYZPrior::setMeasurement
virtual void setMeasurement(const Vector3d &m)
Definition: edge_se3_xyzprior.h:47
rtabmap::OptimizerG2O::saveGraph
bool saveGraph(const std::string &fileName, const std::map< int, Transform > &poses, const std::multimap< int, Link > &edgeConstraints)
Definition: OptimizerG2O.cpp:2018
g2o::EdgeXYZPrior
prior for an XYZ vertex (VertexPointXYZ)
Definition: edge_xyz_prior.h:48
edge_switchPrior.h
util3d_motion_estimation.h
rtabmap::EdgeSE3Gravity
g2o edge with gravity constraint
Definition: edge_se3_gravity.h:44
rtabmap::Optimizer::isSlam2d
bool isSlam2d() const
Definition: Optimizer.h:92
glm::e
GLM_FUNC_DECL genType e()
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
std::swap
void swap(linb::any &lhs, linb::any &rhs) noexcept
rtabmap::Optimizer::epsilon
double epsilon() const
Definition: Optimizer.h:94
UMath.h
Basic mathematics functions.
edge_xy_prior.h
UConversion.h
Some conversion functions.
g2o::EdgeXYPrior::setMeasurement
virtual void setMeasurement(const Vector2d &m)
Definition: edge_xy_prior.h:56
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
edge_se2Switchable.h
rtabmap::OptimizerG2O::optimizeBA
virtual std::map< int, Transform > optimizeBA(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, std::vector< CameraModel > > &models, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, FeatureBA > > &wordReferences, std::set< int > *outliers=0)
Definition: OptimizerG2O.cpp:1399
prior
prior
rtabmap::Optimizer::gravitySigma
float gravitySigma() const
Definition: Optimizer.h:98
EdgeSE3Switchable::setMeasurement
virtual void setMeasurement(const Eigen::Isometry3d &m)
Definition: edge_se3Switchable.h:28
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
rtabmap::FeatureBA::kpt
cv::KeyPoint kpt
Definition: Optimizer.h:52
UASSERT
#define UASSERT(condition)
vertex_switchLinear.h
UStl.h
Wrappers of STL for convenient functions.
edge_se3_xyzprior.h
rtabmap::OptimizerG2O::optimize
virtual std::map< int, Transform > optimize(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &edgeConstraints, cv::Mat &outputCovariance, std::list< std::map< int, Transform > > *intermediateGraphes=0, double *finalError=0, int *iterationsDone=0)
Definition: OptimizerG2O.cpp:204
rtabmap::Optimizer::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Optimizer.cpp:329
rtabmap::Optimizer::landmarksIgnored
bool landmarksIgnored() const
Definition: Optimizer.h:97
edge_se3_gravity.h
edge_sbacam_gravity.h
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
g2o::EdgeXYZPrior::setMeasurement
virtual void setMeasurement(const Vector3d &m)
Definition: edge_xyz_prior.h:60
rtabmap::EdgeSE3Gravity::setMeasurement
virtual void setMeasurement(const Eigen::Matrix< double, 6, 1 > &m)
Definition: edge_se3_gravity.h:83
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:91
edge_se3Switchable.h
MULTICAM_OFFSET
#define MULTICAM_OFFSET
Definition: OptimizerG2O.cpp:113
util3d_transforms.h
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::Transform::fromEigen3d
static Transform fromEigen3d(const Eigen::Affine3d &matrix)
Definition: Transform.cpp:425
EdgeSE2Switchable::setMeasurement
virtual void setMeasurement(const g2o::SE2 &m)
Definition: edge_se2Switchable.h:29
UDEBUG
#define UDEBUG(...)
rtabmap::Optimizer::isRobust
bool isRobust() const
Definition: Optimizer.h:95
edge_xyz_prior.h
UERROR
#define UERROR(...)
rtabmap::Optimizer::setRobust
void setRobust(bool enabled)
Definition: Optimizer.h:105
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
uIsNan
bool uIsNan(const T &value)
Definition: UMath.h:42
g2o::EdgeSE3XYZPrior
Prior for a 3D pose with constraints only in xyz direction.
Definition: edge_se3_xyzprior.h:41
VertexSwitchLinear::setEstimate
virtual void setEstimate(const double &et)
Definition: vertex_switchLinear.cpp:44
rtabmap::Transform::theta
float theta() const
Definition: Transform.cpp:162
rtabmap::EdgeSBACamGravity::setMeasurement
virtual void setMeasurement(const Eigen::Matrix< double, 6, 1 > &m)
Definition: edge_sbacam_gravity.h:87
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:93
rtabmap::OptimizerG2O::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: OptimizerG2O.cpp:166
rtabmap::Optimizer::priorsIgnored
bool priorsIgnored() const
Definition: Optimizer.h:96
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::EdgeSBACamGravity::setCameraInvLocalTransform
void setCameraInvLocalTransform(const Eigen::Matrix3d &t)
Definition: edge_sbacam_gravity.h:51
q
q
rtabmap::Transform::toEigen3d
Eigen::Affine3d toEigen3d() const
Definition: Transform.cpp:386
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29