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) {
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) {
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
123  auto lambdaMin = kShonan.computeMinEigenValue(result);
124  EXPECT_DOUBLES_EQUAL(-5.427688831332745e-07, lambdaMin,
125  1e-4); // Regression test
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);
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));
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);
340  EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns());
341 
342  // Check graph building
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 
367  EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns());
368 
369  // Check graph building
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>(
377  // we expect the factors to be use a robust noise model
378  // (in particular, Huber)
379  EXPECT(robust);
380  }
381 
382  // test result
384  auto result = shonan.run(initial, 2,2);
385  EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6);
386  EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
387 }
388 
389 /* ************************************************************************* */
390 // Test alpha/beta/gamma prior weighting.
391 TEST(ShonanAveraging3, PriorWeights) {
392  auto lmParams = LevenbergMarquardtParams::CeresDefaults();
393  ShonanAveraging3::Parameters params(lmParams);
394  EXPECT_DOUBLES_EQUAL(0, params.alpha, 1e-9);
395  EXPECT_DOUBLES_EQUAL(1, params.beta, 1e-9);
396  EXPECT_DOUBLES_EQUAL(0, params.gamma, 1e-9);
397  double alpha = 100.0, beta = 200.0, gamma = 300.0;
398  params.setAnchorWeight(alpha);
399  params.setKarcherWeight(beta);
400  params.setGaugesWeight(gamma);
401  EXPECT_DOUBLES_EQUAL(alpha, params.alpha, 1e-9);
402  EXPECT_DOUBLES_EQUAL(beta, params.beta, 1e-9);
403  EXPECT_DOUBLES_EQUAL(gamma, params.gamma, 1e-9);
404  params.setKarcherWeight(0);
405  static const ShonanAveraging3 shonan = fromExampleName("Klaus3.g2o", params);
406  for (auto i : {0,1,2}) {
407  const auto& m = shonan.measurement(i);
408  auto isotropic =
409  std::static_pointer_cast<noiseModel::Isotropic>(m.noiseModel());
410  CHECK(isotropic != nullptr);
411  EXPECT_LONGS_EQUAL(3, isotropic->dim());
412  EXPECT_DOUBLES_EQUAL(0.2, isotropic->sigma(), 1e-9);
413  }
414  auto I = genericValue(Rot3());
415  Values initial{{0, I}, {1, I}, {2, I}};
416  EXPECT_DOUBLES_EQUAL(3.0756, shonan.cost(initial), 1e-4);
417  auto result = shonan.run(initial, 3, 3);
418  EXPECT_DOUBLES_EQUAL(0.0015, shonan.cost(result.first), 1e-4);
419 }
420 
421 /* ************************************************************************* */
422 // Check a small graph created using binary measurements
423 TEST(ShonanAveraging3, BinaryMeasurements) {
424  std::vector<BinaryMeasurement<Rot3>> measurements;
425  auto unit3 = noiseModel::Unit::Create(3);
426  measurements.emplace_back(0, 1, Rot3::Yaw(M_PI_2), unit3);
427  measurements.emplace_back(1, 2, Rot3::Yaw(M_PI_2), unit3);
429  Values initial = shonan.initializeRandomly();
430  auto result = shonan.run(initial, 3, 5);
431  EXPECT_DOUBLES_EQUAL(0.0, shonan.cost(result.first), 1e-4);
432 }
433 
434 /* ************************************************************************* */
435 int main() {
436  TestResult tr;
437  return TestRegistry::runAllTests(tr);
438 }
439 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
main
int main()
Definition: testShonanAveraging.cpp:435
gtsam::ShonanAveraging::computeMinEigenValueAP
double computeMinEigenValueAP(const Values &values, Vector *minEigenVector=nullptr) const
Definition: ShonanAveraging.cpp:730
name
Annotation for function names.
Definition: attr.h:51
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
D
MatrixXcd D
Definition: EigenSolver_EigenSolver_MatrixType.cpp:14
gtsam.examples.DogLegOptimizerExample.type
type
Definition: DogLegOptimizerExample.py:111
ShonanAveraging.h
Shonan Averaging algorithm.
gtsam::ShonanAveraging::measurement
const BinaryMeasurement< Rot > & measurement(size_t k) const
k^th binary measurement
Definition: ShonanAveraging.h:171
gtsam::Values::size
size_t size() const
Definition: Values.h:178
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
gtsam::ShonanAveraging::computeLambda
Sparse computeLambda(const Matrix &S) const
Version that takes pxdN Stiefel manifold elements.
Definition: ShonanAveraging.cpp:437
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
gtsam::ShonanAveraging::checkOptimality
bool checkOptimality(const Values &values) const
Definition: ShonanAveraging.cpp:756
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::assert_print_equal
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
Definition: TestableAssertions.h:352
gtsam::ShonanAveraging::measured
const Rot & measured(size_t k) const
k^th measurement, as a Rot.
Definition: ShonanAveraging.h:206
klaus
Definition: testShonanAveraging.cpp:240
lmParams
LevenbergMarquardtParams lmParams
Definition: testSmartProjectionRigFactor.cpp:55
gtsam::ShonanAveraging::roundSolution
Values roundSolution(const Values &values) const
Definition: ShonanAveraging.cpp:302
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::genericValue
GenericValue< T > genericValue(const T &v)
Definition: GenericValue.h:211
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
beta
double beta(double a, double b)
Definition: beta.c:61
kShonan
static const ShonanAveraging3 kShonan
Definition: testShonanAveraging.cpp:46
Rot
typename std::conditional< d==2, Rot2, Rot3 >::type Rot
Definition: testShonanAveraging.cpp:34
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::ShonanAveraging::initializeRandomly
Values initializeRandomly(std::mt19937 &rng) const
Definition: ShonanAveraging.cpp:859
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:306
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
gtsam::ShonanAveraging::computeA
Sparse computeA(const Values &values) const
Compute A matrix whose Eigenvalues we will examine.
Definition: ShonanAveraging.cpp:477
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::SOn
SO< Eigen::Dynamic > SOn
Definition: SOn.h:342
I
#define I
Definition: main.h:112
BetweenFactor.h
gtsam::ShonanAveraging::initializeWithDescent
Values initializeWithDescent(size_t p, const Values &values, const Vector &minEigenVector, double minEigenValue, double gradienTolerance=1e-2, double preconditionedGradNormTolerance=1e-4) const
Definition: ShonanAveraging.cpp:820
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
klaus::wR2
const Rot3 wR2(0.9925378735259738, -0.07993768981394891, 0.0825062894866454, -0.04088089479075661)
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:245
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
fromExampleName
ShonanAveraging3 fromExampleName(const std::string &name, ShonanAveraging3::Parameters parameters=ShonanAveraging3::Parameters())
Definition: testShonanAveraging.cpp:39
gtsam::ShonanAveraging::computeMinEigenValue
double computeMinEigenValue(const Values &values, Vector *minEigenVector=nullptr) const
Definition: ShonanAveraging.cpp:713
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::ShonanAveraging3
Definition: ShonanAveraging.h:438
L
MatrixXd L
Definition: LLT_example.cpp:6
gtsam.examples.DogLegOptimizerExample.run
def run(args)
Definition: DogLegOptimizerExample.py:21
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
TEST
TEST(ShonanAveraging3, checkConstructor)
Definition: testShonanAveraging.cpp:51
gamma
#define gamma
Definition: mconf.h:85
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
lambda
static double lambda[]
Definition: jv.c:524
TestResult
Definition: TestResult.h:26
M_PI_2
#define M_PI_2
Definition: mconf.h:118
gtsam::ShonanAveraging::nrUnknowns
size_t nrUnknowns() const
Return number of unknowns.
Definition: ShonanAveraging.h:165
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
gtsam::ShonanAveraging::costAt
double costAt(size_t p, const Values &values) const
Definition: ShonanAveraging.cpp:163
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
klaus::wR1
const Rot3 wR1(0.998239108728862, -0.049543805396343954, -0.03232420352077356, -0.004386230477751116)
gtsam
traits
Definition: SFMdata.h:40
gtsam::LevenbergMarquardtParams::CeresDefaults
static LevenbergMarquardtParams CeresDefaults()
Definition: LevenbergMarquardtParams.h:106
gtsam::ShonanAveragingParameters3
ShonanAveragingParameters< 3 > ShonanAveragingParameters3
Definition: ShonanAveraging.h:105
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
gtsam::NoiseModelFactor
Definition: NonlinearFactor.h:197
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
gtsam::ShonanAveraging::run
std::pair< Values, double > run(const Values &initialEstimate, size_t pMin=d, size_t pMax=10) const
Definition: ShonanAveraging.cpp:889
kRandomNumberGenerator
static std::mt19937 kRandomNumberGenerator(42)
Eigen::EigenSolver
Computes eigenvalues and eigenvectors of general matrices.
Definition: EigenSolver.h:64
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
min
#define min(a, b)
Definition: datatypes.h:19
gtsam::SO
Definition: SOn.h:54
initial
Definition: testScenarioRunner.cpp:148
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
gtsam::ShonanAveraging::cost
double cost(const Values &values) const
Definition: ShonanAveraging.cpp:311
gtsam::ShonanAveraging::tryOptimizingAt
Values tryOptimizingAt(size_t p, const Values &initial) const
Definition: ShonanAveraging.cpp:190
gtsam::ShonanAveraging::buildGraphAt
NonlinearFactorGraph buildGraphAt(size_t p) const
Definition: ShonanAveraging.cpp:138
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
so3::R12
SO3 R12
Definition: testShonanFactor.cpp:44
klaus::wR0
const Rot3 wR0(0.9992281076190063, -0.02676080288219576, -0.024497002638379624, -0.015064701622500615)
FrobeniusFactor.h
Various factors that minimize some Frobenius norm.
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
S
DiscreteKey S(1, 2)
unit3
static Unit3 unit3(1.0, 2.1, 3.4)
gtsam::Pose2
Definition: Pose2.h:39
gtsam::ShonanAveraging2
Definition: ShonanAveraging.h:428
Eigen::EigenSolver::eigenvalues
const EigenvalueType & eigenvalues() const
Returns the eigenvalues of given matrix.
Definition: EigenSolver.h:244


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