testProjectionFactorPPP.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 
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 
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 K(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
48 
50 
52 namespace gtsam {
53 template<>
54 struct traits<TestProjectionFactor> : public Testable<TestProjectionFactor> {
55 };
56 }
57 
58 /* ************************************************************************* */
59 TEST( ProjectionFactorPPP, nonStandard ) {
61 }
62 
63 /* ************************************************************************* */
64 TEST( ProjectionFactorPPP, Constructor) {
65  Key poseKey(X(1));
66  Key transformKey(T(1));
67  Key pointKey(L(1));
68 
69  Point2 measurement(323.0, 240.0);
70 
71  TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
72 }
73 
74 /* ************************************************************************* */
75 TEST( ProjectionFactorPPP, ConstructorWithTransform) {
76  Key poseKey(X(1));
77  Key transformKey(T(1));
78  Key pointKey(L(1));
79 
80  Point2 measurement(323.0, 240.0);
81  TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
82 }
83 
84 /* ************************************************************************* */
86  // Create two identical factors and make sure they're equal
87  Point2 measurement(323.0, 240.0);
88 
89  TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K);
90  TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K);
91 
92  CHECK(assert_equal(factor1, factor2));
93 }
94 
95 /* ************************************************************************* */
96 TEST( ProjectionFactorPPP, EqualsWithTransform ) {
97  // Create two identical factors and make sure they're equal
98  Point2 measurement(323.0, 240.0);
99  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
100 
101  TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K);
102  TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K);
103 
104  CHECK(assert_equal(factor1, factor2));
105 }
106 
107 /* ************************************************************************* */
109  // Create the factor with a measurement that is 3 pixels off in x
110  Key poseKey(X(1));
111  Key transformKey(T(1));
112  Key pointKey(L(1));
113  Point2 measurement(323.0, 240.0);
114  TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
115 
116  // Set the linearization point
117  Pose3 pose(Rot3(), Point3(0,0,-6));
118  Point3 point(0.0, 0.0, 0.0);
119 
120  // Use the factor to calculate the error
121  Vector actualError(factor.evaluateError(pose, Pose3(), point));
122 
123  // The expected error is (-3.0, 0.0) pixels / UnitCovariance
124  Vector expectedError = Vector2(-3.0, 0.0);
125 
126  // Verify we get the expected error
127  CHECK(assert_equal(expectedError, actualError, 1e-9));
128 }
129 
130 /* ************************************************************************* */
131 TEST( ProjectionFactorPPP, ErrorWithTransform ) {
132  // Create the factor with a measurement that is 3 pixels off in x
133  Key poseKey(X(1));
134  Key transformKey(T(1));
135  Key pointKey(L(1));
136  Point2 measurement(323.0, 240.0);
137  Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
138  TestProjectionFactor factor(measurement, model, poseKey,transformKey, pointKey, K);
139 
140  // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
141  Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
142  Point3 point(0.0, 0.0, 0.0);
143 
144  // Use the factor to calculate the error
145  Vector actualError(factor.evaluateError(pose, transform, point));
146 
147  // The expected error is (-3.0, 0.0) pixels / UnitCovariance
148  Vector expectedError = Vector2(-3.0, 0.0);
149 
150  // Verify we get the expected error
151  CHECK(assert_equal(expectedError, actualError, 1e-9));
152 }
153 
154 /* ************************************************************************* */
155 TEST( ProjectionFactorPPP, Jacobian ) {
156  // Create the factor with a measurement that is 3 pixels off in x
157  Key poseKey(X(1));
158  Key transformKey(T(1));
159  Key pointKey(L(1));
160  Point2 measurement(323.0, 240.0);
161  TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
162 
163  // Set the linearization point
164  Pose3 pose(Rot3(), Point3(0,0,-6));
165  Point3 point(0.0, 0.0, 0.0);
166 
167  // Use the factor to calculate the Jacobians
168  Matrix H1Actual, H2Actual, H3Actual;
169  factor.evaluateError(pose, Pose3(), point, H1Actual, H2Actual, H3Actual);
170 
171  // The expected Jacobians
172  Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished();
173  Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished();
174 
175  // Verify the Jacobians are correct
176  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
177  CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
178 
179  // Verify H2 with numerical derivative
180  Matrix H2Expected = numericalDerivative32<Vector,Pose3, Pose3, Point3>(
181  boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
182  boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
183  boost::none, boost::none, boost::none)), pose, Pose3(), point);
184 
185  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
186 }
187 
188 /* ************************************************************************* */
189 TEST( ProjectionFactorPPP, JacobianWithTransform ) {
190  // Create the factor with a measurement that is 3 pixels off in x
191  Key poseKey(X(1));
192  Key transformKey(T(1));
193  Key pointKey(L(1));
194  Point2 measurement(323.0, 240.0);
195  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -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(pose, body_P_sensor, point, H1Actual, H2Actual, 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  boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
217  boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
218  boost::none, boost::none, boost::none)), pose, body_P_sensor, point);
219 
220  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
221 
222 
223 }
224 
225 /* ************************************************************************* */
226 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
227 /* ************************************************************************* */
228 
const gtsam::Key poseKey
Provides additional testing facilities for common data structures.
#define CHECK(condition)
Definition: Test.h:109
static SharedNoiseModel model(noiseModel::Unit::Create(2))
static int runAllTests(TestResult &result)
Vector2 Point2
Definition: Point2.h:27
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
static double fov
MatrixXd L
Definition: LLT_example.cpp:6
double measurement(10.0)
Definition: Half.h:150
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
Some functions to compute numerical derivatives.
const gtsam::Key pointKey
Point3 point(10, 0,-5)
static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h))
Eigen::VectorXd Vector
Definition: Vector.h:38
ProjectionFactorPPP< Pose3, Point3 > TestProjectionFactor
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
Eigen::Triplet< double > T
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
TEST(ProjectionFactorPPP, nonStandard)
Derived from ProjectionFactor, but estimates body-camera transform in addition to body pose and 3D la...
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
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.
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
Eigen::Vector2d Vector2
Definition: Vector.h:42
static size_t w
3D Point
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))
static size_t h
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Vector3 Point3
Definition: Point3.h:35
#define X
Definition: icosphere.cpp:20
2D Point
3D Pose
boost::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
The most common 5DOF 3D->2D calibration.
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))


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