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

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:59