testFrobeniusFactor.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>
30 
32 
33 using namespace std;
34 using namespace gtsam;
35 
36 //******************************************************************************
37 namespace so3 {
38 SO3 id;
39 Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished();
40 SO3 R1 = SO3::Expmap(v1);
41 Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished();
42 SO3 R2 = SO3::Expmap(v2);
43 SO3 R12 = R1.between(R2);
44 } // namespace so3
45 
46 /* ************************************************************************* */
47 TEST(FrobeniusPriorSO3, evaluateError) {
48  using namespace ::so3;
49  auto factor = FrobeniusPrior<SO3>(1, R2.matrix());
50  Vector actual = factor.evaluateError(R1);
51  Vector expected = R1.vec() - R2.vec();
52  EXPECT(assert_equal(expected, actual, 1e-9));
53 
54  Values values;
55  values.insert(1, R1);
56  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
57 }
58 
59 /* ************************************************************************* */
60 TEST(FrobeniusPriorSO3, ClosestTo) {
61  // Example top-left of SO(4) matrix not quite on SO(3) manifold
62  Matrix3 M;
63  M << 0.79067393, 0.6051136, -0.0930814, //
64  0.4155925, -0.64214347, -0.64324489, //
65  -0.44948549, 0.47046326, -0.75917576;
66 
67  SO3 expected = SO3::ClosestTo(M);
68 
69  // manifold optimization gets same result as SVD solution in ClosestTo
72 
74  initial.insert(1, SO3(I_3x3));
78 }
79 
80 /* ************************************************************************* */
81 TEST(FrobeniusPriorSO3, ChordalL2mean) {
82  // See Hartley13ijcv:
83  // Cost function C(R) = \sum FrobeniusPrior(R,R_i)
84  // Closed form solution = ClosestTo(C_e), where
85  // C_e = \sum R_i !!!!
86 
87  // We will test by computing mean of R1=exp(v1) R1^T=exp(-v1):
88  using namespace ::so3;
89  SO3 expected; // identity
90  Matrix3 M = R1.matrix() + R1.matrix().transpose();
91  EXPECT(assert_equal(expected, SO3::ClosestTo(M), 1e-6));
92  EXPECT(assert_equal(expected, SO3::ChordalMean({R1, R1.inverse()}), 1e-6));
93 
94  // manifold optimization gets same result as ChordalMean
97  graph.emplace_shared<FrobeniusPrior<SO3> >(1, R1.matrix().transpose());
98 
100  initial.insert<SO3>(1, R1.inverse());
102  EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 0.1); // Why so loose?
104 }
105 
106 /* ************************************************************************* */
107 TEST(FrobeniusFactorSO3, evaluateError) {
108  using namespace ::so3;
109  auto factor = FrobeniusFactor<SO3>(1, 2);
110  Vector actual = factor.evaluateError(R1, R2);
111  Vector expected = R2.vec() - R1.vec();
112  EXPECT(assert_equal(expected, actual, 1e-9));
113 
114  Values values;
115  values.insert(1, R1);
116  values.insert(2, R2);
117  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
118 }
119 
120 /* ************************************************************************* */
121 // Commented out as SO(n) not yet supported (and might never be)
122 // TEST(FrobeniusBetweenFactorSOn, evaluateError) {
123 // using namespace ::so3;
124 // auto factor =
125 // FrobeniusBetweenFactor<SOn>(1, 2, SOn::FromMatrix(R12.matrix()));
126 // Vector actual = factor.evaluateError(SOn::FromMatrix(R1.matrix()),
127 // SOn::FromMatrix(R2.matrix()));
128 // Vector expected = Vector9::Zero();
129 // EXPECT(assert_equal(expected, actual, 1e-9));
130 
131 // Values values;
132 // values.insert(1, R1);
133 // values.insert(2, R2);
134 // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
135 // }
136 
137 /* ************************************************************************* */
138 TEST(FrobeniusBetweenFactorSO3, evaluateError) {
139  using namespace ::so3;
140  auto factor = FrobeniusBetweenFactor<SO3>(1, 2, R12);
141  Vector actual = factor.evaluateError(R1, R2);
142  Vector expected = Vector9::Zero();
143  EXPECT(assert_equal(expected, actual, 1e-9));
144 
145  Values values;
146  values.insert(1, R1);
147  values.insert(2, R2);
148  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
149 }
150 
151 //******************************************************************************
152 namespace so4 {
154 Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished();
156 Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished();
158 } // namespace so4
159 
160 /* ************************************************************************* */
161 TEST(FrobeniusFactorSO4, evaluateError) {
162  using namespace ::so4;
163  auto factor = FrobeniusFactor<SO4>(1, 2, noiseModel::Unit::Create(6));
164  Vector actual = factor.evaluateError(Q1, Q2);
165  Vector expected = Q2.vec() - Q1.vec();
166  EXPECT(assert_equal(expected, actual, 1e-9));
167 
168  Values values;
169  values.insert(1, Q1);
170  values.insert(2, Q2);
171  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
172 }
173 
174 /* ************************************************************************* */
175 TEST(FrobeniusBetweenFactorSO4, evaluateError) {
176  using namespace ::so4;
177  Matrix4 M{I_4x4};
178  M.topLeftCorner<3, 3>() = ::so3::R12.matrix();
179  auto factor = FrobeniusBetweenFactor<SO4>(1, 2, Q1.between(Q2));
180  Matrix H1, H2;
181  Vector actual = factor.evaluateError(Q1, Q2, H1, H2);
182  Vector expected = SO4::VectorN2::Zero();
183  EXPECT(assert_equal(expected, actual, 1e-9));
184 
185  Values values;
186  values.insert(1, Q1);
187  values.insert(2, Q2);
188  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
189 }
190 
191 /* ************************************************************************* */
192 int main() {
193  TestResult tr;
194  return TestRegistry::runAllTests(tr);
195 }
196 /* ************************************************************************* */
gtsam::SO3
SO< 3 > SO3
Definition: SO3.h:34
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
gtsam::SO::inverse
SO inverse() const
inverse of a rotation = transpose
Definition: SOn.h:195
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
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
gtsam::GaussNewtonOptimizer
Definition: GaussNewtonOptimizer.h:38
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
so4
Definition: testFrobeniusFactor.cpp:152
gtsam::FrobeniusFactor
Definition: FrobeniusFactor.h:81
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
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)
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::FrobeniusBetweenFactor
Definition: FrobeniusFactor.h:110
so3::R1
SO3 R1
Definition: testShonanFactor.cpp:41
so3
Definition: testShonanFactor.cpp:38
SO3.h
3*3 matrix representation of SO(3)
GaussNewtonOptimizer.h
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
id
static const Similarity3 id
Definition: testSimilarity3.cpp:44
so4::v1
Vector6 v1
Definition: testFrobeniusFactor.cpp:154
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:157
gtsam::FrobeniusPrior
Definition: FrobeniusFactor.h:51
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
TEST
TEST(FrobeniusPriorSO3, evaluateError)
Definition: testFrobeniusFactor.cpp:47
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
main
int main()
Definition: testFrobeniusFactor.cpp:192
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
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
so3::R2
SO3 R2
Definition: testShonanFactor.cpp:43
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
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
gtsam::SO< 3 >
initial
Definition: testScenarioRunner.cpp:148
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
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
gtsam::SO::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H={}) const
Definition: SOn-inl.h:88
FrobeniusFactor.h
Various factors that minimize some Frobenius norm.
so4::v2
Vector6 v2
Definition: testFrobeniusFactor.cpp:156
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:57