testProjectionFactorPPPC.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/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 
31 using namespace std::placeholders;
32 using namespace std;
33 using namespace gtsam;
34 
35 // make a realistic calibration matrix
36 static double fov = 60; // degrees
37 static size_t w=640,h=480;
38 static Cal3_S2::shared_ptr K1(new Cal3_S2(fov,w,h));
39 
40 // Create a noise model for the pixel error
41 static SharedNoiseModel model(noiseModel::Unit::Create(2));
42 
43 // Convenience for named keys
44 using symbol_shorthand::X;
45 using symbol_shorthand::L;
46 using symbol_shorthand::T;
47 using symbol_shorthand::K;
48 
49 typedef ProjectionFactorPPPC<Pose3, Point3, Cal3_S2> TestProjectionFactor;
50 
51 /* ************************************************************************* */
52 TEST( ProjectionFactorPPPC, nonStandard ) {
53  ProjectionFactorPPPC<Pose3, Point3, Cal3DS2> f;
54 }
55 
56 /* ************************************************************************* */
57 TEST( ProjectionFactorPPPC, Constructor) {
58  Point2 measurement(323.0, 240.0);
59  TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
60  // TODO: Actually check something
61 }
62 
63 /* ************************************************************************* */
64 TEST( 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(1), T(1), L(1), K(1));
69  TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K(1));
70 
71  CHECK(assert_equal(factor1, factor2));
72 }
73 
74 /* ************************************************************************* */
75 TEST( 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(1), T(1), L(1), K(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(pose, Pose3(), point, *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 /* ************************************************************************* */
95 TEST( 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(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
99  TestProjectionFactor factor(measurement, model, X(1),T(1), L(1), K(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(pose, transform, point, *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 /* ************************************************************************* */
116 TEST( 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(1), T(1), L(1), K(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(pose, Pose3(), point, *K1, H1Actual, H2Actual, H3Actual, 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  [&factor, &point, &pose](const Pose3& pose_arg) { return factor.evaluateError(pose, pose_arg, point, *K1); },
140  Pose3());
141 
142  Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
143  [&factor, &point, &pose](const Cal3_S2& K_arg) { return factor.evaluateError(pose, Pose3(), point, K_arg); },
144  *K1);
145 
146  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
147  CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
148 }
149 
150 /* ************************************************************************* */
151 TEST( 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(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
155  TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(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(pose, body_P_sensor, point, *K1, H1Actual, H2Actual, H3Actual, 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  [&factor, &pose, &point](const Pose3& body_P_sensor) {
176  return factor.evaluateError(pose, body_P_sensor, point, *K1);
177  },
178  body_P_sensor);
179 
180  Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
181  [&factor, &pose, &body_P_sensor, &point](const Cal3_S2& K) {
182  return factor.evaluateError(pose, body_P_sensor, point, K);
183  },
184  *K1);
185 
186  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
187  CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
188 
189 }
190 
191 /* ************************************************************************* */
192 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
193 /* ************************************************************************* */
194 
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
main
int main()
Definition: testProjectionFactorPPPC.cpp:192
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:203
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
TestHarness.h
ProjectionFactorPPPC.h
Derived from ProjectionFactor, but estimates body-camera transform and calibration in addition to bod...
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
X
#define X
Definition: icosphere.cpp:20
Point3.h
3D Point
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))
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
TestableAssertions.h
Provides additional testing facilities for common data structures.
TEST
TEST(ProjectionFactorPPPC, nonStandard)
Definition: testProjectionFactorPPPC.cpp:52
Point2.h
2D Point
gtsam::GenericProjectionFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, OptionalMatrixType H1, OptionalMatrixType H2) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: ProjectionFactor.h:138
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
h
static size_t h
Definition: testProjectionFactorPPPC.cpp:37
gtsam::GenericProjectionFactor
Definition: ProjectionFactor.h:40
w
static size_t w
Definition: testProjectionFactorPPPC.cpp:37
L
MatrixXd L
Definition: LLT_example.cpp:6
Symbol.h
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
transform
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
model
static SharedNoiseModel model(noiseModel::Unit::Create(2))
K1
static Cal3_S2::shared_ptr K1(new Cal3_S2(fov, w, h))
TestResult
Definition: TestResult.h:26
M_PI_2
#define M_PI_2
Definition: mconf.h:118
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
TestProjectionFactor
ProjectionFactorPPPC< Pose3, Point3, Cal3_S2 > TestProjectionFactor
Definition: testProjectionFactorPPPC.cpp:49
gtsam
traits
Definition: SFMdata.h:40
fov
static double fov
Definition: testProjectionFactorPPPC.cpp:36
K
#define K
Definition: igam.h:8
CHECK
#define CHECK(condition)
Definition: Test.h:108
Cal3DS2.h
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
std
Definition: BFloat16.h:88
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:196
measurement
static Point2 measurement(323.0, 240.0)
Pose3.h
3D Pose
gtsam::ProjectionFactorPPPC
Definition: ProjectionFactorPPPC.h:35

gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:18