testReferenceFrameFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, 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 
12 /*
13  * @file testReferenceFrameFactor.cpp
14  * @author Alex Cunningham
15  */
16 
17 #include <iostream>
18 
19 #include <CppUnitLite/TestHarness.h>
20 
21 #include <gtsam/base/Testable.h>
22 #include <gtsam/base/numericalDerivative.h>
23 #include <gtsam/geometry/Pose2.h>
24 #include <gtsam/inference/Symbol.h>
25 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
26 #include <gtsam/nonlinear/NonlinearEquality.h>
27 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
28 
29 #include <gtsam/slam/ReferenceFrameFactor.h>
30 
31 using namespace std;
32 using namespace boost;
33 using namespace std::placeholders;
34 using namespace gtsam;
35 
36 typedef gtsam::ReferenceFrameFactor<gtsam::Point2, gtsam::Pose2> PointReferenceFrameFactor;
37 typedef gtsam::ReferenceFrameFactor<gtsam::Pose2, gtsam::Pose2> PoseReferenceFrameFactor;
38 
39 Key lA1 = symbol_shorthand::L(1), lA2 = symbol_shorthand::L(2), lB1 = symbol_shorthand::L(11), lB2 = symbol_shorthand::L(12);
40 Key tA1 = symbol_shorthand::T(1), tB1 = symbol_shorthand::T(2);
41 
42 /* ************************************************************************* */
43 TEST( ReferenceFrameFactor, equals ) {
44  PointReferenceFrameFactor
45  c1(lB1, tA1, lA1),
46  c2(lB1, tA1, lA1),
47  c3(lB1, tA1, lA2);
48 
49  EXPECT(assert_equal(c1, c1));
50  EXPECT(assert_equal(c1, c2));
51  EXPECT(!c1.equals(c3));
52 }
53 
54 /* ************************************************************************* */
55 Vector evaluateError_(const PointReferenceFrameFactor& c,
56  const Point2& global, const Pose2& trans, const Point2& local) {
57  return Vector(c.evaluateError(global, trans, local));
58 }
59 TEST( ReferenceFrameFactor, jacobians ) {
60 
61  // from examples below
62  Point2 local(2.0, 3.0), global(-1.0, 2.0);
63  Pose2 trans(1.5, 2.5, 0.3);
64 
65  PointReferenceFrameFactor tc(lA1, tA1, lB1);
66  Matrix actualDT, actualDL, actualDF;
67  tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
68 
69  Matrix numericalDT, numericalDL, numericalDF;
70  numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
71  std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
72  global, trans, local, 1e-5);
73  numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
74  std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
75  global, trans, local, 1e-5);
76  numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
77  std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
78  global, trans, local, 1e-5);
79 
80  EXPECT(assert_equal(numericalDF, actualDF));
81  EXPECT(assert_equal(numericalDL, actualDL));
82  EXPECT(assert_equal(numericalDT, actualDT));
83 }
84 
85 /* ************************************************************************* */
86 TEST( ReferenceFrameFactor, jacobians_zero ) {
87 
88  // get values that are ideal
89  Pose2 trans(2.0, 3.0, 0.0);
90  Point2 global(5.0, 6.0);
91  Point2 local = trans.transformFrom(global);
92 
93  PointReferenceFrameFactor tc(lA1, tA1, lB1);
94  Vector actCost = tc.evaluateError(global, trans, local),
95  expCost = Z_2x1;
96  EXPECT(assert_equal(expCost, actCost, 1e-5));
97 
98  Matrix actualDT, actualDL, actualDF;
99  tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
100 
101  Matrix numericalDT, numericalDL, numericalDF;
102  numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
103  std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
104  global, trans, local, 1e-5);
105  numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
106  std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
107  global, trans, local, 1e-5);
108  numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
109  std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
110  global, trans, local, 1e-5);
111 
112  EXPECT(assert_equal(numericalDF, actualDF));
113  EXPECT(assert_equal(numericalDL, actualDL));
114  EXPECT(assert_equal(numericalDT, actualDT));
115 }
116 
117 /* ************************************************************************* */
118 TEST( ReferenceFrameFactor, converge_trans ) {
119 
120  // initial points
121  Point2 local1(2.0, 2.0), local2(4.0, 5.0),
122  global1(-1.0, 5.0), global2(2.0, 3.0);
123  Pose2 transIdeal(7.0, 3.0, M_PI/2);
124 
125  // verify direction
126  EXPECT(assert_equal(local1, transIdeal.transformFrom(global1)));
127  EXPECT(assert_equal(local2, transIdeal.transformFrom(global2)));
128 
129  // choose transform
130  // Pose2 trans = transIdeal; // ideal - works
131  // Pose2 trans = transIdeal * Pose2(0.1, 1.0, 0.00); // translation - works
132  // Pose2 trans = transIdeal * Pose2(10.1, 1.0, 0.00); // large translation - works
133  // Pose2 trans = transIdeal * Pose2(0.0, 0.0, 0.1); // small rotation - works
134  Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 1.3); // combined - works
135  // Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails
136 
137  NonlinearFactorGraph graph;
138  graph.emplace_shared<PointReferenceFrameFactor>(lB1, tA1, lA1);
139  graph.emplace_shared<PointReferenceFrameFactor>(lB2, tA1, lA2);
140 
141  // hard constraints on points
142  double error_gain = 1000.0;
143  graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lA1, local1, error_gain);
144  graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lA2, local2, error_gain);
145  graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lB1, global1, error_gain);
146  graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lB2, global2, error_gain);
147 
148  // create initial estimate
149  Values init;
150  init.insert(lA1, local1);
151  init.insert(lA2, local2);
152  init.insert(lB1, global1);
153  init.insert(lB2, global2);
154  init.insert(tA1, trans);
155 
156  // optimize
157  LevenbergMarquardtOptimizer solver(graph, init);
158  Values actual = solver.optimize();
159 
160  Values expected;
161  expected.insert(lA1, local1);
162  expected.insert(lA2, local2);
163  expected.insert(lB1, global1);
164  expected.insert(lB2, global2);
165  expected.insert(tA1, transIdeal);
166 
167  EXPECT(assert_equal(expected, actual, 1e-4));
168 }
169 
170 /* ************************************************************************* */
171 TEST( ReferenceFrameFactor, converge_local ) {
172 
173  // initial points
174  Point2 global(-1.0, 2.0);
175  // Pose2 trans(1.5, 2.5, 0.3); // original
176  // Pose2 trans(1.5, 2.5, 1.0); // larger rotation
177  Pose2 trans(1.5, 2.5, 3.1); // significant rotation
178 
179  Point2 idealLocal = trans.transformFrom(global);
180 
181  // perturb the initial estimate
182  // Point2 local = idealLocal; // Ideal case - works
183  // Point2 local = idealLocal + Point2(1.0, 0.0); // works
184  Point2 local = idealLocal + Point2(-10.0, 10.0); // works
185 
186  NonlinearFactorGraph graph;
187  double error_gain = 1000.0;
188  graph.emplace_shared<PointReferenceFrameFactor>(lB1, tA1, lA1);
189  graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lB1, global, error_gain);
190  graph.emplace_shared<NonlinearEquality<gtsam::Pose2> >(tA1, trans, error_gain);
191 
192  // create initial estimate
193  Values init;
194  init.insert(lA1, local);
195  init.insert(lB1, global);
196  init.insert(tA1, trans);
197 
198  // optimize
199  LevenbergMarquardtOptimizer solver(graph, init);
200  Values actual = solver.optimize();
201 
202  CHECK(actual.exists(lA1));
203  EXPECT(assert_equal(idealLocal, actual.at<Point2>(lA1), 1e-5));
204 }
205 
206 /* ************************************************************************* */
207 TEST( ReferenceFrameFactor, converge_global ) {
208 
209  // initial points
210  Point2 local(2.0, 3.0);
211  // Pose2 trans(1.5, 2.5, 0.3); // original
212  // Pose2 trans(1.5, 2.5, 1.0); // larger rotation
213  Pose2 trans(1.5, 2.5, 3.1); // significant rotation
214 
215  Point2 idealForeign = trans.inverse().transformFrom(local);
216 
217  // perturb the initial estimate
218  // Point2 global = idealForeign; // Ideal - works
219  // Point2 global = idealForeign + Point2(1.0, 0.0); // simple - works
220  Point2 global = idealForeign + Point2(10.0, -10.0); // larger - works
221 
222  NonlinearFactorGraph graph;
223  double error_gain = 1000.0;
224  graph.emplace_shared<PointReferenceFrameFactor>(lB1, tA1, lA1);
225  graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lA1, local, error_gain);
226  graph.emplace_shared<NonlinearEquality<gtsam::Pose2> >(tA1, trans, error_gain);
227 
228  // create initial estimate
229  Values init;
230  init.insert(lA1, local);
231  init.insert(lB1, global);
232  init.insert(tA1, trans);
233 
234  // optimize
235  LevenbergMarquardtOptimizer solver(graph, init);
236  Values actual = solver.optimize();
237 
238  // verify
239  CHECK(actual.exists(lB1));
240  EXPECT(assert_equal(idealForeign, actual.at<Point2>(lB1), 1e-5));
241 }
242 
243 /* ************************************************************************* */
244 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
245 /* ************************************************************************* */
246 
247 
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::Pose2::transformFrom
GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
Definition: Pose2.cpp:226
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
PointReferenceFrameFactor
gtsam::ReferenceFrameFactor< gtsam::Point2, gtsam::Pose2 > PointReferenceFrameFactor
Definition: testReferenceFrameFactor.cpp:36
Testable.h
Concept check for values that can be used in unit tests.
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:204
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
M_PI
#define M_PI
Definition: main.h:106
L
MatrixXd L
Definition: LLT_example.cpp:6
std
Definition: BFloat16.h:88
solver
BiCGSTAB< SparseMatrix< double > > solver
Definition: BiCGSTAB_simple.cpp:5
trans
static char trans
Definition: blas_interface.hh:58
numericalDerivative.h
Some functions to compute numerical derivatives.
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::ReferenceFrameFactor::evaluateError
Vector evaluateError(const Point &global, const Transform &trans, const Point &local, OptionalMatrixType Dforeign, OptionalMatrixType Dtrans, OptionalMatrixType Dlocal) const override
Definition: ReferenceFrameFactor.h:100
gtsam
traits
Definition: chartTesting.h:28
TEST
TEST(ReferenceFrameFactor, equals)
Definition: testReferenceFrameFactor.cpp:43
init
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:1882
evaluateError_
Vector evaluateError_(const PointReferenceFrameFactor &c, const Point2 &global, const Pose2 &trans, const Point2 &local)
Definition: testReferenceFrameFactor.cpp:55
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::Pose2::inverse
GTSAM_EXPORT Pose2 inverse() const
inverse
Definition: Pose2.cpp:201
Pose2.h
2D Pose
gtsam::Values::exists
bool exists(Key j) const
Definition: Values.cpp:93
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::Z_2x1
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:45
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
PoseReferenceFrameFactor
gtsam::ReferenceFrameFactor< gtsam::Pose2, gtsam::Pose2 > PoseReferenceFrameFactor
Definition: testReferenceFrameFactor.cpp:37

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