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