OptimizerGTSAM.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 #include "rtabmap/core/Graph.h"
28 
29 #include <rtabmap/utilite/ULogger.h>
30 #include <rtabmap/utilite/UStl.h>
31 #include <rtabmap/utilite/UMath.h>
32 #include <rtabmap/utilite/UConversion.h>
33 #include <rtabmap/utilite/UTimer.h>
34 #include <set>
35 
36 #include <rtabmap/core/optimizer/OptimizerGTSAM.h>
37 
38 #ifdef RTABMAP_GTSAM
39 #include <gtsam/geometry/Pose2.h>
40 #include <gtsam/geometry/Pose3.h>
41 #include <gtsam/inference/Key.h>
42 #include <gtsam/inference/Symbol.h>
43 #include <gtsam/slam/PriorFactor.h>
44 #include <gtsam/slam/BetweenFactor.h>
45 #include <gtsam/sam/BearingRangeFactor.h>
46 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
47 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
48 #include <gtsam/nonlinear/DoglegOptimizer.h>
49 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
50 #include <gtsam/nonlinear/NonlinearOptimizer.h>
51 #include <gtsam/nonlinear/Marginals.h>
52 #include <gtsam/nonlinear/Values.h>
53 #include "gtsam/GravityFactor.h"
54 #include <optimizer/gtsam/XYFactor.h>
55 #include <optimizer/gtsam/XYZFactor.h>
56 #include <gtsam/nonlinear/ISAM2.h>
57 
58 #ifdef RTABMAP_VERTIGO
59 #include "vertigo/gtsam/betweenFactorSwitchable.h"
60 #include "vertigo/gtsam/switchVariableLinear.h"
61 #endif
62 #endif // end RTABMAP_GTSAM
63 
64 namespace rtabmap {
65 
66 OptimizerGTSAM::OptimizerGTSAM(const ParametersMap & parameters) :
67  Optimizer(parameters),
68  internalOptimizerType_(Parameters::defaultGTSAMOptimizer()),
69  isam2_(0),
70  lastSwitchId_(1000000000)
71 {
72  lastRootFactorIndex_.first = 0;
73  parseParameters(parameters);
74 }
75 
76 OptimizerGTSAM::~OptimizerGTSAM()
77 {
78 #ifdef RTABMAP_GTSAM
79  delete isam2_;
80 #endif
81 }
82 
83 bool OptimizerGTSAM::available()
84 {
85 #ifdef RTABMAP_GTSAM
86  return true;
87 #else
88  return false;
89 #endif
90 }
91 
92 void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
93 {
94  Optimizer::parseParameters(parameters);
95 #ifdef RTABMAP_GTSAM
96  Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), internalOptimizerType_);
97 
98  bool incremental = isam2_;
99  double threshold = Parameters::defaultGTSAMIncRelinearizeThreshold();
100  int skip = Parameters::defaultGTSAMIncRelinearizeSkip();
101  Parameters::parse(parameters, Parameters::kGTSAMIncremental(), incremental);
102  Parameters::parse(parameters, Parameters::kGTSAMIncRelinearizeThreshold(), threshold);
103  Parameters::parse(parameters, Parameters::kGTSAMIncRelinearizeSkip(), skip);
104  UDEBUG("GTSAM %s=%d", Parameters::kGTSAMOptimizer().c_str(), internalOptimizerType_);
105  UDEBUG("GTSAM %s=%d", Parameters::kGTSAMIncremental().c_str(), incremental?1:0);
106  UDEBUG("GTSAM %s=%f", Parameters::kGTSAMIncRelinearizeThreshold().c_str(), threshold);
107  UDEBUG("GTSAM %s=%d", Parameters::kGTSAMIncRelinearizeSkip().c_str(), skip);
108  if(incremental && !isam2_)
109  {
110  gtsam::ISAM2Params::OptimizationParams optParams;
111  if(internalOptimizerType_==2)
112  {
113  optParams = gtsam::ISAM2DoglegParams();
114  }
115  else
116  {
117  optParams = gtsam::ISAM2GaussNewtonParams();
118  }
119  gtsam::ISAM2Params params(optParams);
120  params.relinearizeThreshold = threshold;
121  params.relinearizeSkip = skip;
122  params.evaluateNonlinearError = true;
123  isam2_ = new ISAM2(params);
124 
125  addedPoses_.clear();
126  lastAddedConstraints_.clear();
127  lastRootFactorIndex_.first = 0;
128  lastSwitchId_ = 1000000000;
129  }
130  else if(!incremental && isam2_)
131  {
132  delete isam2_;
133  isam2_ = 0;
134  }
135 #endif
136 }
137 
138 std::map<int, Transform> OptimizerGTSAM::optimize(
139  int rootId,
140  const std::map<int, Transform> & poses,
141  const std::multimap<int, Link> & edgeConstraints,
142  cv::Mat & outputCovariance,
143  std::list<std::map<int, Transform> > * intermediateGraphes,
144  double * finalError,
145  int * iterationsDone)
146 {
147  outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
148  std::map<int, Transform> optimizedPoses;
149 #ifdef RTABMAP_GTSAM
150 
151 #ifndef RTABMAP_VERTIGO
152  if(this->isRobust())
153  {
154  UWARN("Vertigo robust optimization is not available! Robust optimization is now disabled.");
155  setRobust(false);
156  }
157 #endif
158 
159  UDEBUG("Optimizing graph...");
160  if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
161  {
162  gtsam::NonlinearFactorGraph graph;
163 
164  // detect if there is a global pose prior set, if so remove rootId
165  bool hasGPSPrior = false;
166  bool hasGravityConstraints = false;
167  if(!priorsIgnored() || (!isSlam2d() && gravitySigma() > 0))
168  {
169  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
170  {
171  if(iter->second.from() == iter->second.to())
172  {
173  if(!priorsIgnored() && iter->second.type() == Link::kPosePrior)
174  {
175  hasGPSPrior = true;
176  if ((isSlam2d() && 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999) ||
177  (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) < 9999.0 &&
178  1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) < 9999.0 &&
179  1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0))
180  {
181  // orientation is set, don't set root prior (it is no GPS)
182  rootId = 0;
183  hasGPSPrior = false;
184  break;
185  }
186  }
187  if(iter->second.type() == Link::kGravity)
188  {
189  hasGravityConstraints = true;
190  if(priorsIgnored())
191  {
192  break;
193  }
194  }
195  }
196  }
197  }
198 
199  std::vector<ConstraintToFactor> addedPrior;
200  gtsam::FactorIndices removeFactorIndices;
201 
202  //prior first pose
203  if(rootId != 0 && (!isam2_ || lastRootFactorIndex_.first != rootId))
204  {
205  UDEBUG("Setting prior for rootId=%d", rootId);
206  UASSERT(uContains(poses, rootId));
207  const Transform & initialPose = poses.at(rootId);
208  UDEBUG("hasGPSPrior=%s", hasGPSPrior?"true":"false");
209  if(isSlam2d())
210  {
211  gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:std::numeric_limits<double>::min()));
212  graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
213  addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1));
214  }
215  else
216  {
217  gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
218  (gtsam::Vector(6) <<
219  (hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), hasGPSPrior?1e-2:std::numeric_limits<double>::min(), // roll, pitch, fixed yaw if there are no priors
220  (hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz
221  ).finished());
222  graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
223  addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1));
224  }
225  if(isam2_ && lastRootFactorIndex_.first!=0)
226  {
227  if(uContains(poses, lastRootFactorIndex_.first))
228  {
229  UDEBUG("isam2: switching rootid from %d to %d", lastRootFactorIndex_.first, rootId);
230  removeFactorIndices.push_back(lastRootFactorIndex_.second);
231  }
232  else
233  {
234  UDEBUG("isam2: reset iSAM2, disjoint mapping sessions between previous root %d and new root %d", lastRootFactorIndex_.first, rootId);
235  // reset iSAM2, disjoint mapping session
236  gtsam::ISAM2Params params = isam2_->params();
237  delete isam2_;
238  isam2_ = new gtsam::ISAM2(params);
239  addedPoses_.clear();
240  lastAddedConstraints_.clear();
241  lastRootFactorIndex_.first = 0;
242  lastSwitchId_ = 1000000000;
243  }
244  lastRootFactorIndex_.first = 0;
245  }
246  }
247 
248  std::map<int, Transform> newPoses;
249  std::multimap<int, Link> newEdgeConstraints;
250 
251  if(isam2_)
252  {
253  UDEBUG("Add new poses...");
254  // new poses?
255  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
256  {
257  if(addedPoses_.find(iter->first) == addedPoses_.end())
258  {
259  newPoses.insert(*iter);
260  UDEBUG("Adding pose %d to factor graph", iter->first);
261  }
262  }
263  UDEBUG("Add new links...");
264  // new links?
265  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
266  {
267  if(addedPoses_.find(iter->second.from()) == addedPoses_.end() ||
268  addedPoses_.find(iter->second.to()) == addedPoses_.end())
269  {
270  newEdgeConstraints.insert(*iter);
271  UDEBUG("Adding constraint %d (%d->%d) to factor graph", iter->first, iter->second.from(), iter->second.to());
272  }
273  }
274 
275  if(!this->isRobust())
276  {
277  UDEBUG("Remove links...");
278  // Remove constraints not there anymore in case the last loop closures were rejected.
279  // As we don't track "switch" constraints, we don't support this if vertigo is used.
280  for(size_t i=0; i<lastAddedConstraints_.size(); ++i)
281  {
282  if(lastAddedConstraints_[i].from != lastAddedConstraints_[i].to &&
283  graph::findLink(edgeConstraints, lastAddedConstraints_[i].from, lastAddedConstraints_[i].to) == edgeConstraints.end())
284  {
285  removeFactorIndices.push_back(lastAddedConstraints_[i].factorIndice);
286  UDEBUG("Removing constraint %d->%d (factor indice=%ld)",
287  lastAddedConstraints_[i].from,
288  lastAddedConstraints_[i].to,
289  lastAddedConstraints_[i].factorIndice);
290  }
291  }
292  }
293  else if(poses.rbegin()->first >= 1000000000)
294  {
295  UERROR("Lastest pose id (%d) is too huge for robust optimization (over switch offset of 1000000000)", poses.rbegin()->first);
296  return optimizedPoses;
297  }
298 
299  lastAddedConstraints_ = addedPrior;
300  }
301  else
302  {
303  newPoses = poses;
304  newEdgeConstraints = edgeConstraints;
305  }
306 
307  UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d landmarksIgnored=%d)",
308  rootId, priorsIgnored()?1:0, landmarksIgnored()?1:0);
309  gtsam::Values initialEstimate;
310  std::map<int, bool> isLandmarkWithRotation;
311  for(std::map<int, Transform>::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
312  {
313  UASSERT(!iter->second.isNull());
314  if(isSlam2d())
315  {
316  if(iter->first > 0)
317  {
318  initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
319  addedPoses_.insert(iter->first);
320  }
321  else if(!landmarksIgnored())
322  {
323  // check if it is SE2 or only PointXY
324  std::multimap<int, Link>::const_iterator jter=newEdgeConstraints.find(iter->first);
325  UASSERT_MSG(jter != newEdgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
326 
327  if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
328  {
329  initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
330  isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
331  }
332  else
333  {
334  initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
335  isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
336  }
337  addedPoses_.insert(iter->first);
338  }
339 
340  }
341  else
342  {
343  if(iter->first > 0)
344  {
345  initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
346  addedPoses_.insert(iter->first);
347  }
348  else if(!landmarksIgnored())
349  {
350  // check if it is SE3 or only PointXYZ
351  std::multimap<int, Link>::const_iterator jter=newEdgeConstraints.find(iter->first);
352  UASSERT_MSG(jter != newEdgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
353 
354  if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
355  1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
356  1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
357  {
358  initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
359  isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
360  }
361  else
362  {
363  initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
364  isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
365  }
366  addedPoses_.insert(iter->first);
367  }
368  }
369  }
370 
371  UDEBUG("fill edges to gtsam...");
372  if(!isam2_)
373  {
374  lastSwitchId_ = newPoses.rbegin()->first+1;
375  }
376  for(std::multimap<int, Link>::const_iterator iter=newEdgeConstraints.begin(); iter!=newEdgeConstraints.end(); ++iter)
377  {
378  int id1 = iter->second.from();
379  int id2 = iter->second.to();
380 
381  UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d", id1).c_str());
382  UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d", id2).c_str());
383 
384  UASSERT(!iter->second.transform().isNull());
385  if(id1 == id2)
386  {
387  if(iter->second.type() == Link::kPosePrior && !priorsIgnored() &&
388  (!landmarksIgnored() || id1>0))
389  {
390  if(isSlam2d())
391  {
392  if(id1 < 0 && !isLandmarkWithRotation.at(id1))
393  {
394  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
395  1/iter->second.infMatrix().at<double>(0,0),
396  1/iter->second.infMatrix().at<double>(1,1)));
397  graph.add(XYFactor<gtsam::Point2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
398  lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
399  }
400  else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
401  {
402  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
403  1/iter->second.infMatrix().at<double>(0,0),
404  1/iter->second.infMatrix().at<double>(1,1)));
405  graph.add(XYFactor<gtsam::Pose2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
406  lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
407  }
408  else
409  {
410  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
411  if(!isCovarianceIgnored())
412  {
413  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
414  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
415  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
416  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
417  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
418  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
419  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
420  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
421  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
422  }
423 
424  gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
425  graph.add(gtsam::PriorFactor<gtsam::Pose2>(id1, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
426  lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
427  }
428  }
429  else
430  {
431  if(id1 < 0 && !isLandmarkWithRotation.at(id1))
432  {
433  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
434  iter->second.infMatrix().at<double>(0,0),
435  iter->second.infMatrix().at<double>(1,1),
436  iter->second.infMatrix().at<double>(2,2)));
437  graph.add(XYZFactor<gtsam::Point3>(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
438  lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
439  }
440  else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
441  1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
442  1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
443  {
444  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
445  iter->second.infMatrix().at<double>(0,0),
446  iter->second.infMatrix().at<double>(1,1),
447  iter->second.infMatrix().at<double>(2,2)));
448  graph.add(XYZFactor<gtsam::Pose3>(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
449  lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
450  }
451  else
452  {
453  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
454  if(!isCovarianceIgnored())
455  {
456  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
457  }
458 
459  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
460  mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
461  mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
462  mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
463  mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
464  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
465 
466  graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
467  lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1));
468  }
469  }
470  }
471  else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && newPoses.find(iter->first) != newPoses.end())
472  {
473  Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
474  gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
475  gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), 10));
476  graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1)));
477  lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1));
478  }
479  }
480  else if(id1<0 || id2 < 0)
481  {
482  if(!landmarksIgnored())
483  {
484  //landmarks
485  UASSERT((id1 < 0 && id2 > 0) || (id1 > 0 && id2 < 0));
486  Transform t;
487  if(id2 < 0)
488  {
489  t = iter->second.transform();
490  }
491  else
492  {
493  t = iter->second.transform().inverse();
494  std::swap(id1, id2); // should be node -> landmark
495  }
496 
497 #ifdef RTABMAP_VERTIGO
498  if(this->isRobust() && isLandmarkWithRotation.at(id2))
499  {
500  // create new switch variable
501  // Sunderhauf IROS 2012:
502  // "Since it is reasonable to initially accept all loop closure constraints,
503  // a proper and convenient initial value for all switch variables would be
504  // sij = 1 when using the linear switch function"
505  double prior = 1.0;
506  initialEstimate.insert(gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior));
507 
508  // create switch prior factor
509  // "If the front-end is not able to assign sound individual values
510  // for Ξij , it is save to set all Ξij = 1, since this value is close
511  // to the individual optimal choice of Ξij for a large range of
512  // outliers."
513  gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
514  graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior), switchPriorModel));
515  }
516  else if(this->isRobust() && !isLandmarkWithRotation.at(id2))
517  {
518  UWARN("%s cannot be used for landmark constraints without orientation.", Parameters::kOptimizerRobust().c_str());
519  }
520 #endif
521 
522  if(isSlam2d())
523  {
524  if(isLandmarkWithRotation.at(id2))
525  {
526  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
527  if(!isCovarianceIgnored())
528  {
529  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
530  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
531  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
532  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
533  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
534  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
535  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
536  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
537  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
538  }
539  gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
540 
541 #ifdef RTABMAP_VERTIGO
542  if(this->isRobust())
543  {
544  // create switchable edge factor
545  graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose2(t.x(), t.y(), t.theta()), model));
546  }
547  else
548 #endif
549  {
550  graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(t.x(), t.y(), t.theta()), model));
551  lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
552  }
553  }
554  else
555  {
556  Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
557  if(!isCovarianceIgnored())
558  {
559  cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,2), cv::Range(0,2)).clone();;
560  memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
561  }
562  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);
563 
564  gtsam::Point2 landmark(t.x(), t.y());
565  gtsam::Pose2 p;
566  graph.add(gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2>(id1, id2, p.bearing(landmark), p.range(landmark), model));
567  lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
568  }
569  }
570  else
571  {
572  if(isLandmarkWithRotation.at(id2))
573  {
574  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
575  if(!isCovarianceIgnored())
576  {
577  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
578  }
579 
580  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
581  mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
582  mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
583  mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
584  mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
585  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
586 
587 #ifdef RTABMAP_VERTIGO
588  if(this->isRobust() &&
589  iter->second.type() != Link::kNeighbor &&
590  iter->second.type() != Link::kNeighborMerged)
591  {
592  // create switchable edge factor
593  graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose3(t.toEigen4d()), model));
594  }
595  else
596 #endif
597  {
598  graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(t.toEigen4d()), model));
599  lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
600  }
601  }
602  else
603  {
604  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
605  if(!isCovarianceIgnored())
606  {
607  cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();;
608  memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
609  }
610  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);
611 
612  gtsam::Point3 landmark(t.x(), t.y(), t.z());
613  gtsam::Pose3 p;
614  graph.add(gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3>(id1, id2, p.bearing(landmark), p.range(landmark), model));
615  lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
616  }
617  }
618  }
619  }
620  else // id1 != id2
621  {
622 #ifdef RTABMAP_VERTIGO
623  if(this->isRobust() &&
624  iter->second.type() != Link::kNeighbor &&
625  iter->second.type() != Link::kNeighborMerged)
626  {
627  // create new switch variable
628  // Sunderhauf IROS 2012:
629  // "Since it is reasonable to initially accept all loop closure constraints,
630  // a proper and convenient initial value for all switch variables would be
631  // sij = 1 when using the linear switch function"
632  double prior = 1.0;
633  initialEstimate.insert(gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior));
634 
635  // create switch prior factor
636  // "If the front-end is not able to assign sound individual values
637  // for Ξij , it is save to set all Ξij = 1, since this value is close
638  // to the individual optimal choice of Ξij for a large range of
639  // outliers."
640  gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
641  graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior), switchPriorModel));
642  }
643 #endif
644  if(isSlam2d())
645  {
646  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
647  if(!isCovarianceIgnored())
648  {
649  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
650  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
651  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
652  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
653  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
654  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
655  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
656  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
657  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
658  }
659  gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
660 
661 #ifdef RTABMAP_VERTIGO
662  if(this->isRobust() &&
663  iter->second.type()!=Link::kNeighbor &&
664  iter->second.type() != Link::kNeighborMerged)
665  {
666  // create switchable edge factor
667  graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
668  }
669  else
670 #endif
671  {
672  graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
673  lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
674  }
675  }
676  else
677  {
678  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
679  if(!isCovarianceIgnored())
680  {
681  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
682  }
683 
684  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
685  mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
686  mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
687  mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
688  mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
689  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
690 
691 #ifdef RTABMAP_VERTIGO
692  if(this->isRobust() &&
693  iter->second.type() != Link::kNeighbor &&
694  iter->second.type() != Link::kNeighborMerged)
695  {
696  // create switchable edge factor
697  graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
698  }
699  else
700 #endif
701  {
702  graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
703  lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
704  }
705  }
706  }
707  }
708 
709  UDEBUG("create optimizer");
710  gtsam::NonlinearOptimizer * optimizer = 0;
711 
712  if(!isam2_) // Batch optimization
713  {
714  UDEBUG("Batch optimization...");
715  if(internalOptimizerType_ == 2)
716  {
717  gtsam::DoglegParams parameters;
718  parameters.relativeErrorTol = epsilon();
719  parameters.maxIterations = iterations();
720  optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters);
721  }
722  else if(internalOptimizerType_ == 1)
723  {
724  gtsam::GaussNewtonParams parameters;
725  parameters.relativeErrorTol = epsilon();
726  parameters.maxIterations = iterations();
727  optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters);
728  }
729  else
730  {
731  gtsam::LevenbergMarquardtParams parameters;
732  parameters.relativeErrorTol = epsilon();
733  parameters.maxIterations = iterations();
734  optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);
735  }
736  }
737  else
738  {
739  UDEBUG("iSAM2 optimization...");
740  }
741 
742  UDEBUG("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
743  UTimer timer;
744  int it = 0;
745  double initialError = optimizer?graph.error(initialEstimate):0;
746  double lastError = optimizer?optimizer->error():0;
747  for(int i=0; i<iterations(); ++i)
748  {
749  if(intermediateGraphes && i > 0)
750  {
751  float x,y,z,roll,pitch,yaw;
752  std::map<int, Transform> tmpPoses;
753  const Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
754 #if GTSAM_VERSION_NUMERIC >= 40200
755  for(gtsam::Values::deref_iterator iter=values.begin(); iter!=values.end(); ++iter)
756 #else
757  for(gtsam::Values::const_iterator iter=values.begin(); iter!=values.end(); ++iter)
758 #endif
759  {
760  int key = (int)iter->key;
761  if(iter->value.dim() > 1 && uContains(newPoses, key))
762  {
763  if(isSlam2d())
764  {
765  if(key > 0)
766  {
767  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
768  tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
769  }
770  else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
771  {
772  if(isLandmarkWithRotation.at(key))
773  {
774  newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
775  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
776  tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
777  }
778  else
779  {
780  newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
781  gtsam::Point2 p = iter->value.cast<gtsam::Point2>();
782  tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll,pitch,yaw)));
783  }
784  }
785  }
786  else
787  {
788  if(key > 0)
789  {
790  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
791  tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
792  }
793  else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
794  {
795  if(isLandmarkWithRotation.at(key))
796  {
797  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
798  tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
799  }
800  else
801  {
802  newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
803  gtsam::Point3 p = iter->value.cast<gtsam::Point3>();
804  tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw)));
805  }
806  }
807  }
808  }
809  }
810  intermediateGraphes->push_back(tmpPoses);
811  }
812 
813  gtsam::ISAM2Result result;
814  double error = 0;
815  try
816  {
817  if(optimizer) // Batch optimization
818  {
819  optimizer->iterate();
820  error = optimizer->error();
821  }
822  else if(i==0) // iSAM2 (add factors)
823  {
824  UDEBUG("Update iSAM with the new factors");
825  result = isam2_->update(graph, initialEstimate, removeFactorIndices);
826 #if BOOST_VERSION >= 106800
827  UASSERT(result.errorBefore.has_value());
828  UASSERT(result.errorAfter.has_value());
829 #else
830  UASSERT(result.errorBefore.is_initialized());
831  UASSERT(result.errorAfter.is_initialized());
832 #endif
833  UDEBUG("error before = %f after=%f", result.errorBefore.value(), result.errorAfter.value());
834  initialError = lastError = result.errorBefore.value();
835  error = result.errorAfter.value();
836  if(!this->isRobust())
837  {
838  UASSERT_MSG(lastAddedConstraints_.size() == result.newFactorsIndices.size(),
839  uFormat("%ld versus %ld", lastAddedConstraints_.size(), result.newFactorsIndices.size()).c_str());
840  for(size_t j=0; j<result.newFactorsIndices.size(); ++j)
841  {
842  UDEBUG("New factor indice: %ld", result.newFactorsIndices[j]);
843  lastAddedConstraints_[j].factorIndice = result.newFactorsIndices[j];
844  }
845  }
846  if(rootId != 0 && lastRootFactorIndex_.first == 0)
847  {
848  UASSERT(result.newFactorsIndices.size()>=1);
849  lastRootFactorIndex_.first = rootId;
850  lastRootFactorIndex_.second = result.newFactorsIndices[0]; // first one should be always the root prior
851  }
852  }
853  else // iSAM2 (more iterations)
854  {
855  result = isam2_->update();
856 #if BOOST_VERSION >= 106800
857  UASSERT(result.errorBefore.has_value());
858  UASSERT(result.errorAfter.has_value());
859 #else
860  UASSERT(result.errorBefore.is_initialized());
861  UASSERT(result.errorAfter.is_initialized());
862 #endif
863  UDEBUG("error before = %f after=%f", result.errorBefore.value(), result.errorAfter.value());
864 
865  lastError = result.errorBefore.value();
866  error = result.errorAfter.value();
867  }
868  ++it;
869  }
870  catch(gtsam::IndeterminantLinearSystemException & e)
871  {
872  UWARN("GTSAM exception caught: %s\n Graph has %d edges and %d vertices", e.what(),
873  (int)newEdgeConstraints.size(),
874  (int)newPoses.size());
875  delete optimizer;
876  if(isam2_)
877  {
878  // We are in bad state, cleanup
879  UDEBUG("Reset iSAM2!");
880  gtsam::ISAM2Params params = isam2_->params();
881  delete isam2_;
882  isam2_ = new gtsam::ISAM2(params);
883  addedPoses_.clear();
884  lastAddedConstraints_.clear();
885  lastRootFactorIndex_.first = 0;
886  lastSwitchId_ = 1000000000;
887  }
888  return optimizedPoses;
889  }
890 
891  // early stop condition
892  UDEBUG("iteration %d error =%f", i+1, error);
893  double errorDelta = lastError - error;
894  if((isam2_ || i>0) && errorDelta < this->epsilon())
895  {
896  if(errorDelta < 0)
897  {
898  UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
899  }
900  else
901  {
902  UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
903  break;
904  }
905  }
906  else if(i==0 && error < this->epsilon())
907  {
908  UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
909  break;
910  }
911  lastError = error;
912  }
913  if(finalError)
914  {
915  *finalError = lastError;
916  }
917  if(iterationsDone)
918  {
919  *iterationsDone = it;
920  }
921  UDEBUG("GTSAM optimizing end (%d iterations done (error initial=%f final=%f), time=%f s)",
922  it, initialError, lastError, timer.ticks());
923 
924  float x,y,z,roll,pitch,yaw;
925  const gtsam::Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
926 #if GTSAM_VERSION_NUMERIC >= 40200
927  for(gtsam::Values::deref_iterator iter=values.begin(); iter!=values.end(); ++iter)
928 #else
929  for(gtsam::Values::const_iterator iter=values.begin(); iter!=values.end(); ++iter)
930 #endif
931  {
932  int key = (int)iter->key;
933  if(iter->value.dim() > 1 && uContains(poses, key))
934  {
935  if(isSlam2d())
936  {
937  if(key > 0)
938  {
939  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
940  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
941  optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
942  }
943  else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
944  {
945  if(isLandmarkWithRotation.at(key))
946  {
947  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
948  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
949  optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
950  }
951  else
952  {
953  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
954  gtsam::Point2 p = iter->value.cast<gtsam::Point2>();
955  optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z,roll,pitch,yaw)));
956  }
957  }
958  }
959  else
960  {
961  if(key > 0)
962  {
963  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
964  optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
965  }
966  else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
967  {
968  if(isLandmarkWithRotation.at(key))
969  {
970  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
971  optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
972  }
973  else
974  {
975  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
976  gtsam::Point3 p = iter->value.cast<gtsam::Point3>();
977  optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw)));
978  }
979  }
980  }
981  }
982  }
983 
984  // compute marginals
985  try {
986  UDEBUG("Computing marginals for node %d...", poses.rbegin()->first);
987  UTimer t;
988  gtsam::Matrix info;
989  if(optimizer)
990  {
991  gtsam::Marginals marginals(graph, optimizer->values());
992  info = marginals.marginalCovariance(poses.rbegin()->first);
993  }
994  else //iSAM2
995  {
996  info = isam2_->marginalCovariance(poses.rbegin()->first);
997  }
998  UDEBUG("Computed marginals = %fs (key=%d)", t.ticks(), poses.rbegin()->first);
999  if(isSlam2d() && info.cols() == 3 && info.cols() == 3)
1000  {
1001  outputCovariance.at<double>(0,0) = info(0,0); // x-x
1002  outputCovariance.at<double>(0,1) = info(0,1); // x-y
1003  outputCovariance.at<double>(0,5) = info(0,2); // x-theta
1004  outputCovariance.at<double>(1,0) = info(1,0); // y-x
1005  outputCovariance.at<double>(1,1) = info(1,1); // y-y
1006  outputCovariance.at<double>(1,5) = info(1,2); // y-theta
1007  outputCovariance.at<double>(5,0) = info(2,0); // theta-x
1008  outputCovariance.at<double>(5,1) = info(2,1); // theta-y
1009  outputCovariance.at<double>(5,5) = info(2,2); // theta-theta
1010  }
1011  else if(!isSlam2d() && info.cols() == 6 && info.cols() == 6)
1012  {
1013  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
1014  mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
1015  mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
1016  mgtsam.block(0,3,3,3) = info.block(3,0,3,3); // off diagonal
1017  mgtsam.block(3,0,3,3) = info.block(0,3,3,3); // off diagonal
1018  memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
1019  }
1020  else
1021  {
1022  UERROR("GTSAM: Could not compute marginal covariance!");
1023  optimizedPoses.clear(); // Failed optimization
1024  }
1025  }
1026  catch(gtsam::IndeterminantLinearSystemException & e)
1027  {
1028  UERROR("GTSAM exception caught: %s", e.what());
1029  optimizedPoses.clear(); // Failed optimization
1030  }
1031  catch(std::exception& e)
1032  {
1033  UERROR("GTSAM exception caught: %s", e.what());
1034  optimizedPoses.clear(); // Failed optimization
1035  }
1036 
1037  delete optimizer;
1038  }
1039  else if(poses.size() == 1 || iterations() <= 0)
1040  {
1041  optimizedPoses = poses;
1042  }
1043  else
1044  {
1045  UWARN("This method should be called at least with 1 pose!");
1046  }
1047  UDEBUG("Optimizing graph...end!");
1048 #else
1049  UERROR("Not built with GTSAM support!");
1050 #endif
1051  return optimizedPoses;
1052 }
1053 
1054 } /* namespace rtabmap */
int
int
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
gtsam::ISAM2::update
virtual ISAM2Result update(const NonlinearFactorGraph &newFactors, const Values &newTheta, const ISAM2UpdateParams &updateParams)
Pose3.h
Pose2.h
gtsam::ISAM2Params
rtabmap::OptimizerGTSAM::isam2_
gtsam::ISAM2 * isam2_
Definition: OptimizerGTSAM.h:64
gtsam::Values::insert
void insert(const Values &values)
UINFO
#define UINFO(...)
gtsam::ISAM2Params::OptimizationParams
std::variant< ISAM2GaussNewtonParams, ISAM2DoglegParams > OptimizationParams
prior
prior
rtabmap::Optimizer::priorsIgnored
bool priorsIgnored() const
Definition: Optimizer.h:95
gtsam::ISAM2
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap::OptimizerGTSAM::internalOptimizerType_
int internalOptimizerType_
Definition: OptimizerGTSAM.h:62
NonlinearOptimizer.h
gtsam::GaussNewtonOptimizer
timer
gtsam::Vector2
Eigen::Vector2d Vector2
gtsam::NonlinearOptimizer::values
const Values & values() const
Values
rtabmap::Optimizer
Definition: Optimizer.h:61
gtsam::ISAM2Result::newFactorsIndices
FactorIndices newFactorsIndices
rtabmap::Optimizer::landmarksIgnored
bool landmarksIgnored() const
Definition: Optimizer.h:96
gtsam::PriorFactor
gtsam::Marginals
gtsam::Values
rtabmap::Transform::theta
float theta() const
Definition: Transform.cpp:162
switchVariableLinear.h
gtsam::Matrix
Eigen::MatrixXd Matrix
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
y
Matrix3f y
gtsam::Vector3
Eigen::Vector3d Vector3
gtsam::noiseModel::Gaussian::shared_ptr
std::shared_ptr< Gaussian > shared_ptr
gtsam::Vector
Eigen::VectorXd Vector
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
Vector2::y
float y
rtabmap::OptimizerGTSAM::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: OptimizerGTSAM.cpp:138
UTimer.h
rtabmap::OptimizerGTSAM::ConstraintToFactor
Definition: OptimizerGTSAM.h:65
UMath.h
Basic mathematics functions.
gtsam::NonlinearOptimizer::error
double error() const
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
gtsam::ISAM2DoglegParams
rtabmap::Optimizer::isSlam2d
bool isSlam2d() const
Definition: Optimizer.h:91
gtsam::DoglegOptimizer
rtabmap::Optimizer::epsilon
double epsilon() const
Definition: Optimizer.h:93
rtabmap::OptimizerGTSAM::lastAddedConstraints_
std::vector< ConstraintToFactor > lastAddedConstraints_
Definition: OptimizerGTSAM.h:77
DoglegOptimizer.h
rtabmap::Transform::y
float & y()
Definition: Transform.h:93
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:407
gtsam::Pose2
LevenbergMarquardtOptimizer.h
handle::cast
T cast() const
gtsam::Values::end
deref_iterator end() const
j
std::ptrdiff_t j
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
UConversion.h
Some conversion functions.
vertigo::BetweenFactorSwitchableLinear
Definition: betweenFactorSwitchable.h:24
rtabmap::OptimizerGTSAM::lastRootFactorIndex_
std::pair< int, std::uint64_t > lastRootFactorIndex_
Definition: OptimizerGTSAM.h:80
gtsam::BearingRangeFactor
gtsam::GaussNewtonParams
gtsam::NonlinearFactorGraph
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
gtsam::noiseModel::Gaussian::Information
static shared_ptr Information(const Matrix &M, bool smart=true)
gtsam::ISAM2Result
rtabmap::OptimizerGTSAM::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: OptimizerGTSAM.cpp:92
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)
gtsam::ISAM2GaussNewtonParams
info
else if n * info
gtsam::LevenbergMarquardtOptimizer
Marginals.h
UASSERT
#define UASSERT(condition)
params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
z
z
Vector2
x
x
error
void error(const char *str)
gtsam::Point2
Vector2 Point2
p
Point3_ p(2)
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
rtabmap::Parameters
Definition: Parameters.h:170
rtabmap::Optimizer::gravitySigma
float gravitySigma() const
Definition: Optimizer.h:97
priorNoise
auto priorNoise
Values.h
XYFactor.h
rtabmap::Transform::toEigen4d
Eigen::Matrix4d toEigen4d() const
Definition: Transform.cpp:381
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
gtsam::NonlinearOptimizer::iterate
virtual GaussianFactorGraph::shared_ptr iterate()=0
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_CORE_EXPORT findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
Definition: Graph.cpp:1024
rtabmap::OptimizerGTSAM::~OptimizerGTSAM
virtual ~OptimizerGTSAM()
Definition: OptimizerGTSAM.cpp:76
graph
FactorGraph< FACTOR > * graph
gtsam::DoglegParams
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
key
const gtsam::Symbol key( 'X', 0)
UWARN
#define UWARN(...)
rtabmap::Optimizer::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Optimizer.cpp:322
gtsam::ISAM2::calculateEstimate
Values calculateEstimate() const
rtabmap::Transform::fromEigen4d
static Transform fromEigen4d(const Eigen::Matrix4d &matrix)
Definition: Transform.cpp:422
gtsam::Values::begin
deref_iterator begin() const
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
gtsam::Pose3
rtabmap::XYZFactor
Definition: XYZFactor.h:24
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ULogger.h
ULogger class and convenient macros.
Key.h
rtabmap::Transform
Definition: Transform.h:41
gtsam::NonlinearOptimizer
Graph.h
gtsam::Values::deref_iterator
vertigo::SwitchVariableLinear
Definition: switchVariableLinear.h:22
values
leaf::MyValues values
gtsam::ISAM2::marginalCovariance
Matrix marginalCovariance(Key key) const
rtabmap::Optimizer::setRobust
void setRobust(bool enabled)
Definition: Optimizer.h:104
rtabmap::OptimizerGTSAM::OptimizerGTSAM
OptimizerGTSAM(const ParametersMap &parameters=ParametersMap())
Definition: OptimizerGTSAM.cpp:66
iter
iterator iter(handle obj)
OptimizerGTSAM.h
gtsam::noiseModel::Diagonal::shared_ptr
std::shared_ptr< Diagonal > shared_ptr
c_str
const char * c_str(Args &&...args)
rtabmap::OptimizerGTSAM::lastSwitchId_
int lastSwitchId_
Definition: OptimizerGTSAM.h:78
rtabmap::Pose3GravityFactor
Definition: GravityFactor.h:177
rtabmap::XYFactor
Definition: XYFactor.h:24
UStl.h
Wrappers of STL for convenient functions.
UDEBUG
#define UDEBUG(...)
betweenFactorSwitchable.h
rtabmap::OptimizerGTSAM::available
static bool available()
Definition: OptimizerGTSAM.cpp:83
UTimer
Definition: UTimer.h:46
GaussNewtonOptimizer.h
gtsam::NonlinearOptimizerParams::relativeErrorTol
double relativeErrorTol
gtsam::LevenbergMarquardtParams
gtsam::NonlinearOptimizerParams::maxIterations
size_t maxIterations
gtsam::Point3
Vector3 Point3
Eigen::Matrix< double, 3, 3 >
XYZFactor.h
ISAM2.h
BetweenFactor.h
GravityFactor.h
Vector2::x
float x
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:90
gtsam::IndeterminantLinearSystemException
gtsam::Unit3
rtabmap::OptimizerGTSAM::addedPoses_
std::set< int > addedPoses_
Definition: OptimizerGTSAM.h:79
incremental
bool incremental
rtabmap::Optimizer::isRobust
bool isRobust() const
Definition: Optimizer.h:94
FactorGraph< NonlinearFactor >::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
t
Point2 t(10, 10)
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:92
marginals
Marginals marginals(graph, result)
rtabmap
Definition: CameraARCore.cpp:35
gtsam::Rot3::RzRyRx
static Rot3 RzRyRx(double x, double y, double z, OptionalJacobian< 3, 1 > Hx={}, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hz={})
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
i
int i
result
RESULT & result
NonlinearFactorGraph.h
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
gtsam::noiseModel::Diagonal::Variances
static shared_ptr Variances(const Vector &variances, bool smart=true)
gtsam::Symbol
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
gtsam::BetweenFactor
gtsam::ISAM2::params
const ISAM2Params & params() const

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