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) && RTABMAP_G2O_CPP11 == 1
157  g2o::Factory::instance()->registerType("CACHE_SE3_OFFSET", std::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 = std::make_unique<SlamLinearEigenSolver>();
254  linearSolver->setBlockOrdering(false);
255  blockSolver = std::make_unique<SlamBlockSolver>(std::move(linearSolver));
256  }
257 #ifdef G2O_HAVE_CHOLMOD
258  else if(solver_ == 2)
259  {
260  //chmold
261  auto linearSolver = std::make_unique<SlamLinearCholmodSolver>();
262  linearSolver->setBlockOrdering(false);
263  blockSolver = std::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 = std::make_unique<SlamLinearCSparseSolver>();
272  linearSolver->setBlockOrdering(false);
273  blockSolver = std::make_unique<SlamBlockSolver>(std::move(linearSolver));
274  }
275 #endif
276  else
277  {
278  //pcg
279  auto linearSolver = std::make_unique<SlamLinearPCGSolver>();
280  blockSolver = std::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 defined(RTABMAP_VERTIGO)
724  VertexSwitchLinear * v = 0;
725  if(this->isRobust() && isLandmarkWithRotation.at(idTag))
726  {
727  // For landmark links, add switchable edges
728  // Currently supporting only constraints with rotation
729 
730  // create new switch variable
731  // Sunderhauf IROS 2012:
732  // "Since it is reasonable to initially accept all loop closure constraints,
733  // a proper and convenient initial value for all switch variables would be
734  // sij = 1 when using the linear switch function"
735  v = new VertexSwitchLinear();
736  v->setEstimate(1.0);
737  v->setId(vertigoVertexId++);
738  UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str());
739 
740  // create switch prior factor
741  // "If the front-end is not able to assign sound individual values
742  // for Ξij , it is save to set all Ξij = 1, since this value is close
743  // to the individual optimal choice of Ξij for a large range of
744  // outliers."
745  EdgeSwitchPrior * prior = new EdgeSwitchPrior();
746  prior->setMeasurement(1.0);
747  prior->setVertex(0, v);
748  UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
749  }
750  else if(this->isRobust() && !isLandmarkWithRotation.at(idTag))
751  {
752  UWARN("%s cannot be used for landmark constraints without orientation.", Parameters::kOptimizerRobust().c_str());
753  }
754 #endif
755 
756  if(isSlam2d())
757  {
758  if(isLandmarkWithRotation.at(idTag))
759  {
760  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
761  if(!isCovarianceIgnored())
762  {
763  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
764  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
765  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
766  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
767  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
768  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
769  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
770  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
771  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
772  }
773 #if defined(RTABMAP_VERTIGO)
774  if(this->isRobust())
775  {
776  EdgeSE2Switchable * e = new EdgeSE2Switchable();
777  g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
778  g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
779  UASSERT(v1 != 0);
780  UASSERT(v2 != 0);
781  e->setVertex(0, v1);
782  e->setVertex(1, v2);
783  e->setVertex(2, v);
784  e->setMeasurement(g2o::SE2(t.x(), t.y(), t.theta()));
785  e->setInformation(information);
786  edge = e;
787  }
788  else
789 #endif
790  {
791  g2o::EdgeSE2 * e = new g2o::EdgeSE2();
792  g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
793  g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
794  UASSERT(v1 != 0);
795  UASSERT(v2 != 0);
796  e->setVertex(0, v1);
797  e->setVertex(1, v2);
798  e->setMeasurement(g2o::SE2(t.x(), t.y(), t.theta()));
799  e->setInformation(information);
800  edge = e;
801  }
802  }
803  else
804  {
805  Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
806  if(!isCovarianceIgnored())
807  {
808  cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,2), cv::Range(0,2)).clone();
809  memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
810  }
811  g2o::EdgeSE2PointXY* e = new g2o::EdgeSE2PointXY;
812  e->vertices()[0] = optimizer.vertex(id1);
813  e->vertices()[1] = optimizer.vertex(id2);
814  e->setMeasurement(Eigen::Vector2d(t.x(), t.y()));
815  e->setInformation(information);
816  e->setParameterId(0, PARAM_OFFSET);
817  edge = e;
818  }
819  }
820  else
821  {
822  if(isLandmarkWithRotation.at(idTag))
823  {
824  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
825  if(!isCovarianceIgnored())
826  {
827  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
828  }
829 
830  Eigen::Affine3d a = t.toEigen3d();
831  Eigen::Isometry3d constraint;
832  constraint = a.linear();
833  constraint.translation() = a.translation();
834 
835 #if defined(RTABMAP_VERTIGO)
836  if(this->isRobust())
837  {
838  EdgeSE3Switchable * e = new EdgeSE3Switchable();
839  g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
840  g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
841  UASSERT(v1 != 0);
842  UASSERT(v2 != 0);
843  e->setVertex(0, v1);
844  e->setVertex(1, v2);
845  e->setVertex(2, v);
846  e->setMeasurement(constraint);
847  e->setInformation(information);
848  edge = e;
849  }
850  else
851 #endif
852  {
853  g2o::EdgeSE3 * e = new g2o::EdgeSE3();
854  g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
855  g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
856  UASSERT(v1 != 0);
857  UASSERT(v2 != 0);
858  e->setVertex(0, v1);
859  e->setVertex(1, v2);
860  e->setMeasurement(constraint);
861  e->setInformation(information);
862  edge = e;
863  }
864  }
865  else
866  {
867  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
868  if(!isCovarianceIgnored())
869  {
870  cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();
871  memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
872  }
873 
874  g2o::EdgeSE3PointXYZ* e = new g2o::EdgeSE3PointXYZ;
875  e->vertices()[0] = optimizer.vertex(id1);
876  e->vertices()[1] = optimizer.vertex(id2);
877  e->setMeasurement(Eigen::Vector3d(t.x(), t.y(), t.z()));
878  e->setInformation(information);
879  e->setParameterId(0, PARAM_OFFSET);
880  edge = e;
881  }
882  }
883  }
884  }
885  else
886  {
887 #if defined(RTABMAP_VERTIGO)
888  VertexSwitchLinear * v = 0;
889  if(this->isRobust() &&
890  iter->second.type() != Link::kNeighbor &&
891  iter->second.type() != Link::kNeighborMerged)
892  {
893  // For loop closure links, add switchable edges
894 
895  // create new switch variable
896  // Sunderhauf IROS 2012:
897  // "Since it is reasonable to initially accept all loop closure constraints,
898  // a proper and convenient initial value for all switch variables would be
899  // sij = 1 when using the linear switch function"
900  v = new VertexSwitchLinear();
901  v->setEstimate(1.0);
902  v->setId(vertigoVertexId++);
903  UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str());
904 
905  // create switch prior factor
906  // "If the front-end is not able to assign sound individual values
907  // for Ξij , it is save to set all Ξij = 1, since this value is close
908  // to the individual optimal choice of Ξij for a large range of
909  // outliers."
910  EdgeSwitchPrior * prior = new EdgeSwitchPrior();
911  prior->setMeasurement(1.0);
912  prior->setVertex(0, v);
913  UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
914  }
915 #endif
916 
917  if(isSlam2d())
918  {
919  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
920  if(!isCovarianceIgnored())
921  {
922  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
923  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
924  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
925  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
926  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
927  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
928  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
929  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
930  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
931  }
932 
933 #if defined(RTABMAP_VERTIGO)
934  if(this->isRobust() &&
935  iter->second.type() != Link::kNeighbor &&
936  iter->second.type() != Link::kNeighborMerged)
937  {
938  EdgeSE2Switchable * e = new EdgeSE2Switchable();
939  g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
940  g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
941  UASSERT(v1 != 0);
942  UASSERT(v2 != 0);
943  e->setVertex(0, v1);
944  e->setVertex(1, v2);
945  e->setVertex(2, v);
946  e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
947  e->setInformation(information);
948  edge = e;
949  }
950  else
951 #endif
952  {
953  g2o::EdgeSE2 * e = new g2o::EdgeSE2();
954  g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
955  g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
956  UASSERT(v1 != 0);
957  UASSERT(v2 != 0);
958  e->setVertex(0, v1);
959  e->setVertex(1, v2);
960  e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
961  e->setInformation(information);
962  edge = e;
963  }
964  }
965  else
966  {
967  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
968  if(!isCovarianceIgnored())
969  {
970  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
971  }
972 
973  Eigen::Affine3d a = iter->second.transform().toEigen3d();
974  Eigen::Isometry3d constraint;
975  constraint = a.linear();
976  constraint.translation() = a.translation();
977 
978 #if defined(RTABMAP_VERTIGO)
979  if(this->isRobust() &&
980  iter->second.type() != Link::kNeighbor &&
981  iter->second.type() != Link::kNeighborMerged)
982  {
983  EdgeSE3Switchable * e = new EdgeSE3Switchable();
984  g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
985  g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
986  UASSERT(v1 != 0);
987  UASSERT(v2 != 0);
988  e->setVertex(0, v1);
989  e->setVertex(1, v2);
990  e->setVertex(2, v);
991  e->setMeasurement(constraint);
992  e->setInformation(information);
993  edge = e;
994  }
995  else
996 #endif
997  {
998  g2o::EdgeSE3 * e = new g2o::EdgeSE3();
999  g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
1000  g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
1001  UASSERT(v1 != 0);
1002  UASSERT(v2 != 0);
1003  e->setVertex(0, v1);
1004  e->setVertex(1, v2);
1005  e->setMeasurement(constraint);
1006  e->setInformation(information);
1007  edge = e;
1008  }
1009  }
1010  }
1011 
1012  if (edge && !optimizer.addEdge(edge))
1013  {
1014  delete edge;
1015  UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
1016  }
1017 
1018 
1019  }
1020 
1021  UDEBUG("Initial optimization...");
1022  optimizer.initializeOptimization();
1023 
1024  if(!optimizer.verifyInformationMatrices(true))
1025  {
1026  UERROR("This error can be caused by (1) bad covariance matrix "
1027  "set in odometry messages "
1028  "(see requirements in g2o::OptimizableGraph::verifyInformationMatrices() function) "
1029  "or that (2) PCL and g2o hadn't "
1030  "been built both with or without \"-march=native\" compilation "
1031  "flag (if one library is built with this flag and not the other, "
1032  "this is causing Eigen to not work properly, resulting in segmentation faults).");
1033  return optimizedPoses;
1034  }
1035 
1036  UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
1037  int it = 0;
1038  UTimer timer;
1039  double lastError = 0.0;
1040 
1041  if (!optimizer.solver()->init()) {
1042  UERROR("g2o: Error while initializing solver");
1043  return optimizedPoses;
1044  }
1045 
1046  if(intermediateGraphes || this->epsilon() > 0.0)
1047  {
1048  for(int i=0; i<iterations(); ++i)
1049  {
1050  if(intermediateGraphes)
1051  {
1052  if(i > 0)
1053  {
1054  std::map<int, Transform> tmpPoses;
1055  if(isSlam2d())
1056  {
1057  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
1058  {
1059  int id = iter->first;
1060  if(id > 0)
1061  {
1062  const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(id);
1063  if(v)
1064  {
1065  float roll, pitch, yaw;
1066  iter->second.getEulerAngles(roll, pitch, yaw);
1067  Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
1068  tmpPoses.insert(std::pair<int, Transform>(id, t));
1069  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1070  }
1071  else
1072  {
1073  UERROR("Vertex %d not found!?", id);
1074  }
1075  }
1076  else if(!landmarksIgnored())
1077  {
1078  const g2o::OptimizableGraph::Vertex* v = (const g2o::OptimizableGraph::Vertex*)optimizer.vertex(landmarkVertexOffset - id);
1079  if(v)
1080  {
1081  if(isLandmarkWithRotation.at(id))
1082  {
1083  const g2o::VertexSE2* vSE2 = (const g2o::VertexSE2*)v;
1084  float roll, pitch, yaw;
1085  iter->second.getEulerAngles(roll, pitch, yaw);
1086  Transform t(vSE2->estimate().translation()[0], vSE2->estimate().translation()[1], iter->second.z(), roll, pitch, vSE2->estimate().rotation().angle());
1087  tmpPoses.insert(std::pair<int, Transform>(id, t));
1088  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1089  }
1090  else
1091  {
1092  const g2o::VertexPointXY* vP = (const g2o::VertexPointXY*)v;
1093  float roll, pitch, yaw;
1094  iter->second.getEulerAngles(roll, pitch, yaw);
1095  Transform t(vP->estimate()[0], vP->estimate()[1], iter->second.z(), roll, pitch, yaw);
1096  tmpPoses.insert(std::pair<int, Transform>(id, t));
1097  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1098  }
1099  }
1100  else
1101  {
1102  UERROR("Vertex %d not found!?", id);
1103  }
1104  }
1105  }
1106  }
1107  else
1108  {
1109  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
1110  {
1111  int id = iter->first;
1112  if(id > 0)
1113  {
1114  const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(id);
1115  if(v)
1116  {
1117  Transform t = Transform::fromEigen3d(v->estimate());
1118  tmpPoses.insert(std::pair<int, Transform>(id, t));
1119  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1120  }
1121  else
1122  {
1123  UERROR("Vertex %d not found!?", id);
1124  }
1125  }
1126  else if(!landmarksIgnored())
1127  {
1128  const g2o::OptimizableGraph::Vertex* v = (const g2o::OptimizableGraph::Vertex*)optimizer.vertex(landmarkVertexOffset - id);
1129  if(v)
1130  {
1131  if(isLandmarkWithRotation.at(id))
1132  {
1133  const g2o::VertexSE3* vSE3 = (const g2o::VertexSE3*)v;
1134  Transform t = Transform::fromEigen3d(vSE3->estimate());
1135  tmpPoses.insert(std::pair<int, Transform>(id, t));
1136  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1137  }
1138  else
1139  {
1140  const g2o::VertexPointXYZ* vP = (const g2o::VertexPointXYZ*)v;
1141  float roll, pitch, yaw;
1142  iter->second.getEulerAngles(roll, pitch, yaw);
1143  Transform t(vP->estimate()[0], vP->estimate()[1], vP->estimate()[2], roll, pitch, yaw);
1144  tmpPoses.insert(std::pair<int, Transform>(id, t));
1145  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1146  }
1147  }
1148  else
1149  {
1150  UERROR("Vertex %d not found!?", id);
1151  }
1152  }
1153  }
1154  }
1155  intermediateGraphes->push_back(tmpPoses);
1156  }
1157  }
1158 
1159  g2o::OptimizationAlgorithm::SolverResult result = optimizer.solver()->solve(i);
1160  ++it;
1161 
1162  // early stop condition
1163  optimizer.computeActiveErrors();
1164  double chi2 = optimizer.activeRobustChi2();
1165  UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
1166 
1167  if(i>0 && optimizer.activeRobustChi2() > 1000000000000.0)
1168  {
1169  UERROR("g2o: Large optimimzation error detected (%f), aborting optimization!");
1170  return optimizedPoses;
1171  }
1172 
1173  if(result == g2o::OptimizationAlgorithm::Fail)
1174  {
1175  UERROR("g2o: Solver failed, aborting optimization!");
1176  return optimizedPoses;
1177  }
1178 
1179  double errorDelta = lastError - chi2;
1180  if(i>0 && errorDelta < this->epsilon())
1181  {
1182  if(errorDelta < 0)
1183  {
1184  UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
1185  }
1186  else
1187  {
1188  UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
1189  break;
1190  }
1191  }
1192  else if(i==0 && chi2 < this->epsilon())
1193  {
1194  UINFO("Stop optimizing, error is already under epsilon (%f < %f)", chi2, this->epsilon());
1195  break;
1196  }
1197  lastError = chi2;
1198  }
1199  }
1200  else
1201  {
1202  it = optimizer.optimize(iterations());
1203  optimizer.computeActiveErrors();
1204  UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
1205  }
1206  if(finalError)
1207  {
1208  *finalError = lastError;
1209  }
1210  if(iterationsDone)
1211  {
1212  *iterationsDone = it;
1213  }
1214  UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
1215 
1216  if(optimizer.activeRobustChi2() > 1000000000000.0)
1217  {
1218  UERROR("g2o: Large optimimzation error detected (%f), aborting optimization!");
1219  return optimizedPoses;
1220  }
1221 
1222  if(isSlam2d())
1223  {
1224  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
1225  {
1226  int id = iter->first;
1227  if(id > 0)
1228  {
1229  const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(id);
1230  if(v)
1231  {
1232  float roll, pitch, yaw;
1233  iter->second.getEulerAngles(roll, pitch, yaw);
1234  Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
1235  optimizedPoses.insert(std::pair<int, Transform>(id, t));
1236  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1237  }
1238  else
1239  {
1240  UERROR("Vertex %d not found!?", id);
1241  }
1242  }
1243  else if(!landmarksIgnored())
1244  {
1245  const g2o::OptimizableGraph::Vertex* v = (const g2o::OptimizableGraph::Vertex*)optimizer.vertex(landmarkVertexOffset - id);
1246 
1247  if(v)
1248  {
1249  if(isLandmarkWithRotation.at(id))
1250  {
1251  const g2o::VertexSE2* vSE2 = (const g2o::VertexSE2*)v;
1252  float roll, pitch, yaw;
1253  iter->second.getEulerAngles(roll, pitch, yaw);
1254  Transform t(vSE2->estimate().translation()[0], vSE2->estimate().translation()[1], iter->second.z(), roll, pitch, vSE2->estimate().rotation().angle());
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::VertexPointXY* vP = (const g2o::VertexPointXY*)v;
1261  float roll, pitch, yaw;
1262  iter->second.getEulerAngles(roll, pitch, yaw);
1263  Transform t(vP->estimate()[0], vP->estimate()[1], iter->second.z(), 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::VertexSE2* v = (g2o::VertexSE2*)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() == 3 && block->cols() == 3);
1286  outputCovariance.at<double>(0,0) = (*block)(0,0); // x-x
1287  outputCovariance.at<double>(0,1) = (*block)(0,1); // x-y
1288  outputCovariance.at<double>(0,5) = (*block)(0,2); // x-theta
1289  outputCovariance.at<double>(1,0) = (*block)(1,0); // y-x
1290  outputCovariance.at<double>(1,1) = (*block)(1,1); // y-y
1291  outputCovariance.at<double>(1,5) = (*block)(1,2); // y-theta
1292  outputCovariance.at<double>(5,0) = (*block)(2,0); // theta-x
1293  outputCovariance.at<double>(5,1) = (*block)(2,1); // theta-y
1294  outputCovariance.at<double>(5,5) = (*block)(2,2); // theta-theta
1295  }
1296  else if(v->hessianIndex() < 0)
1297  {
1298  UWARN("Computing marginals: vertex %d has negative hessian index (%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex());
1299  }
1300  else
1301  {
1302  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());
1303  }
1304  }
1305  else
1306  {
1307  UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
1308  }
1309  }
1310  else
1311  {
1312  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
1313  {
1314  int id = iter->first;
1315  if(id > 0)
1316  {
1317  const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(id);
1318  if(v)
1319  {
1320  Transform t = Transform::fromEigen3d(v->estimate());
1321  optimizedPoses.insert(std::pair<int, Transform>(id, t));
1322  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1323  }
1324  else
1325  {
1326  UERROR("Vertex %d not found!?", id);
1327  }
1328  }
1329  else if(!landmarksIgnored())
1330  {
1331  const g2o::OptimizableGraph::Vertex* v = (const g2o::OptimizableGraph::Vertex*)optimizer.vertex(landmarkVertexOffset - id);
1332 
1333  if(v)
1334  {
1335  if(isLandmarkWithRotation.at(id))
1336  {
1337  const g2o::VertexSE3* vSE3 = (const g2o::VertexSE3*)v;
1338  Transform t = Transform::fromEigen3d(vSE3->estimate());
1339  optimizedPoses.insert(std::pair<int, Transform>(id, t));
1340  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1341  }
1342  else
1343  {
1344  const g2o::VertexPointXYZ* vP = (const g2o::VertexPointXYZ*)v;
1345  float roll, pitch, yaw;
1346  iter->second.getEulerAngles(roll, pitch, yaw);
1347  Transform t(vP->estimate()[0], vP->estimate()[1], vP->estimate()[2], roll, pitch, yaw);
1348  optimizedPoses.insert(std::pair<int, Transform>(id, t));
1349  UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
1350  }
1351  }
1352  else
1353  {
1354  UERROR("Vertex %d not found!?", id);
1355  }
1356  }
1357  }
1358 
1359  g2o::VertexSE3* v = (g2o::VertexSE3*)optimizer.vertex(poses.rbegin()->first);
1360  if(v)
1361  {
1362  UTimer t;
1363  g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
1364  optimizer.computeMarginals(spinv, v);
1365  UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
1366  if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
1367  {
1368  g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
1369  UASSERT(block && block->cols() == 6 && block->cols() == 6);
1370  memcpy(outputCovariance.data, block->data(), outputCovariance.total()*sizeof(double));
1371  }
1372  else if(v->hessianIndex() < 0)
1373  {
1374  UWARN("Computing marginals: vertex %d has negative hessian index (%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex());
1375  }
1376 #ifdef RTABMAP_G2O_CPP11
1377  else
1378  {
1379  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());
1380  }
1381 #endif
1382  }
1383  else
1384  {
1385  UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
1386  }
1387  }
1388  }
1389  else if(poses.size() == 1 || iterations() <= 0)
1390  {
1391  optimizedPoses = poses;
1392  }
1393  else
1394  {
1395  UWARN("This method should be called at least with 1 pose!");
1396  }
1397  UDEBUG("Optimizing graph...end!");
1398 #else
1399 #ifdef RTABMAP_ORB_SLAM
1400  UERROR("G2O graph optimization cannot be used with g2o built from ORB_SLAM, only SBA is available.");
1401 #else
1402  UERROR("Not built with G2O support!");
1403 #endif
1404 #endif
1405  return optimizedPoses;
1406 }
1407 
1408 #ifdef RTABMAP_ORB_SLAM
1409 
1412  class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>
1413 {
1414  public:
1415  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
1416  EdgeSE3Expmap(): BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){}
1417  bool read(std::istream& is)
1418  {
1419  return false;
1420  }
1421 
1422  bool write(std::ostream& os) const
1423  {
1424  return false;
1425  }
1426 
1427  void computeError()
1428  {
1429  const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
1430  const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
1431  g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
1432  _error[0]=delta.translation().x();
1433  _error[1]=delta.translation().y();
1434  _error[2]=delta.translation().z();
1435  _error[3]=delta.rotation().x();
1436  _error[4]=delta.rotation().y();
1437  _error[5]=delta.rotation().z();
1438  }
1439 
1440  virtual void setMeasurement(const g2o::SE3Quat& meas){
1441  _measurement=meas;
1442  _inverseMeasurement=meas.inverse();
1443  }
1444 
1445  virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;}
1446  virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){
1447  g2o::VertexSE3Expmap* from = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
1448  g2o::VertexSE3Expmap* to = static_cast<g2o::VertexSE3Expmap*>(_vertices[1]);
1449  if (from_.count(from) > 0)
1450  to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement);
1451  else
1452  from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement);
1453  }
1454 
1455  virtual bool setMeasurementData(const double* d){
1456  Eigen::Map<const g2o::Vector7d> v(d);
1457  _measurement.fromVector(v);
1458  _inverseMeasurement = _measurement.inverse();
1459  return true;
1460  }
1461 
1462  virtual bool getMeasurementData(double* d) const{
1463  Eigen::Map<g2o::Vector7d> v(d);
1464  v = _measurement.toVector();
1465  return true;
1466  }
1467 
1468  virtual int measurementDimension() const {return 7;}
1469 
1470  virtual bool setMeasurementFromState() {
1471  const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
1472  const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
1473  _measurement = (v1->estimate().inverse()*v2->estimate());
1474  _inverseMeasurement = _measurement.inverse();
1475  return true;
1476  }
1477 
1478  protected:
1479  g2o::SE3Quat _inverseMeasurement;
1480 };
1481 #endif
1482 
1483 std::map<int, Transform> OptimizerG2O::optimizeBA(
1484  int rootId,
1485  const std::map<int, Transform> & poses,
1486  const std::multimap<int, Link> & links,
1487  const std::map<int, std::vector<CameraModel> > & models,
1488  std::map<int, cv::Point3f> & points3DMap,
1489  const std::map<int, std::map<int, FeatureBA> > & wordReferences,
1490  std::set<int> * outliers)
1491 {
1492  std::map<int, Transform> optimizedPoses;
1493 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
1494  UDEBUG("Optimizing graph...");
1495 
1496  optimizedPoses.clear();
1497  if(poses.size()>=2 && iterations() > 0 && (models.size() == poses.size() || poses.begin()->first < 0))
1498  {
1499  g2o::SparseOptimizer optimizer;
1500  //optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
1501 #if defined(RTABMAP_G2O_CPP11) && !defined(RTABMAP_ORB_SLAM)
1502  std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
1503 #else
1504  g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
1505 #endif
1506 
1507 #ifdef RTABMAP_ORB_SLAM
1508  linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
1509 #else
1510  if(solver_ == 3)
1511  {
1512  //eigen
1513 #ifdef RTABMAP_G2O_CPP11
1514  linearSolver = std::make_unique<g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType> >();
1515 #else
1516  linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
1517 #endif
1518  }
1519 #ifdef G2O_HAVE_CHOLMOD
1520  else if(solver_ == 2)
1521  {
1522  //chmold
1523 #ifdef RTABMAP_G2O_CPP11
1524  linearSolver = std::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> >();
1525 #else
1526  linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
1527 #endif
1528  }
1529 #endif
1530 #ifdef G2O_HAVE_CSPARSE
1531  else if(solver_ == 0)
1532  {
1533  //csparse
1534 #ifdef RTABMAP_G2O_CPP11
1535  linearSolver = std::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType> >();
1536 #else
1537  linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
1538 #endif
1539  }
1540 #endif
1541  else
1542  {
1543  //pcg
1544 #ifdef RTABMAP_G2O_CPP11
1545  linearSolver = std::make_unique<g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType> >();
1546 #else
1547  linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
1548 #endif
1549  }
1550 #endif // RTABMAP_ORB_SLAM
1551 
1552 #ifndef RTABMAP_ORB_SLAM
1553  if(optimizer_ == 1)
1554  {
1555 #ifdef RTABMAP_G2O_CPP11
1556  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(
1557  std::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
1558 #else
1559  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(new g2o::BlockSolver_6_3(linearSolver)));
1560 #endif
1561  }
1562  else
1563 #endif
1564  {
1565 #if defined(RTABMAP_G2O_CPP11) && !defined(RTABMAP_ORB_SLAM)
1566  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
1567  std::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
1568 #else
1569  optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(new g2o::BlockSolver_6_3(linearSolver)));
1570 #endif
1571  }
1572 
1573 
1574  UDEBUG("fill poses to g2o...");
1575  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
1576  {
1577  if(iter->first > 0)
1578  {
1579  // Get camera model
1580  std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
1581  UASSERT(iterModel != models.end() && !iterModel->second.empty());
1582  for(size_t i=0; i<iterModel->second.size(); ++i)
1583  {
1584  UASSERT(iterModel->second[i].isValidForProjection());
1585 
1586  Transform camPose = iter->second * iterModel->second[i].localTransform();
1587 
1588  // Add node's pose
1589  UASSERT(!camPose.isNull());
1590 #ifdef RTABMAP_ORB_SLAM
1591  g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
1592 #else
1593  g2o::VertexCam * vCam = new g2o::VertexCam();
1594 #endif
1595 
1596  Eigen::Affine3d a = camPose.toEigen3d();
1597 #ifdef RTABMAP_ORB_SLAM
1598  a = a.inverse();
1599  vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
1600 #else
1601  g2o::SBACam cam(Eigen::Quaterniond(a.linear()), a.translation());
1602  cam.setKcam(
1603  iterModel->second[i].fx(),
1604  iterModel->second[i].fy(),
1605  iterModel->second[i].cx(),
1606  iterModel->second[i].cy(),
1607  iterModel->second[i].Tx()<0.0?-iterModel->second[i].Tx()/iterModel->second[i].fx():baseline_); // baseline in meters
1608  vCam->setEstimate(cam);
1609 #endif
1610  vCam->setId(iter->first*MULTICAM_OFFSET + i);
1611 
1612  // negative root means that all other poses should be fixed instead of the root
1613  vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
1614 
1615  /*UDEBUG("camPose %d (camid=%d) (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
1616  iter->first,
1617  vCam->id(),
1618  vCam->fixed()?1:0,
1619  iterModel->second[i].fx(),
1620  iterModel->second[i].fy(),
1621  iterModel->second[i].cx(),
1622  iterModel->second[i].cy(),
1623  iterModel->second[i].Tx(),
1624  iterModel->second[i].Tx()<0.0?-iterModel->second[i].Tx()/iterModel->second[i].fx():baseline_,
1625  camPose.prettyPrint().c_str());*/
1626 
1627  UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert cam vertex %d (pose=%d)!?", vCam->id(), iter->first).c_str());
1628  }
1629  }
1630  }
1631 
1632  UDEBUG("fill edges to g2o...");
1633  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
1634  {
1635  if(iter->second.from() > 0 &&
1636  iter->second.to() > 0 &&
1637  uContains(poses, iter->second.from()) &&
1638  uContains(poses, iter->second.to()))
1639  {
1640  // add edge
1641  int id1 = iter->second.from();
1642  int id2 = iter->second.to();
1643 
1644  if(id1 == id2)
1645  {
1646 #ifndef RTABMAP_ORB_SLAM
1647  g2o::HyperGraph::Edge * edge = 0;
1648  if(gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
1649  {
1650  Eigen::Matrix<double, 6, 1> m;
1651  // Up vector in robot frame
1652  m.head<3>() = Eigen::Vector3d::UnitZ();
1653  // Observed Gravity vector in world frame
1654  float roll, pitch, yaw;
1655  iter->second.transform().getEulerAngles(roll, pitch, yaw);
1656  m.tail<3>() = Transform(0,0,0,roll,pitch,0).toEigen3d() * -Eigen::Vector3d::UnitZ();
1657 
1658  Eigen::MatrixXd information = Eigen::MatrixXd::Identity(3, 3) * 1.0/(gravitySigma()*gravitySigma());
1659 
1660  g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
1661  EdgeSBACamGravity* priorEdge(new EdgeSBACamGravity());
1662  std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
1663  // Gravity constraint added only to first camera of a pose
1664  UASSERT(iterModel != models.end() && !iterModel->second.empty() && !iterModel->second[0].localTransform().isNull());
1665  priorEdge->setCameraInvLocalTransform(iterModel->second[0].localTransform().inverse().toEigen3d().linear());
1666  priorEdge->setMeasurement(m);
1667  priorEdge->setInformation(information);
1668  priorEdge->vertices()[0] = v1;
1669  edge = priorEdge;
1670  }
1671  if (edge && !optimizer.addEdge(edge))
1672  {
1673  delete edge;
1674  UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
1675  return optimizedPoses;
1676  }
1677 #endif
1678  }
1679  else if(id1>0 && id2>0) // not supporting landmarks
1680  {
1681  UASSERT(!iter->second.transform().isNull());
1682 
1683  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
1684  if(!isCovarianceIgnored())
1685  {
1686  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
1687  }
1688 
1689  // between cameras, not base_link
1690  Transform camLink = models.at(id1)[0].localTransform().inverse()*iter->second.transform()*models.at(id2)[0].localTransform();
1691  /*UDEBUG("added edge %d->%d (camIDs %d->%d) (in cam frame=%s)",
1692  id1,
1693  id2,
1694  id1*MULTICAM_OFFSET,
1695  id2*MULTICAM_OFFSET,
1696  camLink.prettyPrint().c_str());*/
1697 #ifdef RTABMAP_ORB_SLAM
1698  EdgeSE3Expmap * e = new EdgeSE3Expmap();
1699  g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id1*MULTICAM_OFFSET);
1700  g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id2*MULTICAM_OFFSET);
1701 
1702  Transform camPose1 = Transform::fromEigen3d(v1->estimate()).inverse();
1703  Transform camPose2Inv = Transform::fromEigen3d(v2->estimate());
1704 
1705  camLink = camPose1 * camPose1 * camLink * camPose2Inv * camPose2Inv;
1706 #else
1707  g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
1708  g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
1709  g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2*MULTICAM_OFFSET);
1710 #endif
1711  UASSERT(v1 != 0);
1712  UASSERT(v2 != 0);
1713  e->setVertex(0, v1);
1714  e->setVertex(1, v2);
1715  Eigen::Affine3d a = camLink.toEigen3d();
1716  e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
1717  e->setInformation(information);
1718 
1719  if (!optimizer.addEdge(e))
1720  {
1721  delete e;
1722  UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
1723  return optimizedPoses;
1724  }
1725  }
1726  }
1727  }
1728 
1729  UDEBUG("fill hard edges between camera 0 and other cameras (multicam)...");
1730  for(std::map<int, std::vector<CameraModel> >::const_iterator iter=models.begin(); iter!=models.end(); ++iter)
1731  {
1732  int id = iter->first;
1733  if(uContains(poses, id))
1734  {
1735  for(size_t i=1; i<iter->second.size(); ++i)
1736  {
1737  // add edge
1738  // Set large information matrix to keep these links fixed
1739  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*9999999;
1740 
1741  // between cameras, not base_link
1742  Transform camLink = iter->second[0].localTransform().inverse()*iter->second[i].localTransform();
1743 #ifdef RTABMAP_ORB_SLAM
1744  EdgeSE3Expmap * e = new EdgeSE3Expmap();
1745  g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id*MULTICAM_OFFSET);
1746  g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id*MULTICAM_OFFSET+i);
1747 
1748  Transform camPose1 = Transform::fromEigen3d(v1->estimate()).inverse();
1749  Transform camPose2Inv = Transform::fromEigen3d(v2->estimate());
1750 
1751  camLink = camPose1 * camPose1 * camLink * camPose2Inv * camPose2Inv;
1752 #else
1753  g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
1754  g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id*MULTICAM_OFFSET);
1755  g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id*MULTICAM_OFFSET+i);
1756 #endif
1757  /*UDEBUG("added edge between subcam 0->%d id:%d->%d (in cam frame=%s)",
1758  i,
1759  v1->id(),
1760  v2->id(),
1761  camLink.prettyPrint().c_str());*/
1762 
1763  UASSERT(v1 != 0);
1764  UASSERT(v2 != 0);
1765  e->setVertex(0, v1);
1766  e->setVertex(1, v2);
1767  Eigen::Affine3d a = camLink.toEigen3d();
1768  e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
1769  e->setInformation(information);
1770 
1771  if (!optimizer.addEdge(e))
1772  {
1773  delete e;
1774  UERROR("Map: Failed adding constraint between %d and %d, skipping", v1->id(), v2->id());
1775  return optimizedPoses;
1776  }
1777  }
1778  }
1779  }
1780 
1781  UDEBUG("fill 3D points to g2o...");
1782  const int stepVertexId = poses.rbegin()->first*MULTICAM_OFFSET+MULTICAM_OFFSET;
1783  int negVertexOffset = stepVertexId;
1784  if(wordReferences.size() && wordReferences.rbegin()->first>0)
1785  {
1786  negVertexOffset += wordReferences.rbegin()->first;
1787  }
1788  UDEBUG("stepVertexId=%d, negVertexOffset=%d", stepVertexId, negVertexOffset);
1789  std::list<g2o::OptimizableGraph::Edge*> edges;
1790  for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
1791  {
1792  int id = iter->first;
1793  if(points3DMap.find(id) != points3DMap.end())
1794  {
1795  cv::Point3f pt3d = points3DMap.at(id);
1796  if(!util3d::isFinite(pt3d))
1797  {
1798  UWARN("Ignoring 3D point %d because it has nan value(s)!", id);
1799  continue;
1800  }
1801  g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
1802 
1803  vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
1804  if(id<0)
1805  {
1806  vpt3d->setId(negVertexOffset + id*-1);
1807  }
1808  else
1809  {
1810  vpt3d->setId(stepVertexId + id);
1811  }
1812  UASSERT(vpt3d->id() > 0);
1813  vpt3d->setMarginalized(true);
1814  optimizer.addVertex(vpt3d);
1815 
1816  //UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
1817 
1818  // set observations
1819  for(std::map<int, FeatureBA>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
1820  {
1821  int poseId = jter->first;
1822  int camIndex = jter->second.cameraIndex;
1823  int camId = poseId*MULTICAM_OFFSET+camIndex;
1824  if(poses.find(poseId) != poses.end() && optimizer.vertex(camId) != 0)
1825  {
1826  const FeatureBA & pt = jter->second;
1827  double depth = pt.depth;
1828 
1829  //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);
1830 
1831  g2o::OptimizableGraph::Edge * e;
1832  double baseline = 0.0;
1833 #ifdef RTABMAP_ORB_SLAM
1834  g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
1835  std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(poseId);
1836 
1837  UASSERT(iterModel != models.end() && camIndex<iterModel->second.size() && iterModel->second[camIndex].isValidForProjection());
1838  baseline = iterModel->second[camIndex].Tx()<0.0?-iterModel->second[camIndex].Tx()/iterModel->second[camIndex].fx():baseline_;
1839 #else
1840  g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
1841  baseline = vcam->estimate().baseline;
1842 #endif
1843  double variance = pixelVariance_;
1844  if(uIsFinite(depth) && depth > 0.0 && baseline > 0.0)
1845  {
1846  // stereo edge
1847 #ifdef RTABMAP_ORB_SLAM
1848  g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
1849  float disparity = baseline * iterModel->second[camIndex].fx() / depth;
1850  Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
1851  es->setMeasurement(obs);
1852  //variance *= log(exp(1)+disparity);
1853  es->setInformation(Eigen::Matrix3d::Identity() / variance);
1854  es->fx = iterModel->second[camIndex].fx();
1855  es->fy = iterModel->second[camIndex].fy();
1856  es->cx = iterModel->second[camIndex].cx();
1857  es->cy = iterModel->second[camIndex].cy();
1858  es->bf = baseline*es->fx;
1859  e = es;
1860 #else
1861  g2o::EdgeProjectP2SC* es = new g2o::EdgeProjectP2SC();
1862  float disparity = baseline * vcam->estimate().Kcam(0,0) / depth;
1863  Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
1864  es->setMeasurement(obs);
1865  //variance *= log(exp(1)+disparity);
1866  es->setInformation(Eigen::Matrix3d::Identity() / variance);
1867  e = es;
1868 #endif
1869  }
1870  else
1871  {
1872  if(baseline > 0.0)
1873  {
1874  UDEBUG("Stereo camera model detected but current "
1875  "observation (pt=%d to cam=%d, kpt=[%d,%d]) has null depth (%f m), adding "
1876  "mono observation instead.",
1877  vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth);
1878  }
1879  // mono edge
1880 #ifdef RTABMAP_ORB_SLAM
1881  g2o::EdgeSE3ProjectXYZ* em = new g2o::EdgeSE3ProjectXYZ();
1882  Eigen::Vector2d obs( pt.kpt.pt.x, pt.kpt.pt.y);
1883  em->setMeasurement(obs);
1884  em->setInformation(Eigen::Matrix2d::Identity() / variance);
1885  em->fx = iterModel->second[camIndex].fx();
1886  em->fy = iterModel->second[camIndex].fy();
1887  em->cx = iterModel->second[camIndex].cx();
1888  em->cy = iterModel->second[camIndex].cy();
1889  e = em;
1890 
1891 #else
1892  g2o::EdgeProjectP2MC* em = new g2o::EdgeProjectP2MC();
1893  Eigen::Vector2d obs( pt.kpt.pt.x, pt.kpt.pt.y);
1894  em->setMeasurement(obs);
1895  em->setInformation(Eigen::Matrix2d::Identity() / variance);
1896  e = em;
1897 #endif
1898  }
1899  e->setVertex(0, vpt3d);
1900  e->setVertex(1, vcam);
1901 
1902  if(robustKernelDelta_ > 0.0)
1903  {
1904  g2o::RobustKernelHuber* kernel = new g2o::RobustKernelHuber;
1905  kernel->setDelta(robustKernelDelta_);
1906  e->setRobustKernel(kernel);
1907  }
1908 
1909  optimizer.addEdge(e);
1910  edges.push_back(e);
1911  }
1912  }
1913  }
1914  }
1915 
1916  UDEBUG("Initial optimization...");
1917  optimizer.initializeOptimization();
1918 
1919  UASSERT(optimizer.verifyInformationMatrices());
1920 
1921  UDEBUG("g2o optimizing begin (max iterations=%d, robustKernel=%f)", iterations(), robustKernelDelta_);
1922 
1923  int it = 0;
1924  UTimer timer;
1925  int outliersCount = 0;
1926  int outliersCountFar = 0;
1927 
1928  for(int i=0; i<(robustKernelDelta_>0.0?2:1); ++i)
1929  {
1930  it += optimizer.optimize(i==0&&robustKernelDelta_>0.0?5:iterations());
1931 
1932  // early stop condition
1933  optimizer.computeActiveErrors();
1934  double chi2 = optimizer.activeRobustChi2();
1935 
1936  if(uIsNan(chi2))
1937  {
1938  UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
1939  return optimizedPoses;
1940  }
1941  UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
1942 
1943  if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
1944  {
1945  UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
1946  return optimizedPoses;
1947  }
1948 
1949  if(robustKernelDelta_>0.0)
1950  {
1951  for(std::list<g2o::OptimizableGraph::Edge*>::iterator iter=edges.begin(); iter!=edges.end();++iter)
1952  {
1953  if((*iter)->level() == 0 && (*iter)->chi2() > (*iter)->robustKernel()->delta())
1954  {
1955  (*iter)->setLevel(1);
1956  ++outliersCount;
1957  double d = 0.0;
1958 #ifdef RTABMAP_ORB_SLAM
1959  if(dynamic_cast<g2o::EdgeStereoSE3ProjectXYZ*>(*iter) != 0)
1960  {
1961  d = ((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[0]-((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[2];
1962  }
1963  //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());
1964 #else
1965  if(dynamic_cast<g2o::EdgeProjectP2SC*>(*iter) != 0)
1966  {
1967  d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
1968  }
1969  //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());
1970 #endif
1971 
1972  cv::Point3f pt3d;
1973  if((*iter)->vertex(0)->id() > negVertexOffset)
1974  {
1975  pt3d = points3DMap.at(negVertexOffset - (*iter)->vertex(0)->id());
1976  }
1977  else
1978  {
1979  pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
1980  }
1981  ((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
1982 
1983  if(outliers)
1984  {
1985  outliers->insert((*iter)->vertex(0)->id()-stepVertexId);
1986  }
1987  if(d < 5.0)
1988  {
1989  outliersCountFar++;
1990  }
1991  }
1992  //(*iter)->setRobustKernel(0);
1993  }
1994  if(i==0)
1995  optimizer.initializeOptimization(0);
1996  UDEBUG("outliers=%d outliersCountFar=%d", outliersCount, outliersCountFar);
1997  }
1998  }
1999  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());
2000 
2001  if(optimizer.activeRobustChi2() > 1000000000000.0)
2002  {
2003  UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
2004  return optimizedPoses;
2005  }
2006 
2007  // update poses
2008  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2009  {
2010  if(iter->first > 0)
2011  {
2012  int camId = iter->first*MULTICAM_OFFSET;
2013 #ifdef RTABMAP_ORB_SLAM
2014  const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(camId);
2015 #else
2016  const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(camId);
2017 #endif
2018  if(v)
2019  {
2020  Transform t = Transform::fromEigen3d(v->estimate());
2021 
2022 #ifdef RTABMAP_ORB_SLAM
2023  t=t.inverse();
2024 #endif
2025 
2026  // remove model local transform
2027  t *= models.at(iter->first)[0].localTransform().inverse();
2028 
2029  //UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
2030  if(t.isNull())
2031  {
2032  UERROR("Optimized pose %d is null!?!?", iter->first);
2033  optimizedPoses.clear();
2034  return optimizedPoses;
2035  }
2036 
2037  // FIXME: is there a way that we can add the 2D constraint directly in SBA?
2038  if(this->isSlam2d())
2039  {
2040  // get transform between old and new pose
2041  t = iter->second.inverse() * t;
2042  optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
2043  }
2044  else
2045  {
2046  optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
2047  }
2048  }
2049  else
2050  {
2051  UERROR("Vertex (pose) %d (cam=%d) not found!?", iter->first, camId);
2052  }
2053  }
2054  }
2055 
2056  //update points3D
2057 
2058  for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter!=points3DMap.end(); ++iter)
2059  {
2060  const g2o::VertexSBAPointXYZ* v;
2061  int id = iter->first;
2062  if(id<0)
2063  {
2064  v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(negVertexOffset + id*-1);
2065  }
2066  else
2067  {
2068  v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + id);
2069  }
2070 
2071  if(v)
2072  {
2073  cv::Point3f p(v->estimate()[0], v->estimate()[1], v->estimate()[2]);
2074  //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);
2075  iter->second = p;
2076  }
2077  else
2078  {
2079  iter->second.x = iter->second.y = iter->second.z = std::numeric_limits<float>::quiet_NaN();
2080  }
2081  }
2082  }
2083  else if(poses.size() > 1 && (poses.size() != models.size() && poses.begin()->first > 0))
2084  {
2085  UERROR("This method should be called with size of poses = size camera models!");
2086  }
2087  else if(poses.size() == 1 || iterations() <= 0)
2088  {
2089  optimizedPoses = poses;
2090  }
2091  else
2092  {
2093  UWARN("This method should be called at least with 1 pose!");
2094  }
2095  UDEBUG("Optimizing graph...end!");
2096 #else
2097  UERROR("Not built with G2O support!");
2098 #endif
2099  return optimizedPoses;
2100 }
2101 
2102 bool OptimizerG2O::saveGraph(
2103  const std::string & fileName,
2104  const std::map<int, Transform> & poses,
2105  const std::multimap<int, Link> & edgeConstraints)
2106 {
2107  FILE * file = 0;
2108 
2109 #ifdef _MSC_VER
2110  fopen_s(&file, fileName.c_str(), "w");
2111 #else
2112  file = fopen(fileName.c_str(), "w");
2113 #endif
2114 
2115  if(file)
2116  {
2117  // force periods to be used instead of commas
2118  setlocale(LC_ALL, "en_US.UTF-8");
2119 
2120  if(isSlam2d())
2121  {
2122  // PARAMS_SE2OFFSET id x y theta (set for priors)
2123  fprintf(file, "PARAMS_SE2OFFSET %d 0 0 0\n", PARAM_OFFSET);
2124  }
2125  else
2126  {
2127  // PARAMS_SE3OFFSET id x y z qw qx qy qz (set for priors)
2128  Eigen::Vector3f v = Eigen::Vector3f::Zero();
2129  Eigen::Quaternionf q = Eigen::Quaternionf::Identity();
2130  fprintf(file, "PARAMS_SE3OFFSET %d %f %f %f %f %f %f %f\n",
2131  PARAM_OFFSET,
2132  v.x(),
2133  v.y(),
2134  v.z(),
2135  q.x(),
2136  q.y(),
2137  q.z(),
2138  q.w());
2139  }
2140 
2141  // For landmarks, determinate which one has observation with orientation
2142  std::map<int, bool> isLandmarkWithRotation;
2143  for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
2144  {
2145  int landmarkId = iter->second.from() < 0?iter->second.from():iter->second.to() < 0?iter->second.to():0;
2146  if(landmarkId != 0 && isLandmarkWithRotation.find(landmarkId) == isLandmarkWithRotation.end())
2147  {
2148  if(isSlam2d())
2149  {
2150  if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
2151  {
2152  isLandmarkWithRotation.insert(std::make_pair(landmarkId, false));
2153  UDEBUG("Tag %d has no orientation", landmarkId);
2154  }
2155  else
2156  {
2157  isLandmarkWithRotation.insert(std::make_pair(landmarkId, true));
2158  UDEBUG("Tag %d has orientation", landmarkId);
2159  }
2160  }
2161  else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
2162  1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
2163  1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
2164  {
2165  isLandmarkWithRotation.insert(std::make_pair(landmarkId, false));
2166  UDEBUG("Tag %d has no orientation", landmarkId);
2167  }
2168  else
2169  {
2170  isLandmarkWithRotation.insert(std::make_pair(landmarkId, true));
2171  UDEBUG("Tag %d has orientation", landmarkId);
2172  }
2173 
2174  }
2175  }
2176 
2177  int landmarkOffset = poses.size()&&poses.rbegin()->first>0?poses.rbegin()->first:0;
2178  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2179  {
2180  if (isSlam2d())
2181  {
2182  if(iter->first > 0)
2183  {
2184  // VERTEX_SE2 id x y theta
2185  fprintf(file, "VERTEX_SE2 %d %f %f %f\n",
2186  iter->first,
2187  iter->second.x(),
2188  iter->second.y(),
2189  iter->second.theta());
2190  }
2191  else if(!landmarksIgnored())
2192  {
2193  if(uValue(isLandmarkWithRotation, iter->first, false))
2194  {
2195  // VERTEX_SE2 id x y theta
2196  fprintf(file, "VERTEX_SE2 %d %f %f %f\n",
2197  landmarkOffset-iter->first,
2198  iter->second.x(),
2199  iter->second.y(),
2200  iter->second.theta());
2201  }
2202  else
2203  {
2204  // VERTEX_XY id x y
2205  fprintf(file, "VERTEX_XY %d %f %f\n",
2206  landmarkOffset-iter->first,
2207  iter->second.x(),
2208  iter->second.y());
2209  }
2210  }
2211  }
2212  else
2213  {
2214  if(iter->first > 0)
2215  {
2216  // VERTEX_SE3 id x y z qw qx qy qz
2217  Eigen::Quaternionf q = iter->second.getQuaternionf();
2218  fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n",
2219  iter->first,
2220  iter->second.x(),
2221  iter->second.y(),
2222  iter->second.z(),
2223  q.x(),
2224  q.y(),
2225  q.z(),
2226  q.w());
2227  }
2228  else if(!landmarksIgnored())
2229  {
2230  if(uValue(isLandmarkWithRotation, iter->first, false))
2231  {
2232  // VERTEX_SE3 id x y z qw qx qy qz
2233  Eigen::Quaternionf q = iter->second.getQuaternionf();
2234  fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n",
2235  landmarkOffset-iter->first,
2236  iter->second.x(),
2237  iter->second.y(),
2238  iter->second.z(),
2239  q.x(),
2240  q.y(),
2241  q.z(),
2242  q.w());
2243  }
2244  else
2245  {
2246  // VERTEX_TRACKXYZ id x y z
2247  fprintf(file, "VERTEX_TRACKXYZ %d %f %f %f\n",
2248  landmarkOffset-iter->first,
2249  iter->second.x(),
2250  iter->second.y(),
2251  iter->second.z());
2252  }
2253  }
2254  }
2255  }
2256 
2257  int virtualVertexId = landmarkOffset - (poses.size()&&poses.rbegin()->first<0?poses.rbegin()->first:0);
2258  for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
2259  {
2260  if (iter->second.type() == Link::kLandmark)
2261  {
2262  if (this->landmarksIgnored())
2263  {
2264  continue;
2265  }
2266  if(isSlam2d())
2267  {
2268  if(uValue(isLandmarkWithRotation, iter->first, false))
2269  {
2270  // 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
2271  fprintf(file, "EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f\n",
2272  iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
2273  iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
2274  iter->second.transform().x(),
2275  iter->second.transform().y(),
2276  iter->second.transform().theta(),
2277  iter->second.infMatrix().at<double>(0, 0),
2278  iter->second.infMatrix().at<double>(0, 1),
2279  iter->second.infMatrix().at<double>(0, 5),
2280  iter->second.infMatrix().at<double>(1, 1),
2281  iter->second.infMatrix().at<double>(1, 5),
2282  iter->second.infMatrix().at<double>(5, 5));
2283  }
2284  else
2285  {
2286  // EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
2287  fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f\n",
2288  iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
2289  iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
2290  iter->second.transform().x(),
2291  iter->second.transform().y(),
2292  iter->second.infMatrix().at<double>(0, 0),
2293  iter->second.infMatrix().at<double>(0, 1),
2294  iter->second.infMatrix().at<double>(1, 1));
2295  }
2296  }
2297  else
2298  {
2299  if(uValue(isLandmarkWithRotation, iter->first, false))
2300  {
2301  // 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
2302  Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
2303  fprintf(file, "EDGE_SE3 %d %d %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",
2304  iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
2305  iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
2306  iter->second.transform().x(),
2307  iter->second.transform().y(),
2308  iter->second.transform().z(),
2309  q.x(),
2310  q.y(),
2311  q.z(),
2312  q.w(),
2313  iter->second.infMatrix().at<double>(0, 0),
2314  iter->second.infMatrix().at<double>(0, 1),
2315  iter->second.infMatrix().at<double>(0, 2),
2316  iter->second.infMatrix().at<double>(0, 3),
2317  iter->second.infMatrix().at<double>(0, 4),
2318  iter->second.infMatrix().at<double>(0, 5),
2319  iter->second.infMatrix().at<double>(1, 1),
2320  iter->second.infMatrix().at<double>(1, 2),
2321  iter->second.infMatrix().at<double>(1, 3),
2322  iter->second.infMatrix().at<double>(1, 4),
2323  iter->second.infMatrix().at<double>(1, 5),
2324  iter->second.infMatrix().at<double>(2, 2),
2325  iter->second.infMatrix().at<double>(2, 3),
2326  iter->second.infMatrix().at<double>(2, 4),
2327  iter->second.infMatrix().at<double>(2, 5),
2328  iter->second.infMatrix().at<double>(3, 3),
2329  iter->second.infMatrix().at<double>(3, 4),
2330  iter->second.infMatrix().at<double>(3, 5),
2331  iter->second.infMatrix().at<double>(4, 4),
2332  iter->second.infMatrix().at<double>(4, 5),
2333  iter->second.infMatrix().at<double>(5, 5));
2334  }
2335  else
2336  {
2337  // 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
2338  fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f\n",
2339  iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
2340  iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
2341  PARAM_OFFSET,
2342  iter->second.transform().x(),
2343  iter->second.transform().y(),
2344  iter->second.transform().z(),
2345  iter->second.infMatrix().at<double>(0, 0),
2346  iter->second.infMatrix().at<double>(0, 1),
2347  iter->second.infMatrix().at<double>(0, 2),
2348  iter->second.infMatrix().at<double>(1, 1),
2349  iter->second.infMatrix().at<double>(1, 2),
2350  iter->second.infMatrix().at<double>(2, 2));
2351  }
2352  }
2353  continue;
2354  }
2355 
2356  std::string prefix = isSlam2d()? "EDGE_SE2" :"EDGE_SE3:QUAT";
2357  std::string suffix = "";
2358  std::string to = uFormat(" %d", iter->second.to());
2359 
2360  bool isSE2 = true;
2361  bool isSE3 = true;
2362 
2363  if (iter->second.type() == Link::kGravity)
2364  {
2365  continue;
2366  }
2367  else if (iter->second.type() == Link::kPosePrior)
2368  {
2369  if (this->priorsIgnored())
2370  {
2371  continue;
2372  }
2373  if (isSlam2d())
2374  {
2375  if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
2376  {
2377  prefix = "EDGE_PRIOR_SE2_XY";
2378  isSE2 = false;
2379  }
2380  else
2381  {
2382  prefix = "EDGE_PRIOR_SE2";
2383  }
2384 
2385  // based on https://github.com/RainerKuemmerle/g2o/blob/38347944c6ad7a3b31976b97406ff0de20be1530/g2o/types/slam2d/edge_se2_prior.cpp#L42
2386  // there is no pid for the 2d prior case
2387  to = "";
2388  }
2389  else
2390  {
2391  if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
2392  1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
2393  1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
2394  {
2395  to = "";
2396  prefix = "EDGE_POINTXYZ_PRIOR";
2397  isSE3 = false;
2398  }
2399  else
2400  {
2401  to = uFormat(" %d", PARAM_OFFSET);
2402  prefix = "EDGE_SE3_PRIOR";
2403  }
2404  }
2405  }
2406  else if(this->isRobust() &&
2407  iter->second.type() != Link::kNeighbor &&
2408  iter->second.type() != Link::kNeighborMerged)
2409  {
2410  fprintf(file, "VERTEX_SWITCH %d 1\n", virtualVertexId);
2411  fprintf(file, "EDGE_SWITCH_PRIOR %d 1 1.0\n", virtualVertexId);
2412  prefix = isSlam2d() ? "EDGE_SE2_SWITCHABLE" : "EDGE_SE3_SWITCHABLE";
2413  suffix = uFormat(" %d", virtualVertexId++);
2414  }
2415 
2416  if(isSlam2d())
2417  {
2418  if (isSE2)
2419  {
2420  // 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
2421  // EDGE_SE2_PRIOR observed_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
2422  fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
2423  prefix.c_str(),
2424  iter->second.from(),
2425  to.c_str(),
2426  suffix.c_str(),
2427  iter->second.transform().x(),
2428  iter->second.transform().y(),
2429  iter->second.transform().theta(),
2430  iter->second.infMatrix().at<double>(0, 0),
2431  iter->second.infMatrix().at<double>(0, 1),
2432  iter->second.infMatrix().at<double>(0, 5),
2433  iter->second.infMatrix().at<double>(1, 1),
2434  iter->second.infMatrix().at<double>(1, 5),
2435  iter->second.infMatrix().at<double>(5, 5));
2436  }
2437  else
2438  {
2439  // EDGE_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
2440  // EDGE_POINTXY_PRIOR x y inf_11 inf_12 inf_22
2441  fprintf(file, "%s %d%s%s %f %f %f %f %f\n",
2442  prefix.c_str(),
2443  iter->second.from(),
2444  to.c_str(),
2445  suffix.c_str(),
2446  iter->second.transform().x(),
2447  iter->second.transform().y(),
2448  iter->second.infMatrix().at<double>(0, 0),
2449  iter->second.infMatrix().at<double>(0, 1),
2450  iter->second.infMatrix().at<double>(1, 1));
2451  }
2452  }
2453  else
2454  {
2455  if (isSE3)
2456  {
2457  // 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
2458  // 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
2459  Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
2460  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",
2461  prefix.c_str(),
2462  iter->second.from(),
2463  to.c_str(),
2464  suffix.c_str(),
2465  iter->second.transform().x(),
2466  iter->second.transform().y(),
2467  iter->second.transform().z(),
2468  q.x(),
2469  q.y(),
2470  q.z(),
2471  q.w(),
2472  iter->second.infMatrix().at<double>(0, 0),
2473  iter->second.infMatrix().at<double>(0, 1),
2474  iter->second.infMatrix().at<double>(0, 2),
2475  iter->second.infMatrix().at<double>(0, 3),
2476  iter->second.infMatrix().at<double>(0, 4),
2477  iter->second.infMatrix().at<double>(0, 5),
2478  iter->second.infMatrix().at<double>(1, 1),
2479  iter->second.infMatrix().at<double>(1, 2),
2480  iter->second.infMatrix().at<double>(1, 3),
2481  iter->second.infMatrix().at<double>(1, 4),
2482  iter->second.infMatrix().at<double>(1, 5),
2483  iter->second.infMatrix().at<double>(2, 2),
2484  iter->second.infMatrix().at<double>(2, 3),
2485  iter->second.infMatrix().at<double>(2, 4),
2486  iter->second.infMatrix().at<double>(2, 5),
2487  iter->second.infMatrix().at<double>(3, 3),
2488  iter->second.infMatrix().at<double>(3, 4),
2489  iter->second.infMatrix().at<double>(3, 5),
2490  iter->second.infMatrix().at<double>(4, 4),
2491  iter->second.infMatrix().at<double>(4, 5),
2492  iter->second.infMatrix().at<double>(5, 5));
2493  }
2494  else
2495  {
2496  // 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
2497  // EDGE_POINTXYZ_PRIOR observed_vertex_id x y z inf_11 inf_12 .. inf_13 inf_22 .. inf_33
2498  fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
2499  prefix.c_str(),
2500  iter->second.from(),
2501  to.c_str(),
2502  suffix.c_str(),
2503  iter->second.transform().x(),
2504  iter->second.transform().y(),
2505  iter->second.transform().z(),
2506  iter->second.infMatrix().at<double>(0, 0),
2507  iter->second.infMatrix().at<double>(0, 1),
2508  iter->second.infMatrix().at<double>(0, 2),
2509  iter->second.infMatrix().at<double>(1, 1),
2510  iter->second.infMatrix().at<double>(1, 2),
2511  iter->second.infMatrix().at<double>(2, 2));
2512  }
2513  }
2514  }
2515  UINFO("Graph saved to %s", fileName.c_str());
2516 
2517  fclose(file);
2518  }
2519  else
2520  {
2521  UERROR("Cannot save to file %s", fileName.c_str());
2522  return false;
2523  }
2524  return true;
2525 }
2526 
2527 } /* namespace rtabmap */
EdgeSE3Switchable
Definition: edge_se3Switchable.h:18
es
EigenSolver< MatrixXf > es
rtabmap::OptimizerG2O::saveGraph
bool saveGraph(const std::string &fileName, const std::map< int, Transform > &poses, const std::multimap< int, Link > &edgeConstraints)
Definition: OptimizerG2O.cpp:2102
rtabmap::EdgeSE3Gravity
g2o edge with gravity constraint
Definition: edge_se3_gravity.h:44
rtabmap::FeatureBA::kpt
cv::KeyPoint kpt
Definition: Optimizer.h:52
EdgeSE2Switchable
Definition: edge_se2Switchable.h:18
UINFO
#define UINFO(...)
rtabmap::Optimizer::priorsIgnored
bool priorsIgnored() const
Definition: Optimizer.h:95
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
block
void block(const MatrixType &m)
edge_switchPrior.h
rtabmap::OptimizerG2O::robustKernelDelta_
double robustKernelDelta_
Definition: OptimizerG2O.h:79
v1
v1
Eigen::Transform
timer
g2o::EdgeSE3XYZPrior
Prior for a 3D pose with constraints only in xyz direction.
Definition: edge_se3_xyzprior.h:41
rtabmap::OptimizerG2O::OptimizerG2O
OptimizerG2O(const ParametersMap &parameters=ParametersMap())
Definition: OptimizerG2O.cpp:144
edges
vector< MFAS::KeyPair > edges
g2o::EdgeXYZPrior
prior for an XYZ vertex (VertexPointXYZ)
Definition: edge_xyz_prior.h:48
g2o::EdgeXYPrior::setMeasurement
virtual void setMeasurement(const Vector2d &m)
Definition: edge_xy_prior.h:56
rtabmap::Optimizer
Definition: Optimizer.h:61
rtabmap::Optimizer::landmarksIgnored
bool landmarksIgnored() const
Definition: Optimizer.h:96
util3d_motion_estimation.h
edge_se2Switchable.h
rtabmap::OptimizerG2O::isCholmodAvailable
static bool isCholmodAvailable()
Definition: OptimizerG2O.cpp:135
edge_xyz_prior.h
MULTICAM_OFFSET
#define MULTICAM_OFFSET
Definition: OptimizerG2O.cpp:113
util3d.h
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
Vector2::y
float y
UTimer.h
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
cam
rtabmap::SensorCaptureThread * cam
Definition: tools/DataRecorder/main.cpp:58
EdgeSwitchPrior
Definition: edge_switchPrior.h:9
UMath.h
Basic mathematics functions.
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:503
rtabmap::Optimizer::isSlam2d
bool isSlam2d() const
Definition: Optimizer.h:91
rtabmap::Optimizer::epsilon
double epsilon() const
Definition: Optimizer.h:93
rtabmap::OptimizerG2O::pixelVariance_
double pixelVariance_
Definition: OptimizerG2O.h:78
uIsNan
bool uIsNan(const T &value)
Definition: UMath.h:40
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:407
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:1483
edge_xy_prior.h
delta
def delta(g0, g1)
util3d_transforms.h
q
EIGEN_DEVICE_FUNC const Scalar & q
UConversion.h
Some conversion functions.
edge_se3Switchable.h
g2o
Definition: edge_se3_xyzprior.h:34
first
constexpr int first(int i)
rtabmap::util3d::isFinite
bool RTABMAP_CORE_EXPORT isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3436
Eigen::PlainObjectBase::data
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar * data()
std::swap
void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)
rtabmap::FeatureBA::depth
float depth
Definition: Optimizer.h:53
UASSERT
#define UASSERT(condition)
d
d
rtabmap::OptimizerG2O::optimizer_
int optimizer_
Definition: OptimizerG2O.h:77
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:238
m
Matrix3f m
p
Point3_ p(2)
edge_se3_gravity.h
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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::Parameters
Definition: Parameters.h:170
rtabmap::Optimizer::gravitySigma
float gravitySigma() const
Definition: Optimizer.h:97
Eigen::Map
VertexSwitchLinear
Definition: vertex_switchLinear.h:19
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::Transform::fromEigen3d
static Transform fromEigen3d(const Eigen::Affine3d &matrix)
Definition: Transform.cpp:435
rtabmap::FeatureBA
Definition: Optimizer.h:41
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UWARN
#define UWARN(...)
rtabmap::Optimizer::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Optimizer.cpp:322
rtabmap::EdgeSE3Gravity::setMeasurement
virtual void setMeasurement(const Eigen::Matrix< double, 6, 1 > &m)
Definition: edge_se3_gravity.h:83
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::EdgeSBACamGravity::setMeasurement
virtual void setMeasurement(const Eigen::Matrix< double, 6, 1 > &m)
Definition: edge_sbacam_gravity.h:87
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Eigen::Quaternion
ULogger.h
ULogger class and convenient macros.
a
ArrayXXi a
rtabmap::EdgeSBACamGravity
EdgeSBACamGravity.
Definition: edge_sbacam_gravity.h:42
rtabmap::Transform
Definition: Transform.h:41
vertex_switchLinear.h
PARAM_OFFSET
@ PARAM_OFFSET
Definition: OptimizerG2O.cpp:110
rtabmap::EdgeSBACamGravity::setCameraInvLocalTransform
void setCameraInvLocalTransform(const Eigen::Matrix3d &t)
Definition: edge_sbacam_gravity.h:51
rtabmap::OptimizerG2O::solver_
int solver_
Definition: OptimizerG2O.h:76
rtabmap::Optimizer::setRobust
void setRobust(bool enabled)
Definition: Optimizer.h:104
iter
iterator iter(handle obj)
g2o::EdgeXYZPrior::setMeasurement
virtual void setMeasurement(const Vector3d &m)
Definition: edge_xyz_prior.h:60
id
id
c_str
const char * c_str(Args &&...args)
rtabmap::OptimizerG2O::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: OptimizerG2O.cpp:166
UStl.h
Wrappers of STL for convenient functions.
v
Array< int, Dynamic, 1 > v
UDEBUG
#define UDEBUG(...)
g2o::EdgeSE3XYZPrior::setMeasurement
virtual void setMeasurement(const Vector3d &m)
Definition: edge_se3_xyzprior.h:47
UTimer
Definition: UTimer.h:46
rtabmap::OptimizerG2O::available
static bool available()
Definition: OptimizerG2O.cpp:117
scan_step::second
@ second
Eigen::Matrix
prior
const auto prior
rtabmap::Transform::toEigen3d
Eigen::Affine3d toEigen3d() const
Definition: Transform.cpp:396
Vector2::x
float x
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:90
v3
Vector v3
Eigen::Transform::translation
EIGEN_DEVICE_FUNC TranslationPart translation()
rtabmap::Optimizer::isRobust
bool isRobust() const
Definition: Optimizer.h:94
OptimizerG2O.h
t
Point2 t(10, 10)
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:92
g2o::EdgeXYPrior
Definition: edge_xy_prior.h:42
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
rtabmap::OptimizerG2O::isCSparseAvailable
static bool isCSparseAvailable()
Definition: OptimizerG2O.cpp:126
file
file
edge_se3_xyzprior.h
i
int i
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:53
result
RESULT & result
baseline
double baseline
edge_sbacam_gravity.h
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::OptimizerG2O::baseline_
double baseline_
Definition: OptimizerG2O.h:80
v2
v2

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Feb 13 2025 03:44:57