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

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:49:17