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 #include <boost/bind.hpp>
32 
33 using namespace std;
34 using namespace gtsam;
35 
36 // make a realistic calibration matrix
37 static double fov = 60; // degrees
38 static size_t w=640,h=480;
39 static Cal3_S2::shared_ptr K1(new Cal3_S2(fov,w,h));
40 
41 // Create a noise model for the pixel error
42 static SharedNoiseModel model(noiseModel::Unit::Create(2));
43 
44 // Convenience for named keys
45 using symbol_shorthand::X;
46 using symbol_shorthand::L;
47 using symbol_shorthand::T;
48 using symbol_shorthand::K;
49 
50 typedef ProjectionFactorPPPC<Pose3, Point3, Cal3_S2> TestProjectionFactor;
51 
52 /* ************************************************************************* */
53 TEST( ProjectionFactorPPPC, nonStandard ) {
54  ProjectionFactorPPPC<Pose3, Point3, Cal3DS2> f;
55 }
56 
57 /* ************************************************************************* */
58 TEST( ProjectionFactorPPPC, Constructor) {
59  Point2 measurement(323.0, 240.0);
60  TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
61  // TODO: Actually check something
62 }
63 
64 /* ************************************************************************* */
65 TEST( ProjectionFactorPPPC, Equals ) {
66  // Create two identical factors and make sure they're equal
67  Point2 measurement(323.0, 240.0);
68 
69  TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K(1));
70  TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K(1));
71 
72  CHECK(assert_equal(factor1, factor2));
73 }
74 
75 /* ************************************************************************* */
76 TEST( ProjectionFactorPPPC, Error ) {
77  // Create the factor with a measurement that is 3 pixels off in x
78  Point2 measurement(323.0, 240.0);
79  TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
80 
81  // Set the linearization point
82  Pose3 pose(Rot3(), Point3(0,0,-6));
83  Point3 point(0.0, 0.0, 0.0);
84 
85  // Use the factor to calculate the error
86  Vector actualError(factor.evaluateError(pose, Pose3(), point, *K1));
87 
88  // The expected error is (-3.0, 0.0) pixels / UnitCovariance
89  Vector expectedError = Vector2(-3.0, 0.0);
90 
91  // Verify we get the expected error
92  CHECK(assert_equal(expectedError, actualError, 1e-9));
93 }
94 
95 /* ************************************************************************* */
96 TEST( ProjectionFactorPPPC, ErrorWithTransform ) {
97  // Create the factor with a measurement that is 3 pixels off in x
98  Point2 measurement(323.0, 240.0);
99  Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
100  TestProjectionFactor factor(measurement, model, X(1),T(1), L(1), K(1));
101 
102  // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
103  Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
104  Point3 point(0.0, 0.0, 0.0);
105 
106  // Use the factor to calculate the error
107  Vector actualError(factor.evaluateError(pose, transform, point, *K1));
108 
109  // The expected error is (-3.0, 0.0) pixels / UnitCovariance
110  Vector expectedError = Vector2(-3.0, 0.0);
111 
112  // Verify we get the expected error
113  CHECK(assert_equal(expectedError, actualError, 1e-9));
114 }
115 
116 /* ************************************************************************* */
117 TEST( ProjectionFactorPPPC, Jacobian ) {
118  // Create the factor with a measurement that is 3 pixels off in x
119  Point2 measurement(323.0, 240.0);
120  TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
121 
122  // Set the linearization point
123  Pose3 pose(Rot3(), Point3(0,0,-6));
124  Point3 point(0.0, 0.0, 0.0);
125 
126  // Use the factor to calculate the Jacobians
127  Matrix H1Actual, H2Actual, H3Actual, H4Actual;
128  factor.evaluateError(pose, Pose3(), point, *K1, H1Actual, H2Actual, H3Actual, H4Actual);
129 
130  // The expected Jacobians
131  Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished();
132  Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished();
133 
134  // Verify the Jacobians are correct
135  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
136  CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
137 
138  // Verify H2 and H4 with numerical derivatives
139  Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
140  boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
141  *K1, boost::none, boost::none, boost::none, boost::none), Pose3());
142 
143  Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
144  boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point,
145  _1, boost::none, boost::none, boost::none, boost::none), *K1);
146 
147  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
148  CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
149 }
150 
151 /* ************************************************************************* */
152 TEST( ProjectionFactorPPPC, JacobianWithTransform ) {
153  // Create the factor with a measurement that is 3 pixels off in x
154  Point2 measurement(323.0, 240.0);
155  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
156  TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
157 
158  // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
159  Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
160  Point3 point(0.0, 0.0, 0.0);
161 
162  // Use the factor to calculate the Jacobians
163  Matrix H1Actual, H2Actual, H3Actual, H4Actual;
164  factor.evaluateError(pose, body_P_sensor, point, *K1, H1Actual, H2Actual, H3Actual, H4Actual);
165 
166  // The expected Jacobians
167  Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished();
168  Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished();
169 
170  // Verify the Jacobians are correct
171  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
172  CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
173 
174  // Verify H2 and H4 with numerical derivatives
175  Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
176  boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
177  *K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor);
178 
179  Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
180  boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point,
181  _1, boost::none, boost::none, boost::none, boost::none), *K1);
182 
183  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
184  CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
185 
186 }
187 
188 /* ************************************************************************* */
189 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
190 /* ************************************************************************* */
191 
TestableAssertions.h
Provides additional testing facilities for common data structures.
CHECK
#define CHECK(condition)
Definition: Test.h:109
model
static SharedNoiseModel model(noiseModel::Unit::Create(2))
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Point2
Vector2 Point2
Definition: Point2.h:27
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
K
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
L
MatrixXd L
Definition: LLT_example.cpp:6
measurement
double measurement(10.0)
ProjectionFactorPPPC.h
Derived from ProjectionFactor, but estimates body-camera transform and calibration in addition to bod...
std
Definition: Half.h:150
Cal3DS2.h
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
numericalDerivative.h
Some functions to compute numerical derivatives.
TestProjectionFactor
ProjectionFactorPPPC< Pose3, Point3, Cal3_S2 > TestProjectionFactor
Definition: testProjectionFactorPPPC.cpp:50
example::point
Point3 point(10, 0,-5)
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
pose
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:211
fov
static double fov
Definition: testProjectionFactorPPPC.cpp:37
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
K1
static Cal3_S2::shared_ptr K1(new Cal3_S2(fov, w, h))
gtsam::Cal3_S2
Definition: Cal3_S2.h:34
transform
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
h
static size_t h
Definition: testProjectionFactorPPPC.cpp:38
gtsam::GenericProjectionFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: ProjectionFactor.h:131
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
Point3.h
3D Point
w
static size_t w
Definition: testProjectionFactorPPPC.cpp:38
simple_graph::factor2
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
TEST
TEST(ProjectionFactorPPPC, nonStandard)
Definition: testProjectionFactorPPPC.cpp:53
X
#define X
Definition: icosphere.cpp:20
Point2.h
2D Point
Pose3.h
3D Pose
gtsam::Cal3_S2::shared_ptr
boost::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
simple_graph::factor1
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:49:13