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/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/nonlinear/NonlinearFactorGraph.h>
46 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
47 #include <gtsam/nonlinear/DoglegOptimizer.h>
48 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
49 #include <gtsam/nonlinear/NonlinearOptimizer.h>
50 #include <gtsam/nonlinear/Marginals.h>
51 #include <gtsam/nonlinear/Values.h>
52 
53 #ifdef RTABMAP_VERTIGO
54 #include "vertigo/gtsam/betweenFactorMaxMix.h"
55 #include "vertigo/gtsam/betweenFactorSwitchable.h"
56 #include "vertigo/gtsam/switchVariableLinear.h"
57 #include "vertigo/gtsam/switchVariableSigmoid.h"
58 #endif
59 #endif // end RTABMAP_GTSAM
60 
61 namespace rtabmap {
62 
63 bool OptimizerGTSAM::available()
64 {
65 #ifdef RTABMAP_GTSAM
66  return true;
67 #else
68  return false;
69 #endif
70 }
71 
72 void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
73 {
74  Optimizer::parseParameters(parameters);
75  Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), optimizer_);
76 }
77 
78 std::map<int, Transform> OptimizerGTSAM::optimize(
79  int rootId,
80  const std::map<int, Transform> & poses,
81  const std::multimap<int, Link> & edgeConstraints,
82  cv::Mat & outputCovariance,
83  std::list<std::map<int, Transform> > * intermediateGraphes,
84  double * finalError,
85  int * iterationsDone)
86 {
87  outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
88  std::map<int, Transform> optimizedPoses;
89 #ifdef RTABMAP_GTSAM
90 
91 #ifndef RTABMAP_VERTIGO
92  if(this->isRobust())
93  {
94  UWARN("Vertigo robust optimization is not available! Robust optimization is now disabled.");
95  setRobust(false);
96  }
97 #endif
98 
99  UDEBUG("Optimizing graph...");
100  if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
101  {
102  gtsam::NonlinearFactorGraph graph;
103 
104  // detect if there is a global pose prior set, if so remove rootId
105  if(!priorsIgnored())
106  {
107  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
108  {
109  if(iter->second.from() == iter->second.to())
110  {
111  rootId = 0;
112  break;
113  }
114  }
115  }
116 
117  //prior first pose
118  if(rootId != 0)
119  {
120  UASSERT(uContains(poses, rootId));
121  const Transform & initialPose = poses.at(rootId);
122  if(isSlam2d())
123  {
124  gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, 0.01));
125  graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
126  }
127  else
128  {
129  gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
130  graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
131  }
132  }
133 
134  UDEBUG("fill poses to gtsam...");
135  gtsam::Values initialEstimate;
136  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
137  {
138  UASSERT(!iter->second.isNull());
139  if(isSlam2d())
140  {
141  initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
142  }
143  else
144  {
145  initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
146  }
147  }
148 
149  UDEBUG("fill edges to gtsam...");
150  int switchCounter = poses.rbegin()->first+1;
151  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
152  {
153  int id1 = iter->second.from();
154  int id2 = iter->second.to();
155  UASSERT(!iter->second.transform().isNull());
156  if(id1 == id2)
157  {
158  if(!priorsIgnored())
159  {
160  if(isSlam2d())
161  {
162  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
163  if(!isCovarianceIgnored())
164  {
165  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
166  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
167  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
168  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
169  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
170  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
171  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
172  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
173  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
174  }
175 
176  gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
177  graph.add(gtsam::PriorFactor<gtsam::Pose2>(id1, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
178  }
179  else
180  {
181  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
182  if(!isCovarianceIgnored())
183  {
184  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
185  }
186 
187  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
188  mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
189  mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
190  mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
191  mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
192  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
193 
194  graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
195  }
196  }
197  }
198  else
199  {
200 #ifdef RTABMAP_VERTIGO
201  if(this->isRobust() &&
202  iter->second.type() != Link::kNeighbor &&
203  iter->second.type() != Link::kNeighborMerged &&
204  iter->second.type() != Link::kPosePrior)
205  {
206  // create new switch variable
207  // Sunderhauf IROS 2012:
208  // "Since it is reasonable to initially accept all loop closure constraints,
209  // a proper and convenient initial value for all switch variables would be
210  // sij = 1 when using the linear switch function"
211  double prior = 1.0;
212  initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior));
213 
214  // create switch prior factor
215  // "If the front-end is not able to assign sound individual values
216  // for Ξij , it is save to set all Ξij = 1, since this value is close
217  // to the individual optimal choice of Ξij for a large range of
218  // outliers."
219  gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
220  graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
221  }
222 #endif
223 
224  if(isSlam2d())
225  {
226  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
227  if(!isCovarianceIgnored())
228  {
229  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
230  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
231  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
232  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
233  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
234  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
235  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
236  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
237  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
238  }
239  gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
240 
241 #ifdef RTABMAP_VERTIGO
242  if(this->isRobust() &&
243  iter->second.type()!=Link::kNeighbor &&
244  iter->second.type() != Link::kNeighborMerged)
245  {
246  // create switchable edge factor
247  graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
248  }
249  else
250 #endif
251  {
252  graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
253  }
254  }
255  else
256  {
257  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
258  if(!isCovarianceIgnored())
259  {
260  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
261  }
262 
263  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
264  mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
265  mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
266  mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
267  mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
268  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
269 
270 #ifdef RTABMAP_VERTIGO
271  if(this->isRobust() &&
272  iter->second.type() != Link::kNeighbor &&
273  iter->second.type() != Link::kNeighborMerged &&
274  iter->second.type() != Link::kPosePrior)
275  {
276  // create switchable edge factor
277  graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
278  }
279  else
280 #endif
281  {
282  graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
283  }
284  }
285  }
286  }
287 
288  UDEBUG("create optimizer");
289  gtsam::NonlinearOptimizer * optimizer;
290 
291  if(optimizer_ == 2)
292  {
293  gtsam::DoglegParams parameters;
294  parameters.relativeErrorTol = epsilon();
295  parameters.maxIterations = iterations();
296  optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters);
297  }
298  else if(optimizer_ == 1)
299  {
300  gtsam::GaussNewtonParams parameters;
301  parameters.relativeErrorTol = epsilon();
302  parameters.maxIterations = iterations();
303  optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters);
304  }
305  else
306  {
307  gtsam::LevenbergMarquardtParams parameters;
308  parameters.relativeErrorTol = epsilon();
309  parameters.maxIterations = iterations();
310  optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);
311  }
312 
313  UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
314  UTimer timer;
315  int it = 0;
316  double lastError = optimizer->error();
317  for(int i=0; i<iterations(); ++i)
318  {
319  if(intermediateGraphes && i > 0)
320  {
321  std::map<int, Transform> tmpPoses;
322  for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
323  {
324  if(iter->value.dim() > 1)
325  {
326  if(isSlam2d())
327  {
328  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
329  tmpPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
330  }
331  else
332  {
333  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
334  tmpPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
335  }
336  }
337  }
338  intermediateGraphes->push_back(tmpPoses);
339  }
340  try
341  {
342  optimizer->iterate();
343  ++it;
344  }
345  catch(gtsam::IndeterminantLinearSystemException & e)
346  {
347  UERROR("GTSAM exception caught: %s", e.what());
348  delete optimizer;
349  return optimizedPoses;
350  }
351 
352  // early stop condition
353  double error = optimizer->error();
354  UDEBUG("iteration %d error =%f", i+1, error);
355  double errorDelta = lastError - error;
356  if(i>0 && errorDelta < this->epsilon())
357  {
358  if(errorDelta < 0)
359  {
360  UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
361  }
362  else
363  {
364  UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
365  break;
366  }
367  }
368  else if(i==0 && error < this->epsilon())
369  {
370  UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
371  break;
372  }
373  lastError = error;
374  }
375  if(finalError)
376  {
377  *finalError = lastError;
378  }
379  if(iterationsDone)
380  {
381  *iterationsDone = it;
382  }
383  UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)",
384  optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
385 
386  gtsam::Marginals marginals(graph, optimizer->values());
387  for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
388  {
389  if(iter->value.dim() > 1)
390  {
391  if(isSlam2d())
392  {
393  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
394  optimizedPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
395  }
396  else
397  {
398  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
399  optimizedPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
400  }
401  }
402  }
403 
404  // compute marginals
405  try {
406  UTimer t;
407  gtsam::Marginals marginals(graph, optimizer->values());
408  gtsam::Matrix info = marginals.marginalCovariance(optimizer->values().rbegin()->key);
409  UINFO("Computed marginals = %fs (key=%d)", t.ticks(), optimizer->values().rbegin()->key);
410  if(isSlam2d())
411  {
412  UASSERT(info.cols() == 3 && info.cols() == 3);
413  outputCovariance.at<double>(0,0) = info(0,0); // x-x
414  outputCovariance.at<double>(0,1) = info(0,1); // x-y
415  outputCovariance.at<double>(0,5) = info(0,2); // x-theta
416  outputCovariance.at<double>(1,0) = info(1,0); // y-x
417  outputCovariance.at<double>(1,1) = info(1,1); // y-y
418  outputCovariance.at<double>(1,5) = info(1,2); // y-theta
419  outputCovariance.at<double>(5,0) = info(2,0); // theta-x
420  outputCovariance.at<double>(5,1) = info(2,1); // theta-y
421  outputCovariance.at<double>(5,5) = info(2,2); // theta-theta
422  }
423  else
424  {
425  UASSERT(info.cols() == 6 && info.cols() == 6);
426  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
427  mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
428  mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
429  mgtsam.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
430  mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
431  memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
432  }
433  } catch(std::exception& e) {
434  cout << e.what() << endl;
435  }
436 
437  delete optimizer;
438  }
439  else if(poses.size() == 1 || iterations() <= 0)
440  {
441  optimizedPoses = poses;
442  }
443  else
444  {
445  UWARN("This method should be called at least with 1 pose!");
446  }
447  UDEBUG("Optimizing graph...end!");
448 #else
449  UERROR("Not built with GTSAM support!");
450 #endif
451  return optimizedPoses;
452 }
453 
454 } /* namespace rtabmap */
rtabmap::Optimizer::priorsIgnored
bool priorsIgnored() const
Definition: Optimizer.h:78
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:446
rtabmap::OptimizerGTSAM::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: OptimizerGTSAM.cpp:72
UTimer
Definition: UTimer.h:46
rtabmap::Optimizer::isSlam2d
bool isSlam2d() const
Definition: Optimizer.h:74
rtabmap::Transform::toEigen4d
Eigen::Matrix4d toEigen4d() const
Definition: Transform.cpp:334
glm::e
GLM_FUNC_DECL genType e()
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:75
UMath.h
Basic mathematics functions.
UConversion.h
Some conversion functions.
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Transform::theta
float theta() const
Definition: Transform.cpp:162
rtabmap::Optimizer::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Optimizer.cpp:256
rtabmap::Optimizer::epsilon
double epsilon() const
Definition: Optimizer.h:76
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:73
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::Optimizer::isRobust
bool isRobust() const
Definition: Optimizer.h:77
rtabmap::Transform::fromEigen4d
static Transform fromEigen4d(const Eigen::Matrix4d &matrix)
Definition: Transform.cpp:375
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
rtabmap::Optimizer::setRobust
void setRobust(bool enabled)
Definition: Optimizer.h:85
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
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:78
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:32