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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59