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