OptimizerGTSAM.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 #include "rtabmap/core/Graph.h"
28 
29 #include <rtabmap/utilite/ULogger.h>
30 #include <rtabmap/utilite/UStl.h>
31 #include <rtabmap/utilite/UMath.h>
32 #include <rtabmap/utilite/UConversion.h>
33 #include <rtabmap/utilite/UTimer.h>
34 #include <set>
35 
36 #include <rtabmap/core/optimizer/OptimizerGTSAM.h>
37 
38 #ifdef RTABMAP_GTSAM
39 #include <gtsam/geometry/Pose2.h>
40 #include <gtsam/geometry/Pose3.h>
41 #include <gtsam/inference/Key.h>
42 #include <gtsam/inference/Symbol.h>
43 #include <gtsam/slam/PriorFactor.h>
44 #include <gtsam/slam/BetweenFactor.h>
45 #include <gtsam/sam/BearingRangeFactor.h>
46 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
47 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
48 #include <gtsam/nonlinear/DoglegOptimizer.h>
49 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
50 #include <gtsam/nonlinear/NonlinearOptimizer.h>
51 #include <gtsam/nonlinear/Marginals.h>
52 #include <gtsam/nonlinear/Values.h>
53 #include "gtsam/GravityFactor.h"
54 #include <optimizer/gtsam/XYFactor.h>
55 #include <optimizer/gtsam/XYZFactor.h>
56 
57 #ifdef RTABMAP_VERTIGO
58 #include "vertigo/gtsam/betweenFactorSwitchable.h"
59 #include "vertigo/gtsam/switchVariableLinear.h"
60 #endif
61 #endif // end RTABMAP_GTSAM
62 
63 namespace rtabmap {
64 
65 bool OptimizerGTSAM::available()
66 {
67 #ifdef RTABMAP_GTSAM
68  return true;
69 #else
70  return false;
71 #endif
72 }
73 
74 void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
75 {
76  Optimizer::parseParameters(parameters);
77  Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), optimizer_);
78 }
79 
80 std::map<int, Transform> OptimizerGTSAM::optimize(
81  int rootId,
82  const std::map<int, Transform> & poses,
83  const std::multimap<int, Link> & edgeConstraints,
84  cv::Mat & outputCovariance,
85  std::list<std::map<int, Transform> > * intermediateGraphes,
86  double * finalError,
87  int * iterationsDone)
88 {
89  outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
90  std::map<int, Transform> optimizedPoses;
91 #ifdef RTABMAP_GTSAM
92 
93 #ifndef RTABMAP_VERTIGO
94  if(this->isRobust())
95  {
96  UWARN("Vertigo robust optimization is not available! Robust optimization is now disabled.");
97  setRobust(false);
98  }
99 #endif
100 
101  UDEBUG("Optimizing graph...");
102  if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
103  {
104  gtsam::NonlinearFactorGraph graph;
105 
106  // detect if there is a global pose prior set, if so remove rootId
107  bool hasGPSPrior = false;
108  bool hasGravityConstraints = false;
109  if(!priorsIgnored() || (!isSlam2d() && gravitySigma() > 0))
110  {
111  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
112  {
113  if(iter->second.from() == iter->second.to())
114  {
115  if(!priorsIgnored() && iter->second.type() == Link::kPosePrior)
116  {
117  hasGPSPrior = 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 (it is no GPS)
124  rootId = 0;
125  hasGPSPrior = false;
126  break;
127  }
128  }
129  if(iter->second.type() == Link::kGravity)
130  {
131  hasGravityConstraints = true;
132  if(priorsIgnored())
133  {
134  break;
135  }
136  }
137  }
138  }
139  }
140 
141  //prior first pose
142  if(rootId != 0)
143  {
144  UASSERT(uContains(poses, rootId));
145  const Transform & initialPose = poses.at(rootId);
146  UDEBUG("hasGPSPrior=%s", hasGPSPrior?"true":"false");
147  if(isSlam2d())
148  {
149  gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:std::numeric_limits<double>::min()));
150  graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
151  }
152  else
153  {
154  gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
155  (gtsam::Vector(6) <<
156  (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
157  (hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz
158  ).finished());
159  graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
160  }
161  }
162 
163  UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d landmarksIgnored=%d)",
164  rootId, priorsIgnored()?1:0, landmarksIgnored()?1:0);
165  gtsam::Values initialEstimate;
166  std::map<int, bool> isLandmarkWithRotation;
167  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
168  {
169  UASSERT(!iter->second.isNull());
170  if(isSlam2d())
171  {
172  if(iter->first > 0)
173  {
174  initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
175  }
176  else if(!landmarksIgnored())
177  {
178  // check if it is SE2 or only PointXY
179  std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
180  UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
181 
182  if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
183  {
184  initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
185  isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
186  }
187  else
188  {
189  initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
190  isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
191  }
192  }
193 
194  }
195  else
196  {
197  if(iter->first > 0)
198  {
199  initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
200  }
201  else if(!landmarksIgnored())
202  {
203  // check if it is SE3 or only PointXYZ
204  std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
205  UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
206 
207  if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
208  1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
209  1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
210  {
211  initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
212  isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
213  }
214  else
215  {
216  initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
217  isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
218  }
219  }
220  }
221  }
222 
223  UDEBUG("fill edges to gtsam...");
224  int switchCounter = poses.rbegin()->first+1;
225  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
226  {
227  int id1 = iter->second.from();
228  int id2 = iter->second.to();
229 
230  UASSERT_MSG(initialEstimate.find(id1)!=initialEstimate.end(), uFormat("id1=%d", id1).c_str());
231  UASSERT_MSG(initialEstimate.find(id2)!=initialEstimate.end(), uFormat("id2=%d", id2).c_str());
232 
233  UASSERT(!iter->second.transform().isNull());
234  if(id1 == id2)
235  {
236  if(iter->second.type() == Link::kPosePrior && !priorsIgnored() &&
237  (!landmarksIgnored() || id1>0))
238  {
239  if(isSlam2d())
240  {
241  if(id1 < 0 && !isLandmarkWithRotation.at(id1))
242  {
243  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
244  1/iter->second.infMatrix().at<double>(0,0),
245  1/iter->second.infMatrix().at<double>(1,1)));
246  graph.add(XYFactor<gtsam::Point2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
247  }
248  else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
249  {
250  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
251  1/iter->second.infMatrix().at<double>(0,0),
252  1/iter->second.infMatrix().at<double>(1,1)));
253  graph.add(XYFactor<gtsam::Pose2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
254  }
255  else
256  {
257  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
258  if(!isCovarianceIgnored())
259  {
260  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
261  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
262  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
263  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
264  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
265  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
266  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
267  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
268  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
269  }
270 
271  gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
272  graph.add(gtsam::PriorFactor<gtsam::Pose2>(id1, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
273  }
274  }
275  else
276  {
277  if(id1 < 0 && !isLandmarkWithRotation.at(id1))
278  {
279  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
280  iter->second.infMatrix().at<double>(0,0),
281  iter->second.infMatrix().at<double>(1,1),
282  iter->second.infMatrix().at<double>(2,2)));
283  graph.add(XYZFactor<gtsam::Point3>(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
284  }
285  else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
286  1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
287  1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
288  {
289  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
290  iter->second.infMatrix().at<double>(0,0),
291  iter->second.infMatrix().at<double>(1,1),
292  iter->second.infMatrix().at<double>(2,2)));
293  graph.add(XYZFactor<gtsam::Pose3>(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
294  }
295  else
296  {
297  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
298  if(!isCovarianceIgnored())
299  {
300  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
301  }
302 
303  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
304  mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
305  mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
306  mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
307  mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
308  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
309 
310  graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
311  }
312  }
313  }
314  else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
315  {
316  Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
317  gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
318  gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), 10));
319  graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1)));
320  }
321  }
322  else if(id1<0 || id2 < 0)
323  {
324  if(!landmarksIgnored())
325  {
326  //landmarks
327  UASSERT((id1 < 0 && id2 > 0) || (id1 > 0 && id2 < 0));
328  Transform t;
329  if(id2 < 0)
330  {
331  t = iter->second.transform();
332  }
333  else
334  {
335  t = iter->second.transform().inverse();
336  std::swap(id1, id2); // should be node -> landmark
337  }
338  if(isSlam2d())
339  {
340  if(isLandmarkWithRotation.at(id2))
341  {
342  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
343  if(!isCovarianceIgnored())
344  {
345  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
346  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
347  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
348  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
349  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
350  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
351  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
352  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
353  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
354  }
355  gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
356  graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(t.x(), t.y(), t.theta()), model));
357  }
358  else
359  {
360  Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
361  if(!isCovarianceIgnored())
362  {
363  cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,2), cv::Range(0,2)).clone();;
364  memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
365  }
366  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);
367 
368  gtsam::Point2 landmark(t.x(), t.y());
369  gtsam::Pose2 p;
370  graph.add(gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2>(id1, id2, p.bearing(landmark), p.range(landmark), model));
371  }
372  }
373  else
374  {
375  if(isLandmarkWithRotation.at(id2))
376  {
377  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
378  if(!isCovarianceIgnored())
379  {
380  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
381  }
382 
383  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
384  mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
385  mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
386  mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
387  mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
388  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
389  graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(t.toEigen4d()), model));
390  }
391  else
392  {
393  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
394  if(!isCovarianceIgnored())
395  {
396  cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();;
397  memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
398  }
399  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);
400 
401  gtsam::Point3 landmark(t.x(), t.y(), t.z());
402  gtsam::Pose3 p;
403  graph.add(gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3>(id1, id2, p.bearing(landmark), p.range(landmark), model));
404  }
405  }
406  }
407  }
408  else // id1 != id2
409  {
410 #ifdef RTABMAP_VERTIGO
411  if(this->isRobust() &&
412  iter->second.type() != Link::kNeighbor &&
413  iter->second.type() != Link::kNeighborMerged)
414  {
415  // create new switch variable
416  // Sunderhauf IROS 2012:
417  // "Since it is reasonable to initially accept all loop closure constraints,
418  // a proper and convenient initial value for all switch variables would be
419  // sij = 1 when using the linear switch function"
420  double prior = 1.0;
421  initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior));
422 
423  // create switch prior factor
424  // "If the front-end is not able to assign sound individual values
425  // for Ξij , it is save to set all Ξij = 1, since this value is close
426  // to the individual optimal choice of Ξij for a large range of
427  // outliers."
428  gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
429  graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
430  }
431 #endif
432  if(isSlam2d())
433  {
434  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
435  if(!isCovarianceIgnored())
436  {
437  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
438  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
439  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
440  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
441  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
442  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
443  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
444  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
445  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
446  }
447  gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
448 
449 #ifdef RTABMAP_VERTIGO
450  if(this->isRobust() &&
451  iter->second.type()!=Link::kNeighbor &&
452  iter->second.type() != Link::kNeighborMerged)
453  {
454  // create switchable edge factor
455  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));
456  }
457  else
458 #endif
459  {
460  graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
461  }
462  }
463  else
464  {
465  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
466  if(!isCovarianceIgnored())
467  {
468  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
469  }
470 
471  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
472  mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
473  mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
474  mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
475  mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
476  gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
477 
478 #ifdef RTABMAP_VERTIGO
479  if(this->isRobust() &&
480  iter->second.type() != Link::kNeighbor &&
481  iter->second.type() != Link::kNeighborMerged)
482  {
483  // create switchable edge factor
484  graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
485  }
486  else
487 #endif
488  {
489  graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
490  }
491  }
492  }
493  }
494 
495  UDEBUG("create optimizer");
496  gtsam::NonlinearOptimizer * optimizer;
497 
498  if(optimizer_ == 2)
499  {
500  gtsam::DoglegParams parameters;
501  parameters.relativeErrorTol = epsilon();
502  parameters.maxIterations = iterations();
503  optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters);
504  }
505  else if(optimizer_ == 1)
506  {
507  gtsam::GaussNewtonParams parameters;
508  parameters.relativeErrorTol = epsilon();
509  parameters.maxIterations = iterations();
510  optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters);
511  }
512  else
513  {
514  gtsam::LevenbergMarquardtParams parameters;
515  parameters.relativeErrorTol = epsilon();
516  parameters.maxIterations = iterations();
517  optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);
518  }
519 
520  UDEBUG("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
521  UTimer timer;
522  int it = 0;
523  double lastError = optimizer->error();
524  for(int i=0; i<iterations(); ++i)
525  {
526  if(intermediateGraphes && i > 0)
527  {
528  float x,y,z,roll,pitch,yaw;
529  std::map<int, Transform> tmpPoses;
530  for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
531  {
532  if(iter->value.dim() > 1)
533  {
534  int key = (int)iter->key;
535  if(isSlam2d())
536  {
537  if(key > 0)
538  {
539  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
540  tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
541  }
542  else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
543  {
544  if(isLandmarkWithRotation.at(key))
545  {
546  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
547  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
548  tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
549  }
550  else
551  {
552  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
553  gtsam::Point2 p = iter->value.cast<gtsam::Point2>();
554  tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll,pitch,yaw)));
555  }
556  }
557  }
558  else
559  {
560  if(key > 0)
561  {
562  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
563  tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
564  }
565  else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
566  {
567  if(isLandmarkWithRotation.at(key))
568  {
569  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
570  tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
571  }
572  else
573  {
574  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
575  gtsam::Point3 p = iter->value.cast<gtsam::Point3>();
576  tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw)));
577  }
578  }
579  }
580  }
581  }
582  intermediateGraphes->push_back(tmpPoses);
583  }
584  try
585  {
586  optimizer->iterate();
587  ++it;
588  }
589  catch(gtsam::IndeterminantLinearSystemException & e)
590  {
591  UWARN("GTSAM exception caught: %s\n Graph has %d edges and %d vertices", e.what(),
592  (int)edgeConstraints.size(),
593  (int)poses.size());
594  delete optimizer;
595  return optimizedPoses;
596  }
597 
598  // early stop condition
599  double error = optimizer->error();
600  UDEBUG("iteration %d error =%f", i+1, error);
601  double errorDelta = lastError - error;
602  if(i>0 && errorDelta < this->epsilon())
603  {
604  if(errorDelta < 0)
605  {
606  UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
607  }
608  else
609  {
610  UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
611  break;
612  }
613  }
614  else if(i==0 && error < this->epsilon())
615  {
616  UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
617  break;
618  }
619  lastError = error;
620  }
621  if(finalError)
622  {
623  *finalError = lastError;
624  }
625  if(iterationsDone)
626  {
627  *iterationsDone = it;
628  }
629  UDEBUG("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)",
630  optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
631 
632  float x,y,z,roll,pitch,yaw;
633  for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
634  {
635  if(iter->value.dim() > 1)
636  {
637  int key = (int)iter->key;
638  if(isSlam2d())
639  {
640  if(key > 0)
641  {
642  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
643  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
644  optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
645  }
646  else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
647  {
648  if(isLandmarkWithRotation.at(key))
649  {
650  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
651  gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
652  optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
653  }
654  else
655  {
656  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
657  gtsam::Point2 p = iter->value.cast<gtsam::Point2>();
658  optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z,roll,pitch,yaw)));
659  }
660  }
661  }
662  else
663  {
664  if(key > 0)
665  {
666  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
667  optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
668  }
669  else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
670  {
671  if(isLandmarkWithRotation.at(key))
672  {
673  gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
674  optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
675  }
676  else
677  {
678  poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
679  gtsam::Point3 p = iter->value.cast<gtsam::Point3>();
680  optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw)));
681  }
682  }
683  }
684  }
685  }
686 
687  // compute marginals
688  try {
689  UDEBUG("Computing marginals...");
690  UTimer t;
691  gtsam::Marginals marginals(graph, optimizer->values());
692  gtsam::Matrix info = marginals.marginalCovariance(poses.rbegin()->first);
693  UDEBUG("Computed marginals = %fs (key=%d)", t.ticks(), poses.rbegin()->first);
694  if(isSlam2d() && info.cols() == 3 && info.cols() == 3)
695  {
696  outputCovariance.at<double>(0,0) = info(0,0); // x-x
697  outputCovariance.at<double>(0,1) = info(0,1); // x-y
698  outputCovariance.at<double>(0,5) = info(0,2); // x-theta
699  outputCovariance.at<double>(1,0) = info(1,0); // y-x
700  outputCovariance.at<double>(1,1) = info(1,1); // y-y
701  outputCovariance.at<double>(1,5) = info(1,2); // y-theta
702  outputCovariance.at<double>(5,0) = info(2,0); // theta-x
703  outputCovariance.at<double>(5,1) = info(2,1); // theta-y
704  outputCovariance.at<double>(5,5) = info(2,2); // theta-theta
705  }
706  else if(!isSlam2d() && info.cols() == 6 && info.cols() == 6)
707  {
708  Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
709  mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
710  mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
711  mgtsam.block(0,3,3,3) = info.block(3,0,3,3); // off diagonal
712  mgtsam.block(3,0,3,3) = info.block(0,3,3,3); // off diagonal
713  memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
714  }
715  else
716  {
717  UERROR("GTSAM: Could not compute marginal covariance!");
718  optimizedPoses.clear(); // Failed optimization
719  }
720  }
721  catch(gtsam::IndeterminantLinearSystemException & e)
722  {
723  UERROR("GTSAM exception caught: %s", e.what());
724  optimizedPoses.clear(); // Failed optimization
725  }
726  catch(std::exception& e)
727  {
728  UERROR("GTSAM exception caught: %s", e.what());
729  optimizedPoses.clear(); // Failed optimization
730  }
731 
732  delete optimizer;
733  }
734  else if(poses.size() == 1 || iterations() <= 0)
735  {
736  optimizedPoses = poses;
737  }
738  else
739  {
740  UWARN("This method should be called at least with 1 pose!");
741  }
742  UDEBUG("Optimizing graph...end!");
743 #else
744  UERROR("Not built with GTSAM support!");
745 #endif
746  return optimizedPoses;
747 }
748 
749 } /* namespace rtabmap */
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
rtabmap::OptimizerGTSAM::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: OptimizerGTSAM.cpp:74
UTimer
Definition: UTimer.h:46
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
x
x
rtabmap::Optimizer::isSlam2d
bool isSlam2d() const
Definition: Optimizer.h:92
glm::e
GLM_FUNC_DECL genType e()
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
std::swap
void swap(linb::any &lhs, linb::any &rhs) noexcept
rtabmap::Optimizer::epsilon
double epsilon() const
Definition: Optimizer.h:94
UMath.h
Basic mathematics functions.
UConversion.h
Some conversion functions.
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
prior
prior
rtabmap::Optimizer::gravitySigma
float gravitySigma() const
Definition: Optimizer.h:98
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Optimizer::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Optimizer.cpp:329
rtabmap::Optimizer::landmarksIgnored
bool landmarksIgnored() const
Definition: Optimizer.h:97
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:91
rtabmap::Transform::toEigen4d
Eigen::Matrix4d toEigen4d() const
Definition: Transform.cpp:371
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::Transform::fromEigen4d
static Transform fromEigen4d(const Eigen::Matrix4d &matrix)
Definition: Transform.cpp:412
UDEBUG
#define UDEBUG(...)
rtabmap::Optimizer::isRobust
bool isRobust() const
Definition: Optimizer.h:95
UERROR
#define UERROR(...)
rtabmap::Optimizer::setRobust
void setRobust(bool enabled)
Definition: Optimizer.h:105
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:80
trace.model
model
Definition: trace.py:4
rtabmap::Transform::theta
float theta() const
Definition: Transform.cpp:162
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:93
rtabmap::Optimizer::priorsIgnored
bool priorsIgnored() const
Definition: Optimizer.h:96
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
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 Jan 23 2023 03:37:29