testShonanFactor.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 
21 #include <gtsam/base/lieProxies.h>
22 #include <gtsam/base/testLie.h>
23 #include <gtsam/geometry/Rot3.h>
24 #include <gtsam/geometry/SO3.h>
25 #include <gtsam/geometry/SO4.h>
29 #include <gtsam/sfm/ShonanFactor.h>
31 
33 
34 using namespace std;
35 using namespace gtsam;
36 
37 //******************************************************************************
38 namespace so3 {
40 Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished();
42 Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished();
45 } // namespace so3
46 
47 //******************************************************************************
48 namespace submanifold {
50 Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
51 SO3 R1 = SO3::Expmap(v1.tail<3>());
53 Vector6 v2 = (Vector(6) << 0, 0, 0, 0.01, 0.02, 0.03).finished();
54 SO3 R2 = SO3::Expmap(v2.tail<3>());
57 } // namespace submanifold
58 
59 /* ************************************************************************* */
60 TEST(ShonanFactor3, evaluateError) {
61  auto model = noiseModel::Isotropic::Sigma(3, 1.2);
62  for (const size_t p : {5, 4, 3}) {
63  Matrix M = Matrix::Identity(p, p);
64  M.topLeftCorner(3, 3) = submanifold::R1.matrix();
65  SOn Q1(M);
66  M.topLeftCorner(3, 3) = submanifold::R2.matrix();
67  SOn Q2(M);
68  auto factor = ShonanFactor3(1, 2, Rot3(::so3::R12.matrix()), p, model);
69  Matrix H1, H2;
70  factor.evaluateError(Q1, Q2, H1, H2);
71 
72  // Test derivatives
73  Values values;
74  values.insert(1, Q1);
75  values.insert(2, Q2);
76  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
77  }
78 }
79 
80 /* ************************************************************************* */
81 TEST(ShonanFactor3, equivalenceToSO3) {
82  using namespace ::submanifold;
83  auto R12 = ::so3::R12.retract(Vector3(0.1, 0.2, -0.1));
84  auto model = noiseModel::Isotropic::Sigma(6, 1.2); // wrong dimension
86  auto factor4 = ShonanFactor3(1, 2, Rot3(R12.matrix()), 4, model);
87  const Matrix3 E3(factor3.evaluateError(R1, R2).data());
88  const Matrix43 E4(
89  factor4.evaluateError(SOn(Q1.matrix()), SOn(Q2.matrix())).data());
90  EXPECT(assert_equal((Matrix)E4.topLeftCorner<3, 3>(), E3, 1e-9));
91  EXPECT(assert_equal((Matrix)E4.row(3), Matrix13::Zero(), 1e-9));
92 }
93 
94 /* ************************************************************************* */
95 TEST(ShonanFactor2, evaluateError) {
96  auto model = noiseModel::Isotropic::Sigma(1, 1.2);
97  const Rot2 R1(0.3), R2(0.5), R12(0.2);
98  for (const size_t p : {5, 4, 3, 2}) {
99  Matrix M = Matrix::Identity(p, p);
100  M.topLeftCorner(2, 2) = R1.matrix();
101  SOn Q1(M);
102  M.topLeftCorner(2, 2) = R2.matrix();
103  SOn Q2(M);
104  auto factor = ShonanFactor2(1, 2, R12, p, model);
105  Matrix H1, H2;
106  factor.evaluateError(Q1, Q2, H1, H2);
107 
108  // Test derivatives
109  Values values;
110  values.insert(1, Q1);
111  values.insert(2, Q2);
112  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
113  }
114 }
115 
116 /* ************************************************************************* */
117 int main() {
118  TestResult tr;
119  return TestRegistry::runAllTests(tr);
120 }
121 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
EXPECT_CORRECT_FACTOR_JACOBIANS
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
Definition: factorTesting.h:114
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
submanifold
Definition: testShonanFactor.cpp:48
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
ShonanFactor.h
Main factor type in Shonan averaging, on SO(n) pairs.
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
testLie.h
SO4.h
4*4 matrix representation of SO(4)
submanifold::R2
SO3 R2
Definition: testShonanFactor.cpp:54
submanifold::R12
SO3 R12
Definition: testShonanFactor.cpp:56
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::FrobeniusBetweenFactor
Definition: FrobeniusFactor.h:110
so3
Definition: testShonanFactor.cpp:38
SO3.h
3*3 matrix representation of SO(3)
GaussNewtonOptimizer.h
gtsam::ShonanFactor2
ShonanFactor< 2 > ShonanFactor2
Definition: ShonanFactor.h:89
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
id
static const Similarity3 id
Definition: testSimilarity3.cpp:44
TEST
TEST(ShonanFactor3, evaluateError)
Definition: testShonanFactor.cpp:60
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:157
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
submanifold::R1
SO3 R1
Definition: testShonanFactor.cpp:51
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
gtsam::ShonanFactor
Definition: ShonanFactor.h:36
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
TestResult
Definition: TestResult.h:26
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
Q2
static double Q2[8]
Definition: ndtri.c:122
simple_graph::factor3
auto factor3
Definition: testJacobianFactor.cpp:210
gtsam::Rot2
Definition: Rot2.h:35
submanifold::v1
Vector6 v1
Definition: testShonanFactor.cpp:50
gtsam
traits
Definition: SFMdata.h:40
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::ShonanFactor3
ShonanFactor< 3 > ShonanFactor3
Definition: ShonanFactor.h:90
Q1
static double Q1[8]
Definition: ndtri.c:94
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
main
int main()
Definition: testShonanFactor.cpp:117
gtsam::SO< 3 >
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
submanifold::v2
Vector6 v2
Definition: testShonanFactor.cpp:53
lieProxies.h
Provides convenient mappings of common member functions for testing.
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
so3::R12
SO3 R12
Definition: testShonanFactor.cpp:44
FrobeniusFactor.h
Various factors that minimize some Frobenius norm.
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:36