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 "gtsam/GPSPose2XYFactor.h"
55 #include "gtsam/GPSPose3XYZFactor.h"
56 
57 #ifdef RTABMAP_VERTIGO
58 #include "vertigo/gtsam/betweenFactorMaxMix.h"
59 #include "vertigo/gtsam/betweenFactorSwitchable.h"
60 #include "vertigo/gtsam/switchVariableLinear.h"
61 #include "vertigo/gtsam/switchVariableSigmoid.h"
62 #endif
63 #endif // end RTABMAP_GTSAM
64 
65 namespace rtabmap {
66 
67 bool OptimizerGTSAM::available()
68 {
69 #ifdef RTABMAP_GTSAM
70  return true;
71 #else
72  return false;
73 #endif
74 }
75 
76 void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
77 {
78  Optimizer::parseParameters(parameters);
79  Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), optimizer_);
80 }
81 
82 std::map<int, Transform> OptimizerGTSAM::optimize(
83  int rootId,
84  const std::map<int, Transform> & poses,
85  const std::multimap<int, Link> & edgeConstraints,
86  cv::Mat & outputCovariance,
87  std::list<std::map<int, Transform> > * intermediateGraphes,
88  double * finalError,
89  int * iterationsDone)
90 {
91  outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
92  std::map<int, Transform> optimizedPoses;
93 #ifdef RTABMAP_GTSAM
94 
95 #ifndef RTABMAP_VERTIGO
96  if(this->isRobust())
97  {
98  UWARN("Vertigo robust optimization is not available! Robust optimization is now disabled.");
99  setRobust(false);
100  }
101 #endif
102 
103  UDEBUG("Optimizing graph...");
104  if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
105  {
106  gtsam::NonlinearFactorGraph graph;
107 
108  // detect if there is a global pose prior set, if so remove rootId
109  bool gpsPriorOnly = false;
110  bool hasPriorPoses = false;
111  if(!priorsIgnored())
112  {
113  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
114  {
115  if(iter->second.from() == iter->second.to() && iter->second.type() == Link::kPosePrior)
116  {
117  hasPriorPoses = true;
118  if ((isSlam2d() && 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999) ||
119  (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) < 9999.0 &&
120  1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) < 9999.0 &&
121  1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0))
122  {
123  // orientation is set, don't set root prior
124  gpsPriorOnly = false;
125  rootId = 0;
126  break;
127  }
128  else if(gravitySigma()<=0)
129  {
130  gpsPriorOnly = true;
131  }
132  }
133  }
134  }
135 
136  //prior first pose
137  if(rootId != 0)
138  {
139  UASSERT(uContains(poses, rootId));
140  const Transform & initialPose = poses.at(rootId);
141  UDEBUG("hasPriorPoses=%s, gpsPriorOnly=%s", hasPriorPoses?"true":"false", gpsPriorOnly?"true":"false");
142  if(isSlam2d())
143  {
144  gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasPriorPoses?1e-2:std::numeric_limits<double>::min()));
145  graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
146  }
147  else
148  {
149  gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
150  (gtsam::Vector(6) <<
151  1e-2, 1e-2, hasPriorPoses?1e-2:std::numeric_limits<double>::min(), // roll, pitch, fixed yaw if there are no priors
152  (gpsPriorOnly?2:1e-2), gpsPriorOnly?2:1e-2, gpsPriorOnly?2:1e-2 // xyz
153  ).finished());
154  graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
155  }
156  }
157 
158  UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d gpsPriorOnly=%d landmarksIgnored=%d)",
159  rootId, priorsIgnored()?1:0, gpsPriorOnly?1:0, landmarksIgnored()?1:0);
160  gtsam::Values initialEstimate;
161  std::map<int, bool> isLandmarkWithRotation;
162  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
163  {
164  UASSERT(!iter->second.isNull());
165  if(isSlam2d())
166  {
167  if(iter->first > 0)
168  {
169  initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
170  }
171  else if(!landmarksIgnored())
172  {
173  // check if it is SE2 or only PointXY
174  std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
175  UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
176 
177  if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
178  {
179  initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
180  isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
181  }
182  else
183  {
184  initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
185  isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
186  }
187  }
188 
189  }
190  else
191  {
192  if(iter->first > 0)
193  {
194  initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
195  }
196  else if(!landmarksIgnored())
197  {
198  // check if it is SE3 or only PointXYZ
199  std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
200  UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
201 
202  if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
203  1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
204  1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
205  {
206  initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
207  isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
208  }
209  else
210  {
211  initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
212  isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
213  }
214  }
215  }
216  }
217 
218  UDEBUG("fill edges to gtsam...");
219  int switchCounter = poses.rbegin()->first+1;
220  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
221  {
222  int id1 = iter->second.from();
223  int id2 = iter->second.to();
224  UASSERT(!iter->second.transform().isNull());
225  if(id1 == id2)
226  {
227  if(iter->second.type() == Link::kPosePrior && !priorsIgnored())
228  {
229  if(isSlam2d())
230  {
231  if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
232  {
233  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
234  1/iter->second.infMatrix().at<double>(0,0),
235  1/iter->second.infMatrix().at<double>(1,1)));
236  graph.add(GPSPose2XYFactor(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
237  }
238  else
239  {
240  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
241  if(!isCovarianceIgnored())
242  {
243  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
244  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
245  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
246  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
247  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
248  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
249  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
250  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
251  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
252  }
253 
254  gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
255  graph.add(gtsam::PriorFactor<gtsam::Pose2>(id1, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
256  }
257  }
258  else
259  {
260  if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
261  1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
262  1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
263  {
264  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
265  iter->second.infMatrix().at<double>(0,0),
266  iter->second.infMatrix().at<double>(1,1),
267  iter->second.infMatrix().at<double>(2,2)));
268  graph.add(GPSPose3XYZFactor(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
269  }
270  else
271  {
272  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
273  if(!isCovarianceIgnored())
274  {
275  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
276  }
277 
278  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
279  mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
280  mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
281  mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
282  mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
283  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
284 
285  graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
286  }
287  }
288  }
289  else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
290  {
291  Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
292  gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
293  gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), 10));
294  graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1)));
295  }
296  }
297  else if(id1<0 || id2 < 0)
298  {
299  if(!landmarksIgnored())
300  {
301  //landmarks
302  UASSERT((id1 < 0 && id2 > 0) || (id1 > 0 && id2 < 0));
303  Transform t;
304  if(id2 < 0)
305  {
306  t = iter->second.transform();
307  }
308  else
309  {
310  t = iter->second.transform().inverse();
311  std::swap(id1, id2); // should be node -> landmark
312  }
313  if(isSlam2d())
314  {
315  if(isLandmarkWithRotation.at(id2))
316  {
317  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
318  if(!isCovarianceIgnored())
319  {
320  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
321  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
322  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
323  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
324  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
325  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
326  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
327  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
328  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
329  }
330  gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
331  graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(t.x(), t.y(), t.theta()), model));
332  }
333  else
334  {
335  Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
336  if(!isCovarianceIgnored())
337  {
338  cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,2), cv::Range(0,2)).clone();;
339  memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
340  }
341  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);
342 
343  gtsam::Point2 landmark(t.x(), t.y());
344  gtsam::Pose2 p;
345  graph.add(gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2>(id1, id2, p.bearing(landmark), p.range(landmark), model));
346  }
347  }
348  else
349  {
350  if(isLandmarkWithRotation.at(id2))
351  {
352  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
353  if(!isCovarianceIgnored())
354  {
355  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
356  }
357 
358  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
359  mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
360  mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
361  mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
362  mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
363  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
364  graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(t.toEigen4d()), model));
365  }
366  else
367  {
368  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
369  if(!isCovarianceIgnored())
370  {
371  cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();;
372  memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
373  }
374  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);
375 
376  gtsam::Point3 landmark(t.x(), t.y(), t.z());
377  gtsam::Pose3 p;
378  graph.add(gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3>(id1, id2, p.bearing(landmark), p.range(landmark), model));
379  }
380  }
381  }
382  }
383  else // id1 != id2
384  {
385 #ifdef RTABMAP_VERTIGO
386  if(this->isRobust() &&
387  iter->second.type() != Link::kNeighbor &&
388  iter->second.type() != Link::kNeighborMerged)
389  {
390  // create new switch variable
391  // Sunderhauf IROS 2012:
392  // "Since it is reasonable to initially accept all loop closure constraints,
393  // a proper and convenient initial value for all switch variables would be
394  // sij = 1 when using the linear switch function"
395  double prior = 1.0;
396  initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior));
397 
398  // create switch prior factor
399  // "If the front-end is not able to assign sound individual values
400  // for Ξij , it is save to set all Ξij = 1, since this value is close
401  // to the individual optimal choice of Ξij for a large range of
402  // outliers."
403  gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
404  graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
405  }
406 #endif
407 
408  if(isSlam2d())
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  gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
424 
425 #ifdef RTABMAP_VERTIGO
426  if(this->isRobust() &&
427  iter->second.type()!=Link::kNeighbor &&
428  iter->second.type() != Link::kNeighborMerged)
429  {
430  // create switchable edge factor
431  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));
432  }
433  else
434 #endif
435  {
436  graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
437  }
438  }
439  else
440  {
441  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
442  if(!isCovarianceIgnored())
443  {
444  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
445  }
446 
447  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
448  mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
449  mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
450  mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
451  mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
452  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
453 
454 #ifdef RTABMAP_VERTIGO
455  if(this->isRobust() &&
456  iter->second.type() != Link::kNeighbor &&
457  iter->second.type() != Link::kNeighborMerged)
458  {
459  // create switchable edge factor
460  graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
461  }
462  else
463 #endif
464  {
465  graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
466  }
467  }
468  }
469  }
470 
471  UDEBUG("create optimizer");
472  gtsam::NonlinearOptimizer * optimizer;
473 
474  if(optimizer_ == 2)
475  {
476  gtsam::DoglegParams parameters;
477  parameters.relativeErrorTol = epsilon();
478  parameters.maxIterations = iterations();
479  optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters);
480  }
481  else if(optimizer_ == 1)
482  {
483  gtsam::GaussNewtonParams parameters;
484  parameters.relativeErrorTol = epsilon();
485  parameters.maxIterations = iterations();
486  optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters);
487  }
488  else
489  {
490  gtsam::LevenbergMarquardtParams parameters;
491  parameters.relativeErrorTol = epsilon();
492  parameters.maxIterations = iterations();
493  optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);
494  }
495 
496  UDEBUG("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
497  UTimer timer;
498  int it = 0;
499  double lastError = optimizer->error();
500  for(int i=0; i<iterations(); ++i)
501  {
502  if(intermediateGraphes && i > 0)
503  {
504  float x,y,z,roll,pitch,yaw;
505  std::map<int, Transform> tmpPoses;
506  for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
507  {
508  if(iter->value.dim() > 1)
509  {
510  int key = (int)iter->key;
511  if(isSlam2d())
512  {
513  if(key > 0)
514  {
515  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
516  tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
517  }
518  else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
519  {
520  if(isLandmarkWithRotation.at(key))
521  {
522  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
523  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
524  tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
525  }
526  else
527  {
528  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
529  gtsam::Point2 p = iter->value.cast<gtsam::Point2>();
530  tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll,pitch,yaw)));
531  }
532  }
533  }
534  else
535  {
536  if(key > 0)
537  {
538  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
539  tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
540  }
541  else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
542  {
543  if(isLandmarkWithRotation.at(key))
544  {
545  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
546  tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
547  }
548  else
549  {
550  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
551  gtsam::Point3 p = iter->value.cast<gtsam::Point3>();
552  tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw)));
553  }
554  }
555  }
556  }
557  }
558  intermediateGraphes->push_back(tmpPoses);
559  }
560  try
561  {
562  optimizer->iterate();
563  ++it;
564  }
565  catch(gtsam::IndeterminantLinearSystemException & e)
566  {
567  UWARN("GTSAM exception caught: %s\n Graph has %d edges and %d vertices", e.what(),
568  (int)edgeConstraints.size(),
569  (int)poses.size());
570  delete optimizer;
571  return optimizedPoses;
572  }
573 
574  // early stop condition
575  double error = optimizer->error();
576  UDEBUG("iteration %d error =%f", i+1, error);
577  double errorDelta = lastError - error;
578  if(i>0 && errorDelta < this->epsilon())
579  {
580  if(errorDelta < 0)
581  {
582  UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
583  }
584  else
585  {
586  UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
587  break;
588  }
589  }
590  else if(i==0 && error < this->epsilon())
591  {
592  UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
593  break;
594  }
595  lastError = error;
596  }
597  if(finalError)
598  {
599  *finalError = lastError;
600  }
601  if(iterationsDone)
602  {
603  *iterationsDone = it;
604  }
605  UDEBUG("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)",
606  optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
607 
608  float x,y,z,roll,pitch,yaw;
609  for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
610  {
611  if(iter->value.dim() > 1)
612  {
613  int key = (int)iter->key;
614  if(isSlam2d())
615  {
616  if(key > 0)
617  {
618  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
619  optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
620  }
621  else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
622  {
623  if(isLandmarkWithRotation.at(key))
624  {
625  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
626  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
627  optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
628  }
629  else
630  {
631  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
632  gtsam::Point2 p = iter->value.cast<gtsam::Point2>();
633  optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z,roll,pitch,yaw)));
634  }
635  }
636  }
637  else
638  {
639  if(key > 0)
640  {
641  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
642  optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
643  }
644  else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
645  {
646  if(isLandmarkWithRotation.at(key))
647  {
648  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
649  optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
650  }
651  else
652  {
653  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
654  gtsam::Point3 p = iter->value.cast<gtsam::Point3>();
655  optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw)));
656  }
657  }
658  }
659  }
660  }
661 
662  // compute marginals
663  try {
664  UDEBUG("Computing marginals...");
665  UTimer t;
666  gtsam::Marginals marginals(graph, optimizer->values());
667  gtsam::Matrix info = marginals.marginalCovariance(poses.rbegin()->first);
668  UDEBUG("Computed marginals = %fs (key=%d)", t.ticks(), poses.rbegin()->first);
669  if(isSlam2d() && info.cols() == 3 && info.cols() == 3)
670  {
671  outputCovariance.at<double>(0,0) = info(0,0); // x-x
672  outputCovariance.at<double>(0,1) = info(0,1); // x-y
673  outputCovariance.at<double>(0,5) = info(0,2); // x-theta
674  outputCovariance.at<double>(1,0) = info(1,0); // y-x
675  outputCovariance.at<double>(1,1) = info(1,1); // y-y
676  outputCovariance.at<double>(1,5) = info(1,2); // y-theta
677  outputCovariance.at<double>(5,0) = info(2,0); // theta-x
678  outputCovariance.at<double>(5,1) = info(2,1); // theta-y
679  outputCovariance.at<double>(5,5) = info(2,2); // theta-theta
680  }
681  else if(!isSlam2d() && info.cols() == 6 && info.cols() == 6)
682  {
683  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
684  mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
685  mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
686  mgtsam.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
687  mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
688  memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
689  }
690  else
691  {
692  UWARN("GTSAM: Could not compute marginal covariance!");
693  }
694  }
695  catch(gtsam::IndeterminantLinearSystemException & e)
696  {
697  UWARN("GTSAM exception caught: %s", e.what());
698  }
699  catch(std::exception& e)
700  {
701  UWARN("GTSAM exception caught: %s", e.what());
702  }
703 
704  delete optimizer;
705  }
706  else if(poses.size() == 1 || iterations() <= 0)
707  {
708  optimizedPoses = poses;
709  }
710  else
711  {
712  UWARN("This method should be called at least with 1 pose!");
713  }
714  UDEBUG("Optimizing graph...end!");
715 #else
716  UERROR("Not built with GTSAM support!");
717 #endif
718  return optimizedPoses;
719 }
720 
721 } /* namespace rtabmap */
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::Optimizer::priorsIgnored
bool priorsIgnored() const
Definition: Optimizer.h:91
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:476
rtabmap::Optimizer::gravitySigma
float gravitySigma() const
Definition: Optimizer.h:93
rtabmap::OptimizerGTSAM::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: OptimizerGTSAM.cpp:76
UTimer
Definition: UTimer.h:46
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::Optimizer::isSlam2d
bool isSlam2d() const
Definition: Optimizer.h:87
rtabmap::Transform::toEigen4d
Eigen::Matrix4d toEigen4d() const
Definition: Transform.cpp:350
glm::e
GLM_FUNC_DECL genType e()
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:88
UMath.h
Basic mathematics functions.
UConversion.h
Some conversion functions.
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Transform::theta
float theta() const
Definition: Transform.cpp:162
rtabmap::Optimizer::landmarksIgnored
bool landmarksIgnored() const
Definition: Optimizer.h:92
rtabmap::Optimizer::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Optimizer.cpp:321
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::Optimizer::epsilon
double epsilon() const
Definition: Optimizer.h:89
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:86
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
rtabmap::Optimizer::isRobust
bool isRobust() const
Definition: Optimizer.h:90
rtabmap::Transform::fromEigen4d
static Transform fromEigen4d(const Eigen::Matrix4d &matrix)
Definition: Transform.cpp:391
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
rtabmap::Optimizer::setRobust
void setRobust(bool enabled)
Definition: Optimizer.h:100
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
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:82
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
glm::rotate
GLM_FUNC_DECL detail::tmat4x4< T, P > rotate(detail::tmat4x4< T, P > const &m, T const &angle, detail::tvec3< T, P > const &axis)
UINFO
#define UINFO(...)

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