testPosePriorFactor.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/PosePriorFactor.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 PosePriorFactor<Pose3> TestPosePriorFactor;
30 
32 namespace gtsam {
33 template<>
34 struct traits<TestPosePriorFactor> : public Testable<TestPosePriorFactor> {
35 };
36 }
37 
38 /* ************************************************************************* */
39 TEST( PosePriorFactor, Constructor) {
40  Key poseKey(1);
41  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
42  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
43  TestPosePriorFactor factor(poseKey, measurement, model);
44 }
45 
46 /* ************************************************************************* */
47 TEST( PosePriorFactor, ConstructorWithTransform) {
48  Key poseKey(1);
49  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
50  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
51  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
52  TestPosePriorFactor factor(poseKey, measurement, model, body_P_sensor);
53 }
54 
55 /* ************************************************************************* */
56 TEST( PosePriorFactor, Equals ) {
57  // Create two identical factors and make sure they're equal
58  Key poseKey(1);
59  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
60  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
61  TestPosePriorFactor factor1(poseKey, measurement, model);
62  TestPosePriorFactor factor2(poseKey, measurement, model);
63 
64  CHECK(assert_equal(factor1, factor2));
65 
66  // Create a third, different factor and check for inequality
67  Pose3 measurement2(Rot3::RzRyRx(0.20, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
68  TestPosePriorFactor factor3(poseKey, measurement2, model);
69 
70  CHECK(assert_inequal(factor1, factor3));
71 }
72 
73 /* ************************************************************************* */
74 TEST( PosePriorFactor, EqualsWithTransform ) {
75  // Create two identical factors and make sure they're equal
76  Key poseKey(1);
77  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
78  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
79  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
80  TestPosePriorFactor factor1(poseKey, measurement, model, body_P_sensor);
81  TestPosePriorFactor factor2(poseKey, measurement, model, body_P_sensor);
82 
83  CHECK(assert_equal(factor1, factor2));
84 
85  // Create a third, different factor and check for inequality
86  Pose3 body_P_sensor2(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.30, -0.10, 1.0));
87  TestPosePriorFactor factor3(poseKey, measurement, model, body_P_sensor2);
88 
89  CHECK(assert_inequal(factor1, factor3));
90 }
91 
92 /* ************************************************************************* */
93 TEST( PosePriorFactor, Error ) {
94  // Create the measurement and linearization point
95  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
96  Pose3 pose(Rot3::RzRyRx(0.0, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
97 
98  // The expected error
99  Vector expectedError(6);
100  // The solution depends on choice of Pose3 and Rot3 Expmap mode!
101 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
102  expectedError << -0.182948257976108,
103  0.13851858011118,
104  -0.157375974517456,
105 #if defined(GTSAM_POSE3_EXPMAP)
106  0.766913166076379,
107  -1.22976117053126,
108  0.949345561430261;
109 #else
110  0.740211734,
111  -1.19821028,
112  1.00815609;
113 #endif
114 #else
115  expectedError << -0.184137861505414,
116  0.139419283914526,
117  -0.158399296722242,
118  0.740211733817804,
119  -1.198210282560946,
120  1.008156093015192;
121 #endif
122 
123 
124  // Create a factor and calculate the error
125  Key poseKey(1);
126  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
127  TestPosePriorFactor factor(poseKey, measurement, model);
128  Vector actualError(factor.evaluateError(pose));
129 
130  // Verify we get the expected error
131  CHECK(assert_equal(expectedError, actualError, 1e-8));
132 }
133 
134 /* ************************************************************************* */
135 TEST( PosePriorFactor, ErrorWithTransform ) {
136  // Create the measurement and linearization point
137  Pose3 measurement(Rot3::RzRyRx(-M_PI_2+0.15, -0.30, -M_PI_2+0.45), Point3(-4.75, 7.90, -10.0));
138  Pose3 pose(Rot3::RzRyRx(0.0, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
139  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
140 
141  // The expected error
142  Vector expectedError(6);
143  // The solution depends on choice of Pose3 and Rot3 Expmap mode!
144 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
145  expectedError << -0.0224998729281528,
146  0.191947887288328,
147  0.273826035236257,
148 #if defined(GTSAM_POSE3_EXPMAP)
149  1.36483391560855,
150  -0.754590051075035,
151  0.585710674473659;
152 #else
153  1.49751986,
154  -0.549375791,
155  0.452761203;
156 #endif
157 #else
158  expectedError << -0.022712885347328,
159  0.193765110165872,
160  0.276418420819283,
161  1.497519863757366,
162  -0.549375791422721,
163  0.452761203187666;
164 #endif
165 
166  // Create a factor and calculate the error
167  Key poseKey(1);
168  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
169  TestPosePriorFactor factor(poseKey, measurement, model, body_P_sensor);
170  Vector actualError(factor.evaluateError(pose));
171 
172  // Verify we get the expected error
173  CHECK(assert_equal(expectedError, actualError, 1e-8));
174 }
175 
176 /* ************************************************************************* */
177 TEST( PosePriorFactor, Jacobian ) {
178  // Create a factor
179  Key poseKey(1);
180  Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
181  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
182  TestPosePriorFactor factor(poseKey, measurement, model);
183 
184  // Create a linearization point at the zero-error point
185  Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
186 
187  // Calculate numerical derivatives
188  Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
189 
190  // Use the factor to calculate the derivative
191  Matrix actualH1;
192  factor.evaluateError(pose, actualH1);
193 
194  // Verify we get the expected error
195  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
196 }
197 
198 /* ************************************************************************* */
199 TEST( PosePriorFactor, JacobianWithTransform ) {
200  // Create a factor
201  Key poseKey(1);
202  Pose3 measurement(Rot3::RzRyRx(-M_PI_2+0.15, -0.30, -M_PI_2+0.45), Point3(-4.75, 7.90, -10.0));
203  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 0.25);
204  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
205  TestPosePriorFactor factor(poseKey, measurement, model, body_P_sensor);
206 
207  // Create a linearization point at the zero-error point
208  Pose3 pose(Rot3::RzRyRx(-0.303202977317447, -0.143253159173011, 0.494633847678171),
209  Point3(-4.74767676, 7.67044942, -11.00985));
210 
211  // Calculate numerical derivatives
212  Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
213 
214  // Use the factor to calculate the derivative
215  Matrix actualH1;
216  factor.evaluateError(pose, actualH1);
217 
218  // Verify we get the expected error
219  CHECK(assert_equal(expectedH1, actualH1, 1e-5));
220 }
221 
222 /* ************************************************************************* */
223 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
224 /* ************************************************************************* */
225 
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
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.
TEST
TEST(PosePriorFactor, Constructor)
Definition: testPosePriorFactor.cpp:39
simple_graph::factor3
JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3))
TestPosePriorFactor
PosePriorFactor< Pose3 > TestPosePriorFactor
Definition: testPosePriorFactor.cpp:29
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::PosePriorFactor::evaluateError
Vector evaluateError(const POSE &p, boost::optional< Matrix & > H=boost::none) const override
Definition: PosePriorFactor.h:86
pose
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
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::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))
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
main
int main()
Definition: testPosePriorFactor.cpp:223
Pose3.h
3D Pose
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
simple_graph::factor1
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))

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