ShonanAveraging.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #pragma once
20 
21 #include <gtsam/base/Matrix.h>
22 #include <gtsam/base/Vector.h>
23 #include <gtsam/dllexport.h>
24 #include <gtsam/geometry/Rot2.h>
25 #include <gtsam/geometry/Rot3.h>
31 #include <gtsam/slam/dataset.h>
32 
33 #include <Eigen/Sparse>
34 #include <map>
35 #include <string>
36 #include <type_traits>
37 #include <utility>
38 #include <vector>
39 
40 namespace gtsam {
41 class NonlinearFactorGraph;
42 class LevenbergMarquardtOptimizer;
43 
45 template <size_t d>
46 struct GTSAM_EXPORT ShonanAveragingParameters {
47  // Select Rot2 or Rot3 interface based template parameter d
49  using Anchor = std::pair<size_t, Rot>;
50 
51  // Parameters themselves:
55  double alpha;
56  double beta;
57  double gamma;
58  bool useHuber;
62 
65  const std::string &method = "JACOBI",
66  double optimalityThreshold = -1e-4,
67  double alpha = 0.0, double beta = 1.0,
68  double gamma = 0.0);
69 
70  LevenbergMarquardtParams getLMParams() const { return lm; }
71 
72  void setOptimalityThreshold(double value) { optimalityThreshold = value; }
73  double getOptimalityThreshold() const { return optimalityThreshold; }
74 
75  void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; }
76  std::pair<size_t, Rot> getAnchor() const { return anchor; }
77 
78  void setAnchorWeight(double value) { alpha = value; }
79  double getAnchorWeight() const { return alpha; }
80 
81  void setKarcherWeight(double value) { beta = value; }
82  double getKarcherWeight() const { return beta; }
83 
84  void setGaugesWeight(double value) { gamma = value; }
85  double getGaugesWeight() const { return gamma; }
86 
87  void setUseHuber(bool value) { useHuber = value; }
88  bool getUseHuber() const { return useHuber; }
89 
90  void setCertifyOptimality(bool value) { certifyOptimality = value; }
91  bool getCertifyOptimality() const { return certifyOptimality; }
92 
94  void print(const std::string &s = "") const {
95  std::cout << (s.empty() ? s : s + " ");
96  std::cout << " ShonanAveragingParameters: " << std::endl;
97  std::cout << " alpha: " << alpha << std::endl;
98  std::cout << " beta: " << beta << std::endl;
99  std::cout << " gamma: " << gamma << std::endl;
100  std::cout << " useHuber: " << useHuber << std::endl;
101  }
102 };
103 
106 
122 template <size_t d>
123 class GTSAM_EXPORT ShonanAveraging {
124  public:
126 
127  // Define the Parameters type and use its typedef of the rotation type:
129  using Rot = typename Parameters::Rot;
130 
131  // We store SO(d) BetweenFactors to get noise model
132  using Measurements = std::vector<BinaryMeasurement<Rot>>;
133 
134  private:
137  size_t nrUnknowns_;
138  Sparse D_; // Sparse (diagonal) degree matrix
139  Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr
140  Sparse L_; // connection Laplacian L = D - Q, needed for optimality check
141 
146  Sparse buildQ() const;
147 
149  Sparse buildD() const;
150 
151  public:
154 
157  ShonanAveraging(const Measurements &measurements,
158  const Parameters &parameters = Parameters());
159 
163 
165  size_t nrUnknowns() const { return nrUnknowns_; }
166 
168  size_t numberMeasurements() const { return measurements_.size(); }
169 
171  const BinaryMeasurement<Rot> &measurement(size_t k) const {
172  return measurements_[k];
173  }
174 
182  double k = 1.345) const {
183  Measurements robustMeasurements;
184  for (auto &measurement : measurements) {
185  auto model = measurement.noiseModel();
186  const auto &robust =
187  std::dynamic_pointer_cast<noiseModel::Robust>(model);
188 
189  SharedNoiseModel robust_model;
190  // Check if the noise model is already robust
191  if (robust) {
192  robust_model = model;
193  } else {
194  // make robust
195  robust_model = noiseModel::Robust::Create(
197  }
198  BinaryMeasurement<Rot> meas(measurement.key1(), measurement.key2(),
199  measurement.measured(), robust_model);
200  robustMeasurements.push_back(meas);
201  }
202  return robustMeasurements;
203  }
204 
206  const Rot &measured(size_t k) const { return measurements_[k].measured(); }
207 
209  const KeyVector &keys(size_t k) const { return measurements_[k].keys(); }
210 
214 
215  Sparse D() const { return D_; }
216  Matrix denseD() const { return Matrix(D_); }
217  Sparse Q() const { return Q_; }
218  Matrix denseQ() const { return Matrix(Q_); }
219  Sparse L() const { return L_; }
220  Matrix denseL() const { return Matrix(L_); }
221 
223  Sparse computeLambda(const Matrix &S) const;
224 
227  return Matrix(computeLambda(values));
228  }
229 
231  Matrix computeLambda_(const Matrix &S) const {
232  return Matrix(computeLambda(S));
233  }
234 
236  Sparse computeA(const Values &values) const;
237 
239  Sparse computeA(const Matrix &S) const;
240 
242  Matrix computeA_(const Values &values) const {
243  return Matrix(computeA(values));
244  }
245 
247  static Matrix StiefelElementMatrix(const Values &values);
248 
253  double computeMinEigenValue(const Values &values,
254  Vector *minEigenVector = nullptr) const;
255 
260  double computeMinEigenValueAP(const Values &values,
261  Vector *minEigenVector = nullptr) const;
262 
264  Values roundSolutionS(const Matrix &S) const;
265 
267  static VectorValues TangentVectorValues(size_t p, const Vector &v);
268 
270  Matrix riemannianGradient(size_t p, const Values &values) const;
271 
276  static Values LiftwithDescent(size_t p, const Values &values,
277  const Vector &minEigenVector);
278 
286  Values initializeWithDescent(
287  size_t p, const Values &values, const Vector &minEigenVector,
288  double minEigenValue, double gradienTolerance = 1e-2,
289  double preconditionedGradNormTolerance = 1e-4) const;
293 
298  NonlinearFactorGraph buildGraphAt(size_t p) const;
299 
305  Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const;
306 
308  Values initializeRandomlyAt(size_t p) const;
309 
314  double costAt(size_t p, const Values &values) const;
315 
321  Sparse computeLambda(const Values &values) const;
322 
328  std::pair<double, Vector> computeMinEigenVector(const Values &values) const;
329 
334  bool checkOptimality(const Values &values) const;
335 
342  std::shared_ptr<LevenbergMarquardtOptimizer> createOptimizerAt(
343  size_t p, const Values &initial) const;
344 
351  Values tryOptimizingAt(size_t p, const Values &initial) const;
352 
357  Values projectFrom(size_t p, const Values &values) const;
358 
363  Values roundSolution(const Values &values) const;
364 
366  template <class T>
367  static Values LiftTo(size_t p, const Values &values) {
368  Values result;
369  for (const auto& it : values.extract<T>()) {
370  result.insert(it.first, SOn::Lift(p, it.second.matrix()));
371  }
372  return result;
373  }
374 
378 
383  double cost(const Values &values) const;
384 
392  Values initializeRandomly(std::mt19937 &rng) const;
393 
395  Values initializeRandomly() const;
396 
404  std::pair<Values, double> run(const Values &initialEstimate, size_t pMin = d,
405  size_t pMax = 10) const;
407 
417  template <typename T>
418  inline std::vector<BinaryMeasurement<T>> maybeRobust(
419  const std::vector<BinaryMeasurement<T>> &measurements,
420  bool useRobustModel = false) const {
421  return useRobustModel ? makeNoiseModelRobust(measurements) : measurements;
422  }
423 };
424 
425 // Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a
426 // convenience interface with file access.
427 
428 class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> {
429  public:
430  ShonanAveraging2(const Measurements &measurements,
431  const Parameters &parameters = Parameters());
432  explicit ShonanAveraging2(std::string g2oFile,
433  const Parameters &parameters = Parameters());
435  const Parameters &parameters = Parameters());
436 };
437 
438 class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> {
439  public:
440  ShonanAveraging3(const Measurements &measurements,
441  const Parameters &parameters = Parameters());
442  explicit ShonanAveraging3(std::string g2oFile,
443  const Parameters &parameters = Parameters());
444 
445  // TODO(frank): Deprecate after we land pybind wrapper
447  const Parameters &parameters = Parameters());
448 };
449 } // namespace gtsam
typename std::conditional< d==2, Rot2, Rot3 >::type Rot
double beta
weight of Karcher-based prior (default 1)
Parameters for Levenberg-Marquardt trust-region scheme.
const KeyVector & keys(size_t k) const
Keys for k^th measurement, as a vector of Key values.
Sparse Q() const
Sparse version of Q.
std::pair< size_t, Rot > Anchor
size_t numberMeasurements() const
Return number of measurements.
std::vector< BetweenFactor< Pose3 >::shared_ptr > BetweenFactorPose3s
Definition: dataset.h:218
noiseModel::Diagonal::shared_ptr model
static std::mt19937 rng
LevenbergMarquardtParams getLMParams() const
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Definition: Values-inl.h:105
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
typename std::conditional< d==2, Rot2, Rot3 >::type Rot
leaf::MyValues values
const GaussianFactorGraph factors
LevenbergMarquardtParams lm
LM parameters.
Matrix denseL() const
Dense version of L.
typename Parameters::Rot Rot
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
Definition: NoiseModel.cpp:704
static LevenbergMarquardtParams CeresDefaults()
DiscreteKey S(1, 2)
Sparse D() const
Sparse version of D.
Matrix computeLambda_(const Values &values) const
Dense versions of computeLambda for wrapper/testing.
bool certifyOptimality
if enabled solution optimality is certified (default true)
static Point2 measurement(323.0, 240.0)
Sparse L() const
Sparse version of L.
double alpha
weight of anchor-based prior (default 0)
Factor Graph Values.
Parameters governing optimization etc.
accelerated power method for fast eigenvalue and eigenvector computation
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
void setKarcherWeight(double value)
void setOptimalityThreshold(double value)
Power method for fast eigenvalue and eigenvector computation.
Matrix denseD() const
Dense version of D.
void setAnchorWeight(double value)
Array< int, Dynamic, 1 > v
Anchor anchor
pose to use as anchor if not Karcher
std::vector< BinaryMeasurement< T > > maybeRobust(const std::vector< BinaryMeasurement< T >> &measurements, bool useRobustModel=false) const
RealScalar alpha
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
double optimalityThreshold
threshold used in checkOptimality
static ConjugateGradientParameters parameters
void setGaugesWeight(double value)
std::pair< size_t, Rot > getAnchor() const
const Rot & measured(size_t k) const
k^th measurement, as a Rot.
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Definition: SOn.h:103
Measurements makeNoiseModelRobust(const Measurements &measurements, double k=1.345) const
std::vector< BinaryMeasurement< Rot > > Measurements
void print(const std::string &s="") const
Print the parameters and flags used for rotation averaging.
size_t nrUnknowns() const
Return number of unknowns.
float * p
void setAnchor(size_t index, const Rot &value)
void insert(Key j, const Value &val)
Definition: Values.cpp:155
const BinaryMeasurement< Rot > & measurement(size_t k) const
k^th binary measurement
std::vector< BetweenFactor< Pose2 >::shared_ptr > BetweenFactorPose2s
Definition: dataset.h:212
Matrix computeLambda_(const Matrix &S) const
Dense versions of computeLambda for wrapper/testing.
Matrix denseQ() const
Dense version of Q.
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
static Values LiftTo(size_t p, const Values &values)
Lift Values of type T to SO(p)
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
utility functions for loading datasets
Matrix computeA_(const Values &values) const
Dense version of computeA for wrapper/testing.
2D rotation
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
3D rotation represented as a rotation matrix or quaternion


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:42