testProjectionFactor.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 
20 #include <gtsam/inference/Symbol.h>
21 #include <gtsam/geometry/Cal3DS2.h>
22 #include <gtsam/geometry/Cal3_S2.h>
23 #include <gtsam/geometry/Pose3.h>
24 #include <gtsam/geometry/Point3.h>
25 #include <gtsam/geometry/Point2.h>
28 
29 using namespace std;
30 using namespace gtsam;
31 
32 // make a realistic calibration matrix
33 static double fov = 60; // degrees
34 static size_t w=640,h=480;
35 static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
36 
37 // Create a noise model for the pixel error
38 static SharedNoiseModel model(noiseModel::Unit::Create(2));
39 
40 // Convenience for named keys
43 
45 
46 /* ************************************************************************* */
47 TEST( ProjectionFactor, nonStandard ) {
49 }
50 
51 /* ************************************************************************* */
52 TEST( ProjectionFactor, Constructor) {
53  Key poseKey(X(1));
54  Key pointKey(L(1));
55 
56  Point2 measurement(323.0, 240.0);
57 
59 }
60 
61 /* ************************************************************************* */
62 TEST( ProjectionFactor, ConstructorWithTransform) {
63  Key poseKey(X(1));
64  Key pointKey(L(1));
65 
66  Point2 measurement(323.0, 240.0);
67  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
68 
70 }
71 
72 /* ************************************************************************* */
73 TEST( ProjectionFactor, Equals ) {
74  // Create two identical factors and make sure they're equal
75  Point2 measurement(323.0, 240.0);
76 
79 
81 }
82 
83 /* ************************************************************************* */
84 TEST( ProjectionFactor, EqualsWithTransform ) {
85  // Create two identical factors and make sure they're equal
86  Point2 measurement(323.0, 240.0);
87  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
88 
91 
93 }
94 
95 /* ************************************************************************* */
96 TEST( ProjectionFactor, Error ) {
97  // Create the factor with a measurement that is 3 pixels off in x
98  Key poseKey(X(1));
99  Key pointKey(L(1));
100  Point2 measurement(323.0, 240.0);
102 
103  // Set the linearization point
104  Pose3 pose(Rot3(), Point3(0,0,-6));
105  Point3 point(0.0, 0.0, 0.0);
106 
107  // Use the factor to calculate the error
108  Vector actualError(factor.evaluateError(pose, point));
109 
110  // The expected error is (-3.0, 0.0) pixels / UnitCovariance
111  Vector expectedError = Vector2(-3.0, 0.0);
112 
113  // Verify we get the expected error
114  CHECK(assert_equal(expectedError, actualError, 1e-9));
115 }
116 
117 /* ************************************************************************* */
118 TEST( ProjectionFactor, ErrorWithTransform ) {
119  // Create the factor with a measurement that is 3 pixels off in x
120  Key poseKey(X(1));
121  Key pointKey(L(1));
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));
125 
126  // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
127  Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
128  Point3 point(0.0, 0.0, 0.0);
129 
130  // Use the factor to calculate the error
131  Vector actualError(factor.evaluateError(pose, point));
132 
133  // The expected error is (-3.0, 0.0) pixels / UnitCovariance
134  Vector expectedError = Vector2(-3.0, 0.0);
135 
136  // Verify we get the expected error
137  CHECK(assert_equal(expectedError, actualError, 1e-9));
138 }
139 
140 /* ************************************************************************* */
141 TEST( ProjectionFactor, Jacobian ) {
142  // Create the factor with a measurement that is 3 pixels off in x
143  Key poseKey(X(1));
144  Key pointKey(L(1));
145  Point2 measurement(323.0, 240.0);
147 
148  // Set the linearization point
149  Pose3 pose(Rot3(), Point3(0,0,-6));
150  Point3 point(0.0, 0.0, 0.0);
151 
152  // Use the factor to calculate the Jacobians
153  Matrix H1Actual, H2Actual;
154  factor.evaluateError(pose, point, H1Actual, H2Actual);
155 
156  // The expected Jacobians
157  Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished();
158  Matrix H2Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished();
159 
160  // Verify the Jacobians are correct
161  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
162  CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
163 }
164 
165 /* ************************************************************************* */
166 TEST( ProjectionFactor, JacobianWithTransform ) {
167  // Create the factor with a measurement that is 3 pixels off in x
168  Key poseKey(X(1));
169  Key pointKey(L(1));
170  Point2 measurement(323.0, 240.0);
171  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
173 
174  // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
175  Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
176  Point3 point(0.0, 0.0, 0.0);
177 
178  // Use the factor to calculate the Jacobians
179  Matrix H1Actual, H2Actual;
180  factor.evaluateError(pose, point, H1Actual, H2Actual);
181 
182  // The expected Jacobians
183  Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished();
184  Matrix H2Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished();
185 
186  // Verify the Jacobians are correct
187  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
188  CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
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
fov
static double fov
Definition: testProjectionFactor.cpp:33
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
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
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
model
static SharedNoiseModel model(noiseModel::Unit::Create(2))
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
TestableAssertions.h
Provides additional testing facilities for common data structures.
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
TEST
TEST(ProjectionFactor, nonStandard)
Definition: testProjectionFactor.cpp:47
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
gtsam::GenericProjectionFactor
Definition: ProjectionFactor.h:40
L
MatrixXd L
Definition: LLT_example.cpp:6
Symbol.h
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
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
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
gtsam
traits
Definition: SFMdata.h:40
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
main
int main()
Definition: testProjectionFactor.cpp:192
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
TestProjectionFactor
GenericProjectionFactor< Pose3, Point3 > TestProjectionFactor
Definition: testProjectionFactor.cpp:44
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
h
static size_t h
Definition: testProjectionFactor.cpp:34
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:196
measurement
static Point2 measurement(323.0, 240.0)
Pose3.h
3D Pose
w
static size_t w
Definition: testProjectionFactor.cpp:34
K
static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h))


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:41:07