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 testProjectionFactor.cpp
14 * @brief Unit tests for ProjectionFactorPPP Class
15 * @author Chris Beall
16 * @date July 2014
17 */
18
19#include <gtsam/base/numericalDerivative.h>
20#include <gtsam/base/TestableAssertions.h>
21#include <gtsam_unstable/slam/ProjectionFactorPPP.h>
22#include <gtsam/inference/Symbol.h>
23#include <gtsam/geometry/Cal3DS2.h>
24#include <gtsam/geometry/Cal3_S2.h>
25#include <gtsam/geometry/Pose3.h>
26#include <gtsam/geometry/Point3.h>
27#include <gtsam/geometry/Point2.h>
28
29#include <CppUnitLite/TestHarness.h>
30
31using namespace std::placeholders;
32using namespace std;
33using namespace gtsam;
34
35// make a realistic calibration matrix
36static double fov = 60; // degrees
37static size_t w=640,h=480;
38static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
39
40// Create a noise model for the pixel error
41static SharedNoiseModel model(noiseModel::Unit::Create(dim: 2));
42
43// Convenience for named keys
44using symbol_shorthand::X;
45using symbol_shorthand::L;
46using symbol_shorthand::T;
47
48typedef ProjectionFactorPPP<Pose3, Point3> TestProjectionFactor;
49
50/// traits
51namespace gtsam {
52template<>
53struct traits<TestProjectionFactor> : public Testable<TestProjectionFactor> {
54};
55}
56
57/* ************************************************************************* */
58TEST( ProjectionFactorPPP, nonStandard ) {
59 ProjectionFactorPPP<Pose3, Point3, Cal3DS2> f;
60}
61
62/* ************************************************************************* */
63TEST( ProjectionFactorPPP, Constructor) {
64 Key poseKey(X(j: 1));
65 Key transformKey(T(j: 1));
66 Key pointKey(L(j: 1));
67
68 Point2 measurement(323.0, 240.0);
69
70 TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
71}
72
73/* ************************************************************************* */
74TEST( ProjectionFactorPPP, ConstructorWithTransform) {
75 Key poseKey(X(j: 1));
76 Key transformKey(T(j: 1));
77 Key pointKey(L(j: 1));
78
79 Point2 measurement(323.0, 240.0);
80 TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
81}
82
83/* ************************************************************************* */
84TEST( ProjectionFactorPPP, Equals ) {
85 // Create two identical factors and make sure they're equal
86 Point2 measurement(323.0, 240.0);
87
88 TestProjectionFactor factor1(measurement, model, X(j: 1), T(j: 1), L(j: 1), K);
89 TestProjectionFactor factor2(measurement, model, X(j: 1), T(j: 1), L(j: 1), K);
90
91 CHECK(assert_equal(factor1, factor2));
92}
93
94/* ************************************************************************* */
95TEST( ProjectionFactorPPP, EqualsWithTransform ) {
96 // Create two identical factors and make sure they're equal
97 Point2 measurement(323.0, 240.0);
98 Pose3 body_P_sensor(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
99
100 TestProjectionFactor factor1(measurement, model, X(j: 1), T(j: 1), L(j: 1), K);
101 TestProjectionFactor factor2(measurement, model, X(j: 1), T(j: 1), L(j: 1), K);
102
103 CHECK(assert_equal(factor1, factor2));
104}
105
106/* ************************************************************************* */
107TEST( ProjectionFactorPPP, Error ) {
108 // Create the factor with a measurement that is 3 pixels off in x
109 Key poseKey(X(j: 1));
110 Key transformKey(T(j: 1));
111 Key pointKey(L(j: 1));
112 Point2 measurement(323.0, 240.0);
113 TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
114
115 // Set the linearization point
116 Pose3 pose(Rot3(), Point3(0,0,-6));
117 Point3 point(0.0, 0.0, 0.0);
118
119 // Use the factor to calculate the error
120 Vector actualError(factor.evaluateError(x: pose, x: Pose3(), x: point));
121
122 // The expected error is (-3.0, 0.0) pixels / UnitCovariance
123 Vector expectedError = Vector2(-3.0, 0.0);
124
125 // Verify we get the expected error
126 CHECK(assert_equal(expectedError, actualError, 1e-9));
127}
128
129/* ************************************************************************* */
130TEST( ProjectionFactorPPP, ErrorWithTransform ) {
131 // Create the factor with a measurement that is 3 pixels off in x
132 Key poseKey(X(j: 1));
133 Key transformKey(T(j: 1));
134 Key pointKey(L(j: 1));
135 Point2 measurement(323.0, 240.0);
136 Pose3 transform(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
137 TestProjectionFactor factor(measurement, model, poseKey,transformKey, pointKey, K);
138
139 // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
140 Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
141 Point3 point(0.0, 0.0, 0.0);
142
143 // Use the factor to calculate the error
144 Vector actualError(factor.evaluateError(x: pose, x: transform, x: point));
145
146 // The expected error is (-3.0, 0.0) pixels / UnitCovariance
147 Vector expectedError = Vector2(-3.0, 0.0);
148
149 // Verify we get the expected error
150 CHECK(assert_equal(expectedError, actualError, 1e-9));
151}
152
153/* ************************************************************************* */
154TEST( ProjectionFactorPPP, Jacobian ) {
155 // Create the factor with a measurement that is 3 pixels off in x
156 Key poseKey(X(j: 1));
157 Key transformKey(T(j: 1));
158 Key pointKey(L(j: 1));
159 Point2 measurement(323.0, 240.0);
160 TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
161
162 // Set the linearization point
163 Pose3 pose(Rot3(), Point3(0,0,-6));
164 Point3 point(0.0, 0.0, 0.0);
165
166 // Use the factor to calculate the Jacobians
167 Matrix H1Actual, H2Actual, H3Actual;
168 factor.evaluateError(x: pose, x: Pose3(), x: point, H&: H1Actual, H&: H2Actual, H&: H3Actual);
169
170 // The expected Jacobians
171 Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished();
172 Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished();
173
174 // Verify the Jacobians are correct
175 CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
176 CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
177
178 // Verify H2 with numerical derivative
179 Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
180 h: [&factor](const Pose3& pose, const Pose3& transform, const Point3& point) {
181 return factor.evaluateError(x: pose, x: transform, x: point);
182 },
183 x1: pose, x2: Pose3(), x3: point);
184
185 CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
186}
187
188/* ************************************************************************* */
189TEST( ProjectionFactorPPP, JacobianWithTransform ) {
190 // Create the factor with a measurement that is 3 pixels off in x
191 Key poseKey(X(j: 1));
192 Key transformKey(T(j: 1));
193 Key pointKey(L(j: 1));
194 Point2 measurement(323.0, 240.0);
195 Pose3 body_P_sensor(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
196 TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
197
198 // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
199 Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
200 Point3 point(0.0, 0.0, 0.0);
201
202 // Use the factor to calculate the Jacobians
203 Matrix H1Actual, H2Actual, H3Actual;
204 factor.evaluateError(x: pose, x: body_P_sensor, x: point, H&: H1Actual, H&: H2Actual, H&: H3Actual);
205
206 // The expected Jacobians
207 Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished();
208 Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished();
209
210 // Verify the Jacobians are correct
211 CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
212 CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
213
214 // Verify H2 with numerical derivative
215 Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
216 h: [&factor](const Pose3& pose, const Pose3& transform, const Point3& point) {
217 return factor.evaluateError(x: pose, x: transform, x: point);
218 },
219 x1: pose, x2: body_P_sensor, x3: point);
220
221 CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
222
223
224}
225
226/* ************************************************************************* */
227int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
228/* ************************************************************************* */
229
230