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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:14