testShonanAveraging.cpp
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 
24 
25 #include <algorithm>
26 #include <iostream>
27 #include <map>
28 #include <random>
29 
30 using namespace std;
31 using namespace gtsam;
32 
33 template <size_t d>
35 
36 template <size_t d>
38 
40  const std::string &name,
41  ShonanAveraging3::Parameters parameters = ShonanAveraging3::Parameters()) {
42  string g2oFile = findExampleDataFile(name);
43  return ShonanAveraging3(g2oFile, parameters);
44 }
45 
46 static const ShonanAveraging3 kShonan = fromExampleName("toyExample.g2o");
47 
48 static std::mt19937 kRandomNumberGenerator(42);
49 
50 /* ************************************************************************* */
51 TEST(ShonanAveraging3, checkConstructor) {
52  EXPECT_LONGS_EQUAL(5, kShonan.nrUnknowns());
53 
54  EXPECT_LONGS_EQUAL(15, kShonan.D().rows());
55  EXPECT_LONGS_EQUAL(15, kShonan.D().cols());
56  auto D = kShonan.denseD();
57  EXPECT_LONGS_EQUAL(15, D.rows());
58  EXPECT_LONGS_EQUAL(15, D.cols());
59 
60  EXPECT_LONGS_EQUAL(15, kShonan.Q().rows());
61  EXPECT_LONGS_EQUAL(15, kShonan.Q().cols());
62  auto Q = kShonan.denseQ();
63  EXPECT_LONGS_EQUAL(15, Q.rows());
64  EXPECT_LONGS_EQUAL(15, Q.cols());
65 
66  EXPECT_LONGS_EQUAL(15, kShonan.L().rows());
67  EXPECT_LONGS_EQUAL(15, kShonan.L().cols());
68  auto L = kShonan.denseL();
69  EXPECT_LONGS_EQUAL(15, L.rows());
70  EXPECT_LONGS_EQUAL(15, L.cols());
71 }
72 
73 /* ************************************************************************* */
74 TEST(ShonanAveraging3, buildGraphAt) {
75  auto graph = kShonan.buildGraphAt(5);
77 }
78 
79 /* ************************************************************************* */
80 TEST(ShonanAveraging3, checkOptimality) {
81  const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
82  Values random = ShonanAveraging3::LiftTo<Rot3>(4, randomRotations); // lift to 4!
83  auto Lambda = kShonan.computeLambda(random);
84  EXPECT_LONGS_EQUAL(15, Lambda.rows());
85  EXPECT_LONGS_EQUAL(15, Lambda.cols());
86  EXPECT_LONGS_EQUAL(45, Lambda.nonZeros());
87  auto lambdaMin = kShonan.computeMinEigenValue(random);
88  // EXPECT_DOUBLES_EQUAL(-5.2964625490657866, lambdaMin,
89  // 1e-4); // Regression test
90  EXPECT_DOUBLES_EQUAL(-414.87376657555996, lambdaMin,
91  1e-4); // Regression test
92  EXPECT(!kShonan.checkOptimality(random));
93 }
94 
95 /* ************************************************************************* */
96 TEST(ShonanAveraging3, checkSubgraph) {
97  // Create parameter with solver set to SUBGRAPH
100  ShonanAveraging3::Measurements measurements;
101 
102  // The toyExample.g2o has 5 vertices, from 0-4
103  // The edges are: 1-2, 2-3, 3-4, 3-1, 1-4, 0-1,
104  // which can build a connected graph
105  auto subgraphShonan = fromExampleName("toyExample.g2o", params);
106 
107  // Create initial random estimation
108  Values initial;
109  initial = subgraphShonan.initializeRandomly(kRandomNumberGenerator);
110 
111  // Run Shonan with SUBGRAPH solver
112  auto result = subgraphShonan.run(initial, 3, 3);
113  EXPECT_DOUBLES_EQUAL(1e-11, subgraphShonan.cost(result.first), 1e-4);
114 }
115 
116 /* ************************************************************************* */
117 TEST(ShonanAveraging3, tryOptimizingAt3) {
118  const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
119  Values initial = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations); // convert to SOn
120  EXPECT(!kShonan.checkOptimality(initial));
121  const Values result = kShonan.tryOptimizingAt(3, initial);
122  EXPECT(kShonan.checkOptimality(result));
123  auto lambdaMin = kShonan.computeMinEigenValue(result);
124  EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, lambdaMin,
125  1e-4); // Regression test
126  EXPECT_DOUBLES_EQUAL(0, kShonan.costAt(3, result), 1e-4);
127  const Values SO3Values = kShonan.roundSolution(result);
128  EXPECT_DOUBLES_EQUAL(0, kShonan.cost(SO3Values), 1e-4);
129 }
130 
131 /* ************************************************************************* */
132 TEST(ShonanAveraging3, tryOptimizingAt4) {
133  const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
134  Values random = ShonanAveraging3::LiftTo<Rot3>(4, randomRotations); // lift to 4!
135  const Values result = kShonan.tryOptimizingAt(4, random);
136  EXPECT(kShonan.checkOptimality(result));
137  EXPECT_DOUBLES_EQUAL(0, kShonan.costAt(4, result), 1e-3);
138  auto lambdaMin = kShonan.computeMinEigenValue(result);
139  EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, lambdaMin,
140  1e-4); // Regression test
141  const Values SO3Values = kShonan.roundSolution(result);
142  EXPECT_DOUBLES_EQUAL(0, kShonan.cost(SO3Values), 1e-4);
143 }
144 
145 /* ************************************************************************* */
146 TEST(ShonanAveraging3, TangentVectorValues) {
147  Vector9 v;
148  v << 1, 2, 3, 4, 5, 6, 7, 8, 9;
149  Vector expected0(10), expected1(10), expected2(10);
150  expected0 << 0, 3, -2, 1, 0, 0, 0, 0, 0, 0;
151  expected1 << 0, 6, -5, 4, 0, 0, 0, 0, 0, 0;
152  expected2 << 0, 9, -8, 7, 0, 0, 0, 0, 0, 0;
153  const VectorValues xi = ShonanAveraging3::TangentVectorValues(5, v);
154  EXPECT(assert_equal(expected0, xi[0]));
155  EXPECT(assert_equal(expected1, xi[1]));
156  EXPECT(assert_equal(expected2, xi[2]));
157 }
158 
159 /* ************************************************************************* */
161  auto I = genericValue(Rot3());
162  Values initial{{0, I}, {1, I}, {2, I}};
163  Values lifted = ShonanAveraging3::LiftTo<Rot3>(5, initial);
164  EXPECT(assert_equal(SOn(5), lifted.at<SOn>(0)));
165 }
166 
167 /* ************************************************************************* */
168 TEST(ShonanAveraging3, CheckWithEigen) {
169  // control randomness
170  static std::mt19937 rng(0);
171  Vector descentDirection = Vector::Random(15); // for use below
172  const Values randomRotations = kShonan.initializeRandomly(rng);
173  Values random = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);
174 
175  // Optimize
176  const Values Qstar3 = kShonan.tryOptimizingAt(3, random);
177 
178  // Compute Eigenvalue with Spectra solver
179  double lambda = kShonan.computeMinEigenValue(Qstar3);
180 
181  // Check Eigenvalue with slow Eigen version, converts matrix A to dense matrix!
182  const Matrix S = ShonanAveraging3::StiefelElementMatrix(Qstar3);
183  auto A = kShonan.computeA(S);
184  bool computeEigenvectors = false;
185  Eigen::EigenSolver<Matrix> eigenSolver(Matrix(A), computeEigenvectors);
186  auto lambdas = eigenSolver.eigenvalues().real();
187  double minEigenValue = lambdas(0);
188  for (int i = 1; i < lambdas.size(); i++)
189  minEigenValue = min(lambdas(i), minEigenValue);
190 
191  // Compute Eigenvalue with Accelerated Power method
192  double lambdaAP = kShonan.computeMinEigenValueAP(Qstar3);
193 
194  // Actual check
195  EXPECT_DOUBLES_EQUAL(0, lambda, 1e-11);
196  EXPECT_DOUBLES_EQUAL(0, minEigenValue, 1e-11);
197  EXPECT_DOUBLES_EQUAL(0, lambdaAP, 1e-11);
198 
199 
200  // Construct test descent direction (as minEigenVector is not predictable
201  // across platforms, being one from a basically flat 3d- subspace)
202 
203  // Check descent
204  Values initialQ4 =
205  ShonanAveraging3::LiftwithDescent(4, Qstar3, descentDirection);
206  EXPECT_LONGS_EQUAL(5, initialQ4.size());
207 
208  // TODO(frank): uncomment this regression test: currently not repeatable
209  // across platforms.
210  // Matrix expected(4, 4);
211  // expected << 0.0459224, -0.688689, -0.216922, 0.690321, //
212  // 0.92381, 0.191931, 0.255854, 0.21042, //
213  // -0.376669, 0.301589, 0.687953, 0.542111, //
214  // -0.0508588, 0.630804, -0.643587, 0.43046;
215  // EXPECT(assert_equal(SOn(expected), initialQ4.at<SOn>(0), 1e-5));
216 }
217 
218 /* ************************************************************************* */
219 TEST(ShonanAveraging3, initializeWithDescent) {
220  const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
221  Values random = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);
222  const Values Qstar3 = kShonan.tryOptimizingAt(3, random);
223  Vector minEigenVector;
224  double lambdaMin = kShonan.computeMinEigenValue(Qstar3, &minEigenVector);
225  Values initialQ4 =
226  kShonan.initializeWithDescent(4, Qstar3, minEigenVector, lambdaMin);
227  EXPECT_LONGS_EQUAL(5, initialQ4.size());
228 }
229 
230 /* ************************************************************************* */
233  auto result = kShonan.run(initial, 5);
234  EXPECT_DOUBLES_EQUAL(0, kShonan.cost(result.first), 1e-3);
235  EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, result.second,
236  1e-4); // Regression test
237 }
238 
239 /* ************************************************************************* */
240 namespace klaus {
241 // The data in the file is the Colmap solution
242 const Rot3 wR0(0.9992281076190063, -0.02676080288219576, -0.024497002638379624,
243  -0.015064701622500615);
244 const Rot3 wR1(0.998239108728862, -0.049543805396343954, -0.03232420352077356,
245  -0.004386230477751116);
246 const Rot3 wR2(0.9925378735259738, -0.07993768981394891, 0.0825062894866454,
247  -0.04088089479075661);
248 } // namespace klaus
249 
250 TEST(ShonanAveraging3, runKlaus) {
251  using namespace klaus;
252 
253  // Initialize a Shonan instance without the Karcher mean
254  ShonanAveraging3::Parameters parameters;
255  parameters.setKarcherWeight(0);
256 
257  // Load 3 pose example taken in Klaus by Shicong
258  static const ShonanAveraging3 shonan =
259  fromExampleName("Klaus3.g2o", parameters);
260 
261  // Check nr poses
262  EXPECT_LONGS_EQUAL(3, shonan.nrUnknowns());
263 
264  // Colmap uses the Y-down vision frame, and the first 3 rotations are close to
265  // identity. We check that below. Note tolerance is quite high.
266  static const Rot3 identity;
267  EXPECT(assert_equal(identity, wR0, 0.2));
268  EXPECT(assert_equal(identity, wR1, 0.2));
269  EXPECT(assert_equal(identity, wR2, 0.2));
270 
271  // Get measurements
272  const Rot3 R01 = shonan.measured(0);
273  const Rot3 R12 = shonan.measured(1);
274  const Rot3 R02 = shonan.measured(2);
275 
276  // Regression test to make sure data did not change.
277  EXPECT(assert_equal(Rot3(0.9995433591728293, -0.022048798853273946,
278  -0.01796327847857683, 0.010210006313668573),
279  R01));
280 
281  // Check Colmap solution agrees OK with relative rotation measurements.
282  EXPECT(assert_equal(R01, wR0.between(wR1), 0.1));
283  EXPECT(assert_equal(R12, wR1.between(wR2), 0.1));
284  EXPECT(assert_equal(R02, wR0.between(wR2), 0.1));
285 
286  // Run Shonan (with prior on first rotation)
288  auto result = shonan.run(initial, 5);
289  EXPECT_DOUBLES_EQUAL(0, shonan.cost(result.first), 1e-2);
290  EXPECT_DOUBLES_EQUAL(-9.2259161494467889e-05, result.second,
291  1e-4); // Regression
292 
293  // Get Shonan solution in new frame R (R for result)
294  const Rot3 rR0 = result.first.at<Rot3>(0);
295  const Rot3 rR1 = result.first.at<Rot3>(1);
296  const Rot3 rR2 = result.first.at<Rot3>(2);
297 
298  // rR0 = rRw * wR0 => rRw = rR0 * wR0.inverse()
299  // rR1 = rRw * wR1
300  // rR2 = rRw * wR2
301 
302  const Rot3 rRw = rR0 * wR0.inverse();
303  EXPECT(assert_equal(rRw * wR1, rR1, 0.1))
304  EXPECT(assert_equal(rRw * wR2, rR2, 0.1))
305 }
306 
307 /* ************************************************************************* */
308 TEST(ShonanAveraging3, runKlausKarcher) {
309  using namespace klaus;
310 
311  // Load 3 pose example taken in Klaus by Shicong
312  static const ShonanAveraging3 shonan = fromExampleName("Klaus3.g2o");
313 
314  // Run Shonan (with Karcher mean prior)
316  auto result = shonan.run(initial, 5);
317  EXPECT_DOUBLES_EQUAL(0, shonan.cost(result.first), 1e-2);
318  EXPECT_DOUBLES_EQUAL(-1.361402670507772e-05, result.second,
319  1e-4); // Regression test
320 
321  // Get Shonan solution in new frame R (R for result)
322  const Rot3 rR0 = result.first.at<Rot3>(0);
323  const Rot3 rR1 = result.first.at<Rot3>(1);
324  const Rot3 rR2 = result.first.at<Rot3>(2);
325 
326  const Rot3 rRw = rR0 * wR0.inverse();
327  EXPECT(assert_equal(rRw * wR1, rR1, 0.1))
328  EXPECT(assert_equal(rRw * wR2, rR2, 0.1))
329 }
330 
331 /* ************************************************************************* */
332 TEST(ShonanAveraging2, noisyToyGraph) {
333  // Load 2D toy example
334  auto lmParams = LevenbergMarquardtParams::CeresDefaults();
335  // lmParams.setVerbosityLM("SUMMARY");
336  string g2oFile = findExampleDataFile("noisyToyGraph.txt");
337  ShonanAveraging2::Parameters parameters(lmParams);
338  auto measurements = parseMeasurements<Rot2>(g2oFile);
339  ShonanAveraging2 shonan(measurements, parameters);
340  EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns());
341 
342  // Check graph building
343  NonlinearFactorGraph graph = shonan.buildGraphAt(2);
344  EXPECT_LONGS_EQUAL(6, graph.size());
345  auto initial = shonan.initializeRandomly(kRandomNumberGenerator);
346  auto result = shonan.run(initial, 2);
347  EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6);
348  EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
349 }
350 
351 /* ************************************************************************* */
352 TEST(ShonanAveraging2, noisyToyGraphWithHuber) {
353  // Load 2D toy example
354  auto lmParams = LevenbergMarquardtParams::CeresDefaults();
355  string g2oFile = findExampleDataFile("noisyToyGraph.txt");
356  ShonanAveraging2::Parameters parameters(lmParams);
357  auto measurements = parseMeasurements<Rot2>(g2oFile);
358  parameters.setUseHuber(true);
359  parameters.setCertifyOptimality(false);
360 
361  string parameters_print =
362  " ShonanAveragingParameters: \n alpha: 0\n beta: 1\n gamma: 0\n "
363  "useHuber: 1\n";
364  assert_print_equal(parameters_print, parameters);
365 
366  ShonanAveraging2 shonan(measurements, parameters);
367  EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns());
368 
369  // Check graph building
371  EXPECT_LONGS_EQUAL(6, graph.size());
372 
373  // test that each factor is actually robust
374  for (size_t i=0; i<=4; i++) { // note: last is the Gauge factor and is not robust
375  const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(
376  std::dynamic_pointer_cast<NoiseModelFactor>(graph[i])->noiseModel());
377  EXPECT(robust); // we expect the factors to be use a robust noise model (in particular, Huber)
378  }
379 
380  // test result
382  auto result = shonan.run(initial, 2,2);
383  EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6);
384  EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
385 }
386 
387 /* ************************************************************************* */
388 // Test alpha/beta/gamma prior weighting.
389 TEST(ShonanAveraging3, PriorWeights) {
390  auto lmParams = LevenbergMarquardtParams::CeresDefaults();
391  ShonanAveraging3::Parameters params(lmParams);
392  EXPECT_DOUBLES_EQUAL(0, params.alpha, 1e-9);
393  EXPECT_DOUBLES_EQUAL(1, params.beta, 1e-9);
394  EXPECT_DOUBLES_EQUAL(0, params.gamma, 1e-9);
395  double alpha = 100.0, beta = 200.0, gamma = 300.0;
396  params.setAnchorWeight(alpha);
397  params.setKarcherWeight(beta);
398  params.setGaugesWeight(gamma);
399  EXPECT_DOUBLES_EQUAL(alpha, params.alpha, 1e-9);
400  EXPECT_DOUBLES_EQUAL(beta, params.beta, 1e-9);
401  EXPECT_DOUBLES_EQUAL(gamma, params.gamma, 1e-9);
402  params.setKarcherWeight(0);
403  static const ShonanAveraging3 shonan = fromExampleName("Klaus3.g2o", params);
404  for (auto i : {0,1,2}) {
405  const auto& m = shonan.measurement(i);
406  auto isotropic =
407  std::static_pointer_cast<noiseModel::Isotropic>(m.noiseModel());
408  CHECK(isotropic != nullptr);
409  EXPECT_LONGS_EQUAL(3, isotropic->dim());
410  EXPECT_DOUBLES_EQUAL(0.2, isotropic->sigma(), 1e-9);
411  }
412  auto I = genericValue(Rot3());
413  Values initial{{0, I}, {1, I}, {2, I}};
414  EXPECT_DOUBLES_EQUAL(3.0756, shonan.cost(initial), 1e-4);
415  auto result = shonan.run(initial, 3, 3);
416  EXPECT_DOUBLES_EQUAL(0.0015, shonan.cost(result.first), 1e-4);
417 }
418 /* ************************************************************************* */
419 int main() {
420  TestResult tr;
421  return TestRegistry::runAllTests(tr);
422 }
423 /* ************************************************************************* */
Matrix3f m
Provides additional testing facilities for common data structures.
Index cols() const
Definition: SparseMatrix.h:140
#define CHECK(condition)
Definition: Test.h:108
#define I
Definition: main.h:112
Values initializeRandomly(std::mt19937 &rng) const
Sparse Q() const
Sparse version of Q.
static int runAllTests(TestResult &result)
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
const ValueType at(Key j) const
Definition: Values-inl.h:204
Index rows() const
Definition: SparseMatrix.h:138
#define min(a, b)
Definition: datatypes.h:19
Sparse computeLambda(const Matrix &S) const
Version that takes pxdN Stiefel manifold elements.
static std::mt19937 rng
std::pair< Values, double > run(const Values &initialEstimate, size_t pMin=d, size_t pMax=10) const
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
typename std::conditional< d==2, Rot2, Rot3 >::type Rot
double costAt(size_t p, const Values &values) const
Values initial
Values roundSolution(const Values &values) const
MatrixXd L
Definition: LLT_example.cpp:6
Definition: BFloat16.h:88
TEST(ShonanAveraging3, checkConstructor)
Matrix denseL() const
Dense version of L.
double computeMinEigenValue(const Values &values, Vector *minEigenVector=nullptr) const
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
Values tryOptimizingAt(size_t p, const Values &initial) const
static LevenbergMarquardtParams CeresDefaults()
DiscreteKey S(1, 2)
NonlinearFactorGraph graph
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Definition: SOn.h:54
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
double cost(const Values &values) const
bool checkOptimality(const Values &values) const
Sparse D() const
Sparse version of D.
static const SmartProjectionParams params
NonlinearFactorGraph buildGraphAt(size_t p) const
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
size_t size() const
Definition: FactorGraph.h:334
Sparse L() const
Sparse version of L.
static const ShonanAveraging3 kShonan
static std::mt19937 kRandomNumberGenerator(42)
Eigen::VectorXd Vector
Definition: Vector.h:38
const Rot3 wR0(0.9992281076190063, -0.02676080288219576, -0.024497002638379624, -0.015064701622500615)
Values result
const Rot3 wR1(0.998239108728862, -0.049543805396343954, -0.03232420352077356, -0.004386230477751116)
Matrix denseD() const
Dense version of D.
#define EXPECT(condition)
Definition: Test.h:150
Array< int, Dynamic, 1 > v
int main()
LevenbergMarquardtParams lmParams
RealScalar alpha
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ShonanAveragingParameters< 3 > ShonanAveragingParameters3
size_t size() const
Definition: Values.h:178
Various factors that minimize some Frobenius norm.
static ConjugateGradientParameters parameters
cout<< "The eigenvalues of A are:"<< endl<< ces.eigenvalues()<< endl;cout<< "The matrix of eigenvectors, V, is:"<< endl<< ces.eigenvectors()<< endl<< endl;complex< float > lambda
Vector xi
Definition: testPose2.cpp:148
const Rot & measured(size_t k) const
k^th measurement, as a Rot.
traits
Definition: chartTesting.h:28
GenericValue< T > genericValue(const T &v)
Definition: GenericValue.h:211
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
Sparse computeA(const Values &values) const
Compute A matrix whose Eigenvalues we will examine.
double computeMinEigenValueAP(const Values &values, Vector *minEigenVector=nullptr) const
The quaternion class used to represent 3D orientations and rotations.
Class between(const Class &g) const
Definition: Lie.h:52
size_t nrUnknowns() const
Return number of unknowns.
SO< Eigen::Dynamic > SOn
Definition: SOn.h:342
Annotation for function names.
Definition: attr.h:48
const BinaryMeasurement< Rot > & measurement(size_t k) const
k^th binary measurement
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
const EigenvalueType & eigenvalues() const
Returns the eigenvalues of given matrix.
Definition: EigenSolver.h:244
Computes eigenvalues and eigenvectors of general matrices.
Definition: EigenSolver.h:64
Matrix denseQ() const
Dense version of Q.
const Rot3 wR2(0.9925378735259738, -0.07993768981394891, 0.0825062894866454, -0.04088089479075661)
Shonan Averaging algorithm.
ShonanAveraging3 fromExampleName(const std::string &name, ShonanAveraging3::Parameters parameters=ShonanAveraging3::Parameters())
Values initializeWithDescent(size_t p, const Values &values, const Vector &minEigenVector, double minEigenValue, double gradienTolerance=1e-2, double preconditionedGradNormTolerance=1e-4) const


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:31