gtsam  4.1.0
gtsam
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>
29 #include <gtsam/slam/dataset.h>
30 
31 #include <Eigen/Sparse>
32 #include <map>
33 #include <string>
34 #include <type_traits>
35 #include <utility>
36 #include <vector>
37 
38 namespace gtsam {
39 class NonlinearFactorGraph;
40 class LevenbergMarquardtOptimizer;
41 
43 template <size_t d>
44 struct GTSAM_EXPORT ShonanAveragingParameters {
45  // Select Rot2 or Rot3 interface based template parameter d
46  using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
47  using Anchor = std::pair<size_t, Rot>;
48 
49  // Paremeters themselves:
50  LevenbergMarquardtParams lm; // LM parameters
51  double optimalityThreshold; // threshold used in checkOptimality
52  Anchor anchor; // pose to use as anchor if not Karcher
53  double alpha; // weight of anchor-based prior (default 0)
54  double beta; // weight of Karcher-based prior (default 1)
55  double gamma; // weight of gauge-fixing factors (default 0)
56 
58  LevenbergMarquardtParams::CeresDefaults(),
59  const std::string &method = "JACOBI",
60  double optimalityThreshold = -1e-4,
61  double alpha = 0.0, double beta = 1.0,
62  double gamma = 0.0);
63 
64  LevenbergMarquardtParams getLMParams() const { return lm; }
65 
66  void setOptimalityThreshold(double value) { optimalityThreshold = value; }
67  double getOptimalityThreshold() const { return optimalityThreshold; }
68 
69  void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; }
70  std::pair<size_t, Rot> getAnchor() { return anchor; }
71 
72  void setAnchorWeight(double value) { alpha = value; }
73  double getAnchorWeight() { return alpha; }
74 
75  void setKarcherWeight(double value) { beta = value; }
76  double getKarcherWeight() { return beta; }
77 
78  void setGaugesWeight(double value) { gamma = value; }
79  double getGaugesWeight() { return gamma; }
80 };
81 
84 
100 template <size_t d>
101 class GTSAM_EXPORT ShonanAveraging {
102  public:
103  using Sparse = Eigen::SparseMatrix<double>;
104 
105  // Define the Parameters type and use its typedef of the rotation type:
107  using Rot = typename Parameters::Rot;
108 
109  // We store SO(d) BetweenFactors to get noise model
110  // TODO(frank): use BinaryMeasurement?
111  using Measurements = std::vector<BinaryMeasurement<Rot>>;
112 
113  private:
114  Parameters parameters_;
115  Measurements measurements_;
116  size_t nrUnknowns_;
117  Sparse D_; // Sparse (diagonal) degree matrix
118  Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr
119  Sparse L_; // connection Laplacian L = D - Q, needed for optimality check
120 
125  Sparse buildQ() const;
126 
128  Sparse buildD() const;
129 
130  public:
133 
136  ShonanAveraging(const Measurements &measurements,
137  const Parameters &parameters = Parameters());
138 
142 
144  size_t nrUnknowns() const { return nrUnknowns_; }
145 
147  size_t nrMeasurements() const { return measurements_.size(); }
148 
150  const BinaryMeasurement<Rot> &measurement(size_t k) const {
151  return measurements_[k];
152  }
153 
155  const Rot &measured(size_t k) const { return measurements_[k].measured(); }
156 
158  const KeyVector &keys(size_t k) const { return measurements_[k].keys(); }
159 
163 
164  Sparse D() const { return D_; }
165  Matrix denseD() const { return Matrix(D_); }
166  Sparse Q() const { return Q_; }
167  Matrix denseQ() const { return Matrix(Q_); }
168  Sparse L() const { return L_; }
169  Matrix denseL() const { return Matrix(L_); }
170 
172  Sparse computeLambda(const Matrix &S) const;
173 
175  Matrix computeLambda_(const Values &values) const {
176  return Matrix(computeLambda(values));
177  }
178 
180  Matrix computeLambda_(const Matrix &S) const {
181  return Matrix(computeLambda(S));
182  }
183 
185  Sparse computeA(const Values &values) const;
186 
188  Sparse computeA(const Matrix &S) const;
189 
191  Matrix computeA_(const Values &values) const {
192  return Matrix(computeA(values));
193  }
194 
196  static Matrix StiefelElementMatrix(const Values &values);
197 
202  double computeMinEigenValue(const Values &values,
203  Vector *minEigenVector = nullptr) const;
204 
206  Values roundSolutionS(const Matrix &S) const;
207 
209  static VectorValues TangentVectorValues(size_t p, const Vector &v);
210 
212  Matrix riemannianGradient(size_t p, const Values &values) const;
213 
218  static Values LiftwithDescent(size_t p, const Values &values,
219  const Vector &minEigenVector);
220 
228  Values initializeWithDescent(
229  size_t p, const Values &values, const Vector &minEigenVector,
230  double minEigenValue, double gradienTolerance = 1e-2,
231  double preconditionedGradNormTolerance = 1e-4) const;
235 
240  NonlinearFactorGraph buildGraphAt(size_t p) const;
241 
246  Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const;
247 
249  Values initializeRandomlyAt(size_t p) const;
250 
255  double costAt(size_t p, const Values &values) const;
256 
262  Sparse computeLambda(const Values &values) const;
263 
269  std::pair<double, Vector> computeMinEigenVector(const Values &values) const;
270 
275  bool checkOptimality(const Values &values) const;
276 
283  boost::shared_ptr<LevenbergMarquardtOptimizer> createOptimizerAt(
284  size_t p, const Values &initial) const;
285 
292  Values tryOptimizingAt(size_t p, const Values &initial) const;
293 
298  Values projectFrom(size_t p, const Values &values) const;
299 
304  Values roundSolution(const Values &values) const;
305 
307  template <class T>
308  static Values LiftTo(size_t p, const Values &values) {
309  Values result;
310  for (const auto it : values.filter<T>()) {
311  result.insert(it.key, SOn::Lift(p, it.value.matrix()));
312  }
313  return result;
314  }
315 
319 
324  double cost(const Values &values) const;
325 
333  Values initializeRandomly(std::mt19937 &rng) const;
334 
336  Values initializeRandomly() const;
337 
345  std::pair<Values, double> run(const Values &initialEstimate, size_t pMin = d,
346  size_t pMax = 10) const;
348 };
349 
350 // Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a
351 // convenience interface with file access.
352 
354  public:
355  ShonanAveraging2(const Measurements &measurements,
356  const Parameters &parameters = Parameters());
357  explicit ShonanAveraging2(string g2oFile,
358  const Parameters &parameters = Parameters());
359 };
360 
362  public:
363  ShonanAveraging3(const Measurements &measurements,
364  const Parameters &parameters = Parameters());
365  explicit ShonanAveraging3(string g2oFile,
366  const Parameters &parameters = Parameters());
367 
368  // TODO(frank): Deprecate after we land pybind wrapper
369  ShonanAveraging3(const BetweenFactorPose3s &factors,
370  const Parameters &parameters = Parameters());
371 };
372 } // namespace gtsam
gtsam::ShonanAveraging::computeA_
Matrix computeA_(const Values &values) const
Dense version of computeA for wrapper/testing.
Definition: ShonanAveraging.h:191
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
gtsam::BinaryMeasurement< Rot >
gtsam::ShonanAveraging2
Definition: ShonanAveraging.h:353
gtsam::ShonanAveraging::Q
Sparse Q() const
Sparse version of Q.
Definition: ShonanAveraging.h:166
gtsam::ShonanAveragingParameters
Parameters governing optimization etc.
Definition: ShonanAveraging.h:44
gtsam::Values::filter
Filtered< Value > filter(const boost::function< bool(Key)> &filterFcn)
Return a filtered view of this Values class, without copying any data.
Definition: Values-inl.h:237
LevenbergMarquardtParams.h
Parameters for Levenberg-Marquardt trust-region scheme.
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::ShonanAveraging::projectFrom
Values projectFrom(size_t p, const Values &values) const
Project from SO(p) to Rot2 or Rot3 Values should be of type SO(p)
gtsam::ShonanAveraging::D
Sparse D() const
Sparse version of D.
Definition: ShonanAveraging.h:164
gtsam::ShonanAveraging::denseD
Matrix denseD() const
Dense version of D.
Definition: ShonanAveraging.h:165
VectorValues.h
Factor Graph Values.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::ShonanAveraging::denseL
Matrix denseL() const
Dense version of L.
Definition: ShonanAveraging.h:169
dataset.h
utility functions for loading datasets
gtsam::ShonanAveraging::L
Sparse L() const
Sparse version of L.
Definition: ShonanAveraging.h:168
gtsam::ShonanAveraging::computeLambda_
Matrix computeLambda_(const Matrix &S) const
Dense versions of computeLambda for wrapper/testing.
Definition: ShonanAveraging.h:180
Vector.h
typedef and functions to augment Eigen's VectorXd
gtsam::ShonanAveraging::keys
const KeyVector & keys(size_t k) const
Keys for k^th measurement, as a vector of Key values.
Definition: ShonanAveraging.h:158
gtsam::ShonanAveraging::roundSolutionS
Values roundSolutionS(const Matrix &S) const
Project pxdN Stiefel manifold matrix S to Rot3^N.
Rot2.h
2D rotation
gtsam::ShonanAveraging::measured
const Rot & measured(size_t k) const
k^th measurement, as a Rot.
Definition: ShonanAveraging.h:155
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::ShonanAveraging::nrUnknowns
size_t nrUnknowns() const
Return number of unknowns.
Definition: ShonanAveraging.h:144
gtsam::ShonanAveraging
Class that implements Shonan Averaging from our ECCV'20 paper.
Definition: ShonanAveraging.h:101
gtsam::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition: LevenbergMarquardtParams.h:33
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::ShonanAveraging3
Definition: ShonanAveraging.h:361
gtsam::ShonanAveraging::denseQ
Matrix denseQ() const
Dense version of Q.
Definition: ShonanAveraging.h:167
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::ShonanAveraging::computeLambda_
Matrix computeLambda_(const Values &values) const
Dense versions of computeLambda for wrapper/testing.
Definition: ShonanAveraging.h:175
gtsam::ShonanAveraging::LiftTo
static Values LiftTo(size_t p, const Values &values)
Lift Values of type T to SO(p)
Definition: ShonanAveraging.h:308
gtsam::ShonanAveraging::nrMeasurements
size_t nrMeasurements() const
Return number of measurements.
Definition: ShonanAveraging.h:147
gtsam::SO::Lift
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Definition: SOn.h:99
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::ShonanAveraging::measurement
const BinaryMeasurement< Rot > & measurement(size_t k) const
k^th binary measurement
Definition: ShonanAveraging.h:150