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 
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:
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:
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
Eigen::SparseMatrix< double >
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
gtsam::noiseModel::mEstimator::Huber::Create
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
Definition: LossFunctions.cpp:203
D
MatrixXcd D
Definition: EigenSolver_EigenSolver_MatrixType.cpp:14
gtsam::ShonanAveragingParameters::lm
LevenbergMarquardtParams lm
LM parameters.
Definition: ShonanAveraging.h:52
gtsam.examples.DogLegOptimizerExample.type
type
Definition: DogLegOptimizerExample.py:111
Vector.h
typedef and functions to augment Eigen's VectorXd
gtsam::ShonanAveraging::measurement
const BinaryMeasurement< Rot > & measurement(size_t k) const
k^th binary measurement
Definition: ShonanAveraging.h:171
gtsam::ShonanAveragingParameters::getGaugesWeight
double getGaugesWeight() const
Definition: ShonanAveraging.h:85
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
gtsam::ShonanAveraging< 3 >::Rot
typename Parameters::Rot Rot
Definition: ShonanAveraging.h:129
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::ShonanAveragingParameters::setCertifyOptimality
void setCertifyOptimality(bool value)
Definition: ShonanAveraging.h:90
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
gtsam::ShonanAveraging::makeNoiseModelRobust
Measurements makeNoiseModelRobust(const Measurements &measurements, double k=1.345) const
Definition: ShonanAveraging.h:181
gtsam::ShonanAveraging::parameters_
Parameters parameters_
Definition: ShonanAveraging.h:135
gtsam::noiseModel::Robust::Create
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
Definition: NoiseModel.cpp:740
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::ShonanAveragingParameters::Rot
typename std::conditional< d==2, Rot2, Rot3 >::type Rot
Definition: ShonanAveraging.h:48
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
gtsam::ShonanAveraging::measured
const Rot & measured(size_t k) const
k^th measurement, as a Rot.
Definition: ShonanAveraging.h:206
gtsam::ShonanAveragingParameters::gamma
double gamma
Definition: ShonanAveraging.h:57
gtsam::ShonanAveragingParameters::getUseHuber
bool getUseHuber() const
Definition: ShonanAveraging.h:88
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::ShonanAveragingParameters::alpha
double alpha
weight of anchor-based prior (default 0)
Definition: ShonanAveraging.h:55
gtsam::ShonanAveragingParameters::setKarcherWeight
void setKarcherWeight(double value)
Definition: ShonanAveraging.h:81
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::ShonanAveragingParameters::getCertifyOptimality
bool getCertifyOptimality() const
Definition: ShonanAveraging.h:91
gtsam::ShonanAveraging::computeLambda_
Matrix computeLambda_(const Values &values) const
Dense versions of computeLambda for wrapper/testing.
Definition: ShonanAveraging.h:226
LevenbergMarquardtParams.h
Parameters for Levenberg-Marquardt trust-region scheme.
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
beta
double beta(double a, double b)
Definition: beta.c:61
Q
Quaternion Q
Definition: testQuaternion.cpp:27
Rot
typename std::conditional< d==2, Rot2, Rot3 >::type Rot
Definition: testShonanAveraging.cpp:34
Rot2.h
2D rotation
gtsam::ShonanAveragingParameters::optimalityThreshold
double optimalityThreshold
threshold used in checkOptimality
Definition: ShonanAveraging.h:53
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::ShonanAveraging::Q_
Sparse Q_
Definition: ShonanAveraging.h:139
gtsam::ShonanAveraging::measurements_
Measurements measurements_
Definition: ShonanAveraging.h:136
gtsam::ShonanAveragingParameters::anchor
Anchor anchor
pose to use as anchor if not Karcher
Definition: ShonanAveraging.h:54
AcceleratedPowerMethod.h
accelerated power method for fast eigenvalue and eigenvector computation
gtsam::ShonanAveragingParameters::getAnchor
std::pair< size_t, Rot > getAnchor() const
Definition: ShonanAveraging.h:76
gtsam::ShonanAveragingParameters::getKarcherWeight
double getKarcherWeight() const
Definition: ShonanAveraging.h:82
PowerMethod.h
Power method for fast eigenvalue and eigenvector computation.
dataset.h
utility functions for loading datasets
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::ShonanAveragingParameters::getLMParams
LevenbergMarquardtParams getLMParams() const
Definition: ShonanAveraging.h:70
gtsam::ShonanAveragingParameters::setGaugesWeight
void setGaugesWeight(double value)
Definition: ShonanAveraging.h:84
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
gtsam::ShonanAveragingParameters
Parameters governing optimization etc.
Definition: ShonanAveraging.h:46
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::ShonanAveraging3
Definition: ShonanAveraging.h:438
gtsam::ShonanAveragingParameters::getAnchorWeight
double getAnchorWeight() const
Definition: ShonanAveraging.h:79
gtsam::ShonanAveragingParameters::print
void print(const std::string &s="") const
Print the parameters and flags used for rotation averaging.
Definition: ShonanAveraging.h:94
L
MatrixXd L
Definition: LLT_example.cpp:6
gtsam::SO::Lift
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Definition: SOn.h:103
gtsam.examples.DogLegOptimizerExample.run
def run(args)
Definition: DogLegOptimizerExample.py:21
gtsam::ShonanAveraging::computeLambda_
Matrix computeLambda_(const Matrix &S) const
Dense versions of computeLambda for wrapper/testing.
Definition: ShonanAveraging.h:231
VectorValues.h
Factor Graph Values.
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
Eigen::Triplet< double >
gamma
#define gamma
Definition: mconf.h:85
gtsam::ShonanAveraging::numberMeasurements
size_t numberMeasurements() const
Return number of measurements.
Definition: ShonanAveraging.h:168
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::ShonanAveragingParameters::setOptimalityThreshold
void setOptimalityThreshold(double value)
Definition: ShonanAveraging.h:72
gtsam::ShonanAveragingParameters::certifyOptimality
bool certifyOptimality
if enabled solution optimality is certified (default true)
Definition: ShonanAveraging.h:61
gtsam::ShonanAveraging::nrUnknowns
size_t nrUnknowns() const
Return number of unknowns.
Definition: ShonanAveraging.h:165
gtsam::BinaryMeasurement
Definition: BinaryMeasurement.h:36
gtsam
traits
Definition: SFMdata.h:40
gtsam::BetweenFactorPose3s
std::vector< BetweenFactor< Pose3 >::shared_ptr > BetweenFactorPose3s
Definition: dataset.h:218
gtsam::LevenbergMarquardtParams::CeresDefaults
static LevenbergMarquardtParams CeresDefaults()
Definition: LevenbergMarquardtParams.h:106
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
gtsam::Sparse
Eigen::SparseMatrix< double > Sparse
Definition: AcceleratedPowerMethod.h:26
gtsam::ShonanAveragingParameters::setAnchorWeight
void setAnchorWeight(double value)
Definition: ShonanAveraging.h:78
gtsam::ShonanAveraging::LiftTo
static Values LiftTo(size_t p, const Values &values)
Lift Values of type T to SO(p)
Definition: ShonanAveraging.h:367
gtsam::Values
Definition: Values.h:65
gtsam::ShonanAveraging::D_
Sparse D_
Definition: ShonanAveraging.h:138
gtsam::ShonanAveraging< 3 >::Measurements
std::vector< BinaryMeasurement< Rot > > Measurements
Definition: ShonanAveraging.h:132
gtsam::ShonanAveraging::maybeRobust
std::vector< BinaryMeasurement< T > > maybeRobust(const std::vector< BinaryMeasurement< T >> &measurements, bool useRobustModel=false) const
Definition: ShonanAveraging.h:418
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
gtsam::ShonanAveraging::computeA_
Matrix computeA_(const Values &values) const
Dense version of computeA for wrapper/testing.
Definition: ShonanAveraging.h:242
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::BetweenFactorPose2s
std::vector< BetweenFactor< Pose2 >::shared_ptr > BetweenFactorPose2s
Definition: dataset.h:212
initial
Definition: testScenarioRunner.cpp:148
gtsam::ShonanAveragingParameters::Anchor
std::pair< size_t, Rot > Anchor
Definition: ShonanAveraging.h:49
gtsam::ShonanAveraging
Definition: ShonanAveraging.h:123
gtsam::ShonanAveragingParameters::setUseHuber
void setUseHuber(bool value)
Definition: ShonanAveraging.h:87
gtsam::ShonanAveragingParameters::beta
double beta
weight of Karcher-based prior (default 1)
Definition: ShonanAveraging.h:56
test_callbacks.value
value
Definition: test_callbacks.py:160
measurement
static Point2 measurement(323.0, 240.0)
gtsam::ShonanAveraging::keys
const KeyVector & keys(size_t k) const
Keys for k^th measurement, as a vector of Key values.
Definition: ShonanAveraging.h:209
gtsam::ShonanAveragingParameters::getOptimalityThreshold
double getOptimalityThreshold() const
Definition: ShonanAveraging.h:73
S
DiscreteKey S(1, 2)
gtsam::ShonanAveraging::nrUnknowns_
size_t nrUnknowns_
Definition: ShonanAveraging.h:137
gtsam::ShonanAveragingParameters::setAnchor
void setAnchor(size_t index, const Rot &value)
Definition: ShonanAveraging.h:75
gtsam::ShonanAveraging2
Definition: ShonanAveraging.h:428
gtsam::ShonanAveraging::L_
Sparse L_
Definition: ShonanAveraging.h:140


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:15