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 testProjectionFactorPPPC.cpp
14 * @brief Unit tests for Pose+Transform+Calibration ProjectionFactor Class
15 * @author Chris Beall
16 * @date Jul 29, 2014
17 */
18
19#include <gtsam/base/numericalDerivative.h>
20#include <gtsam/base/TestableAssertions.h>
21#include <gtsam_unstable/slam/ProjectionFactorPPPC.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 K1(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;
47using symbol_shorthand::K;
48
49typedef ProjectionFactorPPPC<Pose3, Point3, Cal3_S2> TestProjectionFactor;
50
51/* ************************************************************************* */
52TEST( ProjectionFactorPPPC, nonStandard ) {
53 ProjectionFactorPPPC<Pose3, Point3, Cal3DS2> f;
54}
55
56/* ************************************************************************* */
57TEST( ProjectionFactorPPPC, Constructor) {
58 Point2 measurement(323.0, 240.0);
59 TestProjectionFactor factor(measurement, model, X(j: 1), T(j: 1), L(j: 1), K(j: 1));
60 // TODO: Actually check something
61}
62
63/* ************************************************************************* */
64TEST( ProjectionFactorPPPC, Equals ) {
65 // Create two identical factors and make sure they're equal
66 Point2 measurement(323.0, 240.0);
67
68 TestProjectionFactor factor1(measurement, model, X(j: 1), T(j: 1), L(j: 1), K(j: 1));
69 TestProjectionFactor factor2(measurement, model, X(j: 1), T(j: 1), L(j: 1), K(j: 1));
70
71 CHECK(assert_equal(factor1, factor2));
72}
73
74/* ************************************************************************* */
75TEST( ProjectionFactorPPPC, Error ) {
76 // Create the factor with a measurement that is 3 pixels off in x
77 Point2 measurement(323.0, 240.0);
78 TestProjectionFactor factor(measurement, model, X(j: 1), T(j: 1), L(j: 1), K(j: 1));
79
80 // Set the linearization point
81 Pose3 pose(Rot3(), Point3(0,0,-6));
82 Point3 point(0.0, 0.0, 0.0);
83
84 // Use the factor to calculate the error
85 Vector actualError(factor.evaluateError(x: pose, x: Pose3(), x: point, x: *K1));
86
87 // The expected error is (-3.0, 0.0) pixels / UnitCovariance
88 Vector expectedError = Vector2(-3.0, 0.0);
89
90 // Verify we get the expected error
91 CHECK(assert_equal(expectedError, actualError, 1e-9));
92}
93
94/* ************************************************************************* */
95TEST( ProjectionFactorPPPC, ErrorWithTransform ) {
96 // Create the factor with a measurement that is 3 pixels off in x
97 Point2 measurement(323.0, 240.0);
98 Pose3 transform(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
99 TestProjectionFactor factor(measurement, model, X(j: 1),T(j: 1), L(j: 1), K(j: 1));
100
101 // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
102 Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
103 Point3 point(0.0, 0.0, 0.0);
104
105 // Use the factor to calculate the error
106 Vector actualError(factor.evaluateError(x: pose, x: transform, x: point, x: *K1));
107
108 // The expected error is (-3.0, 0.0) pixels / UnitCovariance
109 Vector expectedError = Vector2(-3.0, 0.0);
110
111 // Verify we get the expected error
112 CHECK(assert_equal(expectedError, actualError, 1e-9));
113}
114
115/* ************************************************************************* */
116TEST( ProjectionFactorPPPC, Jacobian ) {
117 // Create the factor with a measurement that is 3 pixels off in x
118 Point2 measurement(323.0, 240.0);
119 TestProjectionFactor factor(measurement, model, X(j: 1), T(j: 1), L(j: 1), K(j: 1));
120
121 // Set the linearization point
122 Pose3 pose(Rot3(), Point3(0,0,-6));
123 Point3 point(0.0, 0.0, 0.0);
124
125 // Use the factor to calculate the Jacobians
126 Matrix H1Actual, H2Actual, H3Actual, H4Actual;
127 factor.evaluateError(x: pose, x: Pose3(), x: point, x: *K1, H&: H1Actual, H&: H2Actual, H&: H3Actual, H&: H4Actual);
128
129 // The expected Jacobians
130 Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished();
131 Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished();
132
133 // Verify the Jacobians are correct
134 CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
135 CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
136
137 // Verify H2 and H4 with numerical derivatives
138 Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
139 h: [&factor, &point, &pose](const Pose3& pose_arg) { return factor.evaluateError(x: pose, x: pose_arg, x: point, x: *K1); },
140 x: Pose3());
141
142 Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
143 h: [&factor, &point, &pose](const Cal3_S2& K_arg) { return factor.evaluateError(x: pose, x: Pose3(), x: point, x: K_arg); },
144 x: *K1);
145
146 CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
147 CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
148}
149
150/* ************************************************************************* */
151TEST( ProjectionFactorPPPC, JacobianWithTransform ) {
152 // Create the factor with a measurement that is 3 pixels off in x
153 Point2 measurement(323.0, 240.0);
154 Pose3 body_P_sensor(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
155 TestProjectionFactor factor(measurement, model, X(j: 1), T(j: 1), L(j: 1), K(j: 1));
156
157 // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
158 Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
159 Point3 point(0.0, 0.0, 0.0);
160
161 // Use the factor to calculate the Jacobians
162 Matrix H1Actual, H2Actual, H3Actual, H4Actual;
163 factor.evaluateError(x: pose, x: body_P_sensor, x: point, x: *K1, H&: H1Actual, H&: H2Actual, H&: H3Actual, H&: H4Actual);
164
165 // The expected Jacobians
166 Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished();
167 Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished();
168
169 // Verify the Jacobians are correct
170 CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
171 CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
172
173 // Verify H2 and H4 with numerical derivatives
174 Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
175 h: [&factor, &pose, &point](const Pose3& body_P_sensor) {
176 return factor.evaluateError(x: pose, x: body_P_sensor, x: point, x: *K1);
177 },
178 x: body_P_sensor);
179
180 Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
181 h: [&factor, &pose, &body_P_sensor, &point](const Cal3_S2& K) {
182 return factor.evaluateError(x: pose, x: body_P_sensor, x: point, x: K);
183 },
184 x: *K1);
185
186 CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
187 CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
188
189}
190
191/* ************************************************************************* */
192int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
193/* ************************************************************************* */
194
195