testPoseBetweenFactor.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 
19 #include <gtsam_unstable/slam/PoseBetweenFactor.h>
20 #include <gtsam/inference/Symbol.h>
21 #include <gtsam/geometry/Pose3.h>
22 #include <gtsam/base/numericalDerivative.h>
23 #include <gtsam/base/TestableAssertions.h>
24 #include <CppUnitLite/TestHarness.h>
25 
26 using namespace std;
27 using namespace gtsam;
28 
29 typedef PoseBetweenFactor<Pose3> TestPoseBetweenFactor;
30 
32 namespace gtsam {
33 template<>
34 struct traits<TestPoseBetweenFactor> : public Testable<TestPoseBetweenFactor> {
35 };
36 }
37 
38 /* ************************************************************************* */
39 TEST( PoseBetweenFactor, Constructor) {
40  Key poseKey1(1);
41  Key poseKey2(2);
42  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
43  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
44  TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model);
45 }
46 
47 /* ************************************************************************* */
48 TEST( PoseBetweenFactor, ConstructorWithTransform) {
49  Key poseKey1(1);
50  Key poseKey2(2);
51  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
52  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
53  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
54  TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model, body_P_sensor);
55 }
56 
57 /* ************************************************************************* */
58 TEST( PoseBetweenFactor, Equals ) {
59  // Create two identical factors and make sure they're equal
60  Key poseKey1(1);
61  Key poseKey2(2);
62  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
63  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
64  TestPoseBetweenFactor factor1(poseKey1, poseKey2, measurement, model);
65  TestPoseBetweenFactor factor2(poseKey1, poseKey2, measurement, model);
66 
67  CHECK(assert_equal(factor1, factor2));
68 
69  // Create a third, different factor and check for inequality
70  Pose3 measurement2(Rot3::RzRyRx(0.20, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
71  TestPoseBetweenFactor factor3(poseKey1, poseKey2, measurement2, model);
72 
73  CHECK(assert_inequal(factor1, factor3));
74 }
75 
76 /* ************************************************************************* */
77 TEST( PoseBetweenFactor, EqualsWithTransform ) {
78  // Create two identical factors and make sure they're equal
79  Key poseKey1(1);
80  Key poseKey2(2);
81  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
82  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
83  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
84  TestPoseBetweenFactor factor1(poseKey1, poseKey2, measurement, model, body_P_sensor);
85  TestPoseBetweenFactor factor2(poseKey1, poseKey2, measurement, model, body_P_sensor);
86 
87  CHECK(assert_equal(factor1, factor2));
88 
89  // Create a third, different factor and check for inequality
90  Pose3 body_P_sensor2(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.30, -0.10, 1.0));
91  TestPoseBetweenFactor factor3(poseKey1, poseKey2, measurement, model, body_P_sensor2);
92 
93  CHECK(assert_inequal(factor1, factor3));
94 }
95 
96 /* ************************************************************************* */
97 TEST( PoseBetweenFactor, Error ) {
98  // Create the measurement and linearization point
99  Pose3 measurement(Rot3::RzRyRx(0.15, 0.15, -0.20), Point3(+0.5, -1.0, +1.0));
100  Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
101  Pose3 pose2(Rot3::RzRyRx(0.15, 0.00, 0.20), Point3(-3.5, 6.0, -9.0));
102 
103  // The expected error
104  Vector expectedError(6);
105  // The solution depends on choice of Pose3 and Rot3 Expmap mode!
106 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
107  expectedError << -0.0298135267953815,
108  0.0131341515747393,
109  0.0968868439682154,
110 #if defined(GTSAM_POSE3_EXPMAP)
111  -0.145701634472172,
112  -0.134898525569125,
113  -0.0421026389164264;
114 #else
115  -0.13918755,
116  -0.142346243,
117  -0.0390885321;
118 #endif
119 #else
120  expectedError << -0.029839512616488,
121  0.013145599455949,
122  0.096971291682578,
123  -0.139187549519821,
124  -0.142346243063553,
125  -0.039088532100977;
126 #endif
127 
128  // Create a factor and calculate the error
129  Key poseKey1(1);
130  Key poseKey2(2);
131  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
132  TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model);
133  Vector actualError(factor.evaluateError(pose1, pose2));
134 
135  // Verify we get the expected error
136  CHECK(assert_equal(expectedError, actualError, 1e-9));
137 }
138 
139 /* ************************************************************************* */
140 TEST( PoseBetweenFactor, ErrorWithTransform ) {
141  // Create the measurement and linearization point
142  Pose3 measurement(Rot3::RzRyRx(-0.15, 0.10, 0.15), Point3(+1.25, -0.90, +.45));
143  Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
144  Pose3 pose2(Rot3::RzRyRx(0.15, 0.00, 0.20), Point3(-3.5, 6.0, -9.0));
145  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
146 
147  // The expected error
148  Vector expectedError(6);
149  // The solution depends on choice of Pose3 and Rot3 Expmap mode!
150 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
151  expectedError << 0.0173358202010741,
152  0.0222210698409755,
153  -0.0125032003886145,
154 #if defined(GTSAM_POSE3_EXPMAP)
155  0.0263800787416566,
156  0.00540285006310398,
157  0.000175859555693563;
158 #else
159  0.0264132886,
160  0.0052376953,
161  -7.16127036e-05;
162 #endif
163 #else
164  expectedError << 0.017337193670445,
165  0.022222830355243,
166  -0.012504190982804,
167  0.026413288603739,
168  0.005237695303536,
169  -0.000071612703633;
170 #endif
171 
172 
173  // Create a factor and calculate the error
174  Key poseKey1(1);
175  Key poseKey2(2);
176  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
177  TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model, body_P_sensor);
178  Vector actualError(factor.evaluateError(pose1, pose2));
179 
180  // Verify we get the expected error
181  CHECK(assert_equal(expectedError, actualError, 1e-9));
182 }
183 
184 /* ************************************************************************* */
185 TEST( PoseBetweenFactor, Jacobian ) {
186  // Create a factor
187  Key poseKey1(1);
188  Key poseKey2(2);
189  Pose3 measurement(Rot3::RzRyRx(0.15, 0.15, -0.20), Point3(+0.5, -1.0, +1.0));
190  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
191  TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model);
192 
193  // Create a linearization point at the zero-error point
194  Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
195  Pose3 pose2(Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840),
196  Point3(-3.37493895, 6.14660244, -8.93650986));
197 
198  // Calculate numerical derivatives
199  Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
200  Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
201 
202  // Use the factor to calculate the derivative
203  Matrix actualH1;
204  Matrix actualH2;
205  factor.evaluateError(pose1, pose2, actualH1, actualH2);
206 
207  // Verify we get the expected error
208  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
209  CHECK(assert_equal(expectedH2, actualH2, 1e-6));
210 }
211 
212 /* ************************************************************************* */
213 TEST( PoseBetweenFactor, JacobianWithTransform ) {
214  // Create a factor
215  Key poseKey1(1);
216  Key poseKey2(2);
217  Pose3 measurement(Rot3::RzRyRx(-0.15, 0.10, 0.15), Point3(+1.25, -0.90, +.45));
218  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
219  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
220  TestPoseBetweenFactor factor(poseKey1, poseKey2, measurement, model, body_P_sensor);
221 
222  // Create a linearization point at the zero-error point
223  Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
224  Pose3 pose2(Rot3::RzRyRx(0.162672458989103, 0.013665177349534, 0.224649482928184),
225  Point3(-3.5257579, 6.02637531, -8.98382384));
226 
227  // Calculate numerical derivatives
228  Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
229  Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
230 
231  // Use the factor to calculate the derivative
232  Matrix actualH1;
233  Matrix actualH2;
234  Vector error = factor.evaluateError(pose1, pose2, actualH1, actualH2);
235 
236  // Verify we get the expected error
237  CHECK(assert_equal(expectedH1, actualH1, 1e-6));
238  CHECK(assert_equal(expectedH2, actualH2, 1e-5));
239 }
240 
241 /* ************************************************************************* */
242 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
243 /* ************************************************************************* */
244 
TestableAssertions.h
Provides additional testing facilities for common data structures.
CHECK
#define CHECK(condition)
Definition: Test.h:109
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
measurement
double measurement(10.0)
std
Definition: Half.h:150
numericalDerivative.h
Some functions to compute numerical derivatives.
simple_graph::factor3
JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3))
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
main
int main()
Definition: testPoseBetweenFactor.cpp:242
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
TestPoseBetweenFactor
PoseBetweenFactor< Pose3 > TestPoseBetweenFactor
Definition: testPoseBetweenFactor.cpp:29
gtsam::assert_inequal
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:62
simple_graph::factor2
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))
error
static double error
Definition: testRot3.cpp:39
pose2
static const Pose3 pose2
Definition: testTriangulation.cpp:48
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
gtsam::PoseBetweenFactor::evaluateError
Vector evaluateError(const POSE &p1, const POSE &p2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Definition: PoseBetweenFactor.h:91
Pose3.h
3D Pose
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
poseKey1
static Key poseKey1(x1)
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
TEST
TEST(PoseBetweenFactor, Constructor)
Definition: testPoseBetweenFactor.cpp:39
simple_graph::factor1
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:48:39