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