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

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