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 
20 
21 #include <gtsam/base/Testable.h>
23 #include <gtsam/geometry/Pose2.h>
24 #include <gtsam/inference/Symbol.h>
28 
30 
31 using namespace std;
32 using namespace boost;
33 using namespace std::placeholders;
34 using namespace gtsam;
35 
38 
41 
42 /* ************************************************************************* */
45  c1(lB1, tA1, lA1),
46  c2(lB1, tA1, lA1),
47  c3(lB1, tA1, lA2);
48 
51  EXPECT(!c1.equals(c3));
52 }
53 
54 /* ************************************************************************* */
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 
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 
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 
140 
141  // hard constraints on points
142  double error_gain = 1000.0;
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
158  Values actual = solver.optimize();
159 
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 
187  double error_gain = 1000.0;
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
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 
223  double error_gain = 1000.0;
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
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 
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
tA1
Key tA1
Definition: testReferenceFrameFactor.cpp:40
Pose2.h
2D Pose
gtsam::Values::exists
bool exists(Key j) const
Definition: Values.cpp:93
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
lA2
Key lA2
Definition: testReferenceFrameFactor.cpp:39
tB1
Key tB1
Definition: testReferenceFrameFactor.cpp:40
trans
static char trans
Definition: blas_interface.hh:58
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
boost
Definition: boostmultiprec.cpp:109
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::NonlinearEquality
Definition: NonlinearEquality.h:43
solver
BiCGSTAB< SparseMatrix< double > > solver
Definition: BiCGSTAB_simple.cpp:5
c1
static double c1
Definition: airy.c:54
PoseReferenceFrameFactor
gtsam::ReferenceFrameFactor< gtsam::Pose2, gtsam::Pose2 > PoseReferenceFrameFactor
Definition: testReferenceFrameFactor.cpp:37
PointReferenceFrameFactor
gtsam::ReferenceFrameFactor< gtsam::Point2, gtsam::Pose2 > PointReferenceFrameFactor
Definition: testReferenceFrameFactor.cpp:36
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
numericalDerivative.h
Some functions to compute numerical derivatives.
TEST
TEST(ReferenceFrameFactor, equals)
Definition: testReferenceFrameFactor.cpp:43
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::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
lB2
Key lB2
Definition: testReferenceFrameFactor.cpp:39
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam::ReferenceFrameFactor
Definition: ReferenceFrameFactor.h:57
L
MatrixXd L
Definition: LLT_example.cpp:6
Symbol.h
evaluateError_
Vector evaluateError_(const PointReferenceFrameFactor &c, const Point2 &global, const Pose2 &trans, const Point2 &local)
Definition: testReferenceFrameFactor.cpp:55
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
NonlinearEquality.h
init
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:2006
gtsam::equals
Definition: Testable.h:112
TestResult
Definition: TestResult.h:26
gtsam
traits
Definition: SFMdata.h:40
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
lB1
Key lB1
Definition: testReferenceFrameFactor.cpp:39
c2
static double c2
Definition: airy.c:55
gtsam::Z_2x1
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:45
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
ReferenceFrameFactor.h
gtsam::Pose2::transformFrom
Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
Definition: Pose2.cpp:226
M_PI
#define M_PI
Definition: mconf.h:117
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
main
int main()
Definition: testReferenceFrameFactor.cpp:244
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
init
Definition: TutorialInplaceLU.cpp:2
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
lA1
Key lA1
Definition: testReferenceFrameFactor.cpp:39
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Pose2
Definition: Pose2.h:39


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