testProjectionFactorRollingShutter.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 <CppUnitLite/TestHarness.h>
20 #include <gtsam/base/TestableAssertions.h>
21 #include <gtsam/base/numericalDerivative.h>
22 #include <gtsam/geometry/Cal3DS2.h>
23 #include <gtsam/geometry/Cal3_S2.h>
24 #include <gtsam/geometry/Point2.h>
25 #include <gtsam/geometry/Point3.h>
26 #include <gtsam/geometry/Pose3.h>
27 #include <gtsam/inference/Symbol.h>
28 #include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
29 
30 using namespace std::placeholders;
31 using namespace std;
32 using namespace gtsam;
33 
34 // make a realistic calibration matrix
35 static double fov = 60; // degrees
36 static size_t w = 640, h = 480;
37 static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h));
38 
39 // Create a noise model for the pixel error
40 static SharedNoiseModel model(noiseModel::Unit::Create(2));
41 
42 // Convenience for named keys
43 using symbol_shorthand::L;
44 using symbol_shorthand::T;
45 using symbol_shorthand::X;
46 
47 // Convenience to define common variables across many tests
48 static Key poseKey1(X(1));
49 static Key poseKey2(X(2));
50 static Key pointKey(L(1));
51 static double interp_params = 0.5;
52 static Point2 measurement(323.0, 240.0);
53 static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
54  Point3(0.25, -0.10, 1.0));
55 
56 /* ************************************************************************* */
57 TEST(ProjectionFactorRollingShutter, Constructor) {
58  ProjectionFactorRollingShutter factor(measurement, interp_params, model,
59  poseKey1, poseKey2, pointKey, K);
60 }
61 
62 /* ************************************************************************* */
63 TEST(ProjectionFactorRollingShutter, ConstructorWithTransform) {
64  ProjectionFactorRollingShutter factor(measurement, interp_params, model,
65  poseKey1, poseKey2, pointKey, K,
66  body_P_sensor);
67 }
68 
69 /* ************************************************************************* */
70 TEST(ProjectionFactorRollingShutter, Equals) {
71  { // factors are equal
72  ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
73  poseKey1, poseKey2, pointKey, K);
74  ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
75  poseKey1, poseKey2, pointKey, K);
76  CHECK(assert_equal(factor1, factor2));
77  }
78  { // factors are NOT equal (keys are different)
79  ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
80  poseKey1, poseKey2, pointKey, K);
81  ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
82  poseKey1, poseKey1, pointKey, K);
83  CHECK(!assert_equal(factor1, factor2)); // not equal
84  }
85  { // factors are NOT equal (different interpolation)
86  ProjectionFactorRollingShutter factor1(measurement, 0.1, model, poseKey1,
87  poseKey1, pointKey, K);
88  ProjectionFactorRollingShutter factor2(measurement, 0.5, model, poseKey1,
89  poseKey2, pointKey, K);
90  CHECK(!assert_equal(factor1, factor2)); // not equal
91  }
92 }
93 
94 /* ************************************************************************* */
95 TEST(ProjectionFactorRollingShutter, EqualsWithTransform) {
96  { // factors are equal
97  ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
98  poseKey1, poseKey2, pointKey, K,
99  body_P_sensor);
100  ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
101  poseKey1, poseKey2, pointKey, K,
102  body_P_sensor);
103  CHECK(assert_equal(factor1, factor2));
104  }
105  { // factors are NOT equal
106  ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
107  poseKey1, poseKey2, pointKey, K,
108  body_P_sensor);
109  Pose3 body_P_sensor2(
110  Rot3::RzRyRx(0.0, 0.0, 0.0),
111  Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor
112  ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
113  poseKey1, poseKey2, pointKey, K,
114  body_P_sensor2);
115  CHECK(!assert_equal(factor1, factor2));
116  }
117 }
118 
119 /* ************************************************************************* */
120 TEST(ProjectionFactorRollingShutter, Error) {
121  {
122  // Create the factor with a measurement that is 3 pixels off in x
123  // Camera pose corresponds to the first camera
124  double t = 0.0;
125  ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1,
126  poseKey2, pointKey, K);
127 
128  // Set the linearization point
129  Pose3 pose1(Rot3(), Point3(0, 0, -6));
130  Pose3 pose2(Rot3(), Point3(0, 0, -4));
131  Point3 point(0.0, 0.0, 0.0);
132 
133  // Use the factor to calculate the error
134  Vector actualError(factor.evaluateError(pose1, pose2, point));
135 
136  // The expected error is (-3.0, 0.0) pixels / UnitCovariance
137  Vector expectedError = Vector2(-3.0, 0.0);
138 
139  // Verify we get the expected error
140  CHECK(assert_equal(expectedError, actualError, 1e-9));
141  }
142  {
143  // Create the factor with a measurement that is 3 pixels off in x
144  // Camera pose is actually interpolated now
145  double t = 0.5;
146  ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1,
147  poseKey2, pointKey, K);
148 
149  // Set the linearization point
150  Pose3 pose1(Rot3(), Point3(0, 0, -8));
151  Pose3 pose2(Rot3(), Point3(0, 0, -4));
152  Point3 point(0.0, 0.0, 0.0);
153 
154  // Use the factor to calculate the error
155  Vector actualError(factor.evaluateError(pose1, pose2, point));
156 
157  // The expected error is (-3.0, 0.0) pixels / UnitCovariance
158  Vector expectedError = Vector2(-3.0, 0.0);
159 
160  // Verify we get the expected error
161  CHECK(assert_equal(expectedError, actualError, 1e-9));
162  }
163  {
164  // Create measurement by projecting 3D landmark
165  double t = 0.3;
166  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
167  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
168  Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
169  PinholeCamera<Cal3_S2> camera(poseInterp, *K);
170  Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
171  Point2 measured = camera.project(point);
172 
173  // create factor
174  ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
175  poseKey2, pointKey, K);
176 
177  // Use the factor to calculate the error
178  Vector actualError(factor.evaluateError(pose1, pose2, point));
179 
180  // The expected error is zero
181  Vector expectedError = Vector2(0.0, 0.0);
182 
183  // Verify we get the expected error
184  CHECK(assert_equal(expectedError, actualError, 1e-9));
185  }
186 }
187 
188 /* ************************************************************************* */
189 TEST(ProjectionFactorRollingShutter, ErrorWithTransform) {
190  // Create measurement by projecting 3D landmark
191  double t = 0.3;
192  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
193  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
194  Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
195  Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1));
196  PinholeCamera<Cal3_S2> camera(poseInterp * body_P_sensor3, *K);
197  Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
198  Point2 measured = camera.project(point);
199 
200  // create factor
201  ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
202  pointKey, K, body_P_sensor3);
203 
204  // Use the factor to calculate the error
205  Vector actualError(factor.evaluateError(pose1, pose2, point));
206 
207  // The expected error is zero
208  Vector expectedError = Vector2(0.0, 0.0);
209 
210  // Verify we get the expected error
211  CHECK(assert_equal(expectedError, actualError, 1e-9));
212 }
213 
214 /* ************************************************************************* */
215 TEST(ProjectionFactorRollingShutter, Jacobian) {
216  // Create measurement by projecting 3D landmark
217  double t = 0.3;
218  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
219  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
220  Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
221  PinholeCamera<Cal3_S2> camera(poseInterp, *K);
222  Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
223  Point2 measured = camera.project(point);
224 
225  // create factor
226  ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
227  pointKey, K);
228 
229  // Use the factor to calculate the Jacobians
230  Matrix H1Actual, H2Actual, H3Actual;
231  factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual);
232 
233  auto f = [&factor](const Pose3& p1, const Pose3& p2, const Point3& p3) {
234  return factor.evaluateError(p1, p2, p3);
235  };
236  // Expected Jacobians via numerical derivatives
237  Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>(f, pose1, pose2, point);
238 
239  Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(f, pose1, pose2, point);
240 
241  Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(f, pose1, pose2, point);
242 
243  CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
244  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
245  CHECK(assert_equal(H3Expected, H3Actual, 1e-5));
246 }
247 
248 /* ************************************************************************* */
249 TEST(ProjectionFactorRollingShutter, JacobianWithTransform) {
250  // Create measurement by projecting 3D landmark
251  double t = 0.6;
252  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
253  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
254  Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
255  Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1));
256  PinholeCamera<Cal3_S2> camera(poseInterp * body_P_sensor3, *K);
257  Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
258  Point2 measured = camera.project(point);
259 
260  // create factor
261  ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
262  pointKey, K, body_P_sensor3);
263 
264  // Use the factor to calculate the Jacobians
265  Matrix H1Actual, H2Actual, H3Actual;
266  factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual);
267 
268  auto f = [&factor](const Pose3& p1, const Pose3& p2, const Point3& p3) {
269  return factor.evaluateError(p1, p2, p3);
270  };
271  // Expected Jacobians via numerical derivatives
272  Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>(f, pose1, pose2, point);
273 
274  Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(f, pose1, pose2, point);
275 
276  Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(f, pose1, pose2, point);
277 
278  CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
279  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
280  CHECK(assert_equal(H3Expected, H3Actual, 1e-5));
281 }
282 
283 /* ************************************************************************* */
284 TEST(ProjectionFactorRollingShutter, cheirality) {
285  // Create measurement by projecting 3D landmark behind camera
286  double t = 0.3;
287  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
288  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
289  Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
290  PinholeCamera<Cal3_S2> camera(poseInterp, *K);
291  Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera
292 
293 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
294  Point2 measured = Point2(0.0, 0.0); // project would throw an exception
295  { // check that exception is thrown if we set throwCheirality = true
296  bool throwCheirality = true;
297  bool verboseCheirality = true;
298  ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
299  poseKey2, pointKey, K,
300  throwCheirality, verboseCheirality);
301  CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point),
302  CheiralityException);
303  }
304  { // check that exception is NOT thrown if we set throwCheirality = false,
305  // and outputs are correct
306  bool throwCheirality = false; // default
307  bool verboseCheirality = false; // default
308  ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
309  poseKey2, pointKey, K,
310  throwCheirality, verboseCheirality);
311 
312  // Use the factor to calculate the error
313  Matrix H1Actual, H2Actual, H3Actual;
314  Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual,
315  H2Actual, H3Actual));
316 
317  // The expected error is zero
318  Vector expectedError = Vector2::Constant(
319  2.0 * K->fx()); // this is what we return when point is behind camera
320 
321  // Verify we get the expected error
322  CHECK(assert_equal(expectedError, actualError, 1e-9));
323  CHECK(assert_equal(Matrix::Zero(2, 6), H1Actual, 1e-5));
324  CHECK(assert_equal(Matrix::Zero(2, 6), H2Actual, 1e-5));
325  CHECK(assert_equal(Matrix::Zero(2, 3), H3Actual, 1e-5));
326  }
327 #else
328  {
329  // everything is well defined, hence this matches the test "Jacobian" above:
330  Point2 measured = camera.project(point);
331 
332  // create factor
333  ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
334  poseKey2, pointKey, K);
335 
336  // Use the factor to calculate the Jacobians
337  Matrix H1Actual, H2Actual, H3Actual;
338  factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual);
339 
340  // Expected Jacobians via numerical derivatives
341  Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>(
342  std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
343  std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
344  std::placeholders::_1, std::placeholders::_2,
345  std::placeholders::_3, {}, {},
346  {})),
347  pose1, pose2, point);
348 
349  Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
350  std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
351  std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
352  std::placeholders::_1, std::placeholders::_2,
353  std::placeholders::_3, {}, {},
354  {})),
355  pose1, pose2, point);
356 
357  Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
358  std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
359  std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
360  std::placeholders::_1, std::placeholders::_2,
361  std::placeholders::_3, {}, {},
362  {})),
363  pose1, pose2, point);
364 
365  CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
366  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
367  CHECK(assert_equal(H3Expected, H3Actual, 1e-5));
368  }
369 #endif
370 }
371 
372 /* ************************************************************************* */
373 int main() {
374  TestResult tr;
375  return TestRegistry::runAllTests(tr);
376 }
377 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
CHECK_EXCEPTION
#define CHECK_EXCEPTION(condition, exception_name)
Definition: Test.h:118
h
static size_t h
Definition: testProjectionFactorRollingShutter.cpp:36
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
interp_params
static double interp_params
Definition: testProjectionFactorRollingShutter.cpp:51
measured
Point2 measured(-17, 30)
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
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.
Point2.h
2D Point
gtsam::CheiralityException
Definition: CalibratedCamera.h:34
fov
static double fov
Definition: testProjectionFactorRollingShutter.cpp:35
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
model
static SharedNoiseModel model(noiseModel::Unit::Create(2))
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
pointKey
static Key pointKey(L(1))
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
K
static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h))
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::ProjectionFactorRollingShutter
Definition: ProjectionFactorRollingShutter.h:43
L
MatrixXd L
Definition: LLT_example.cpp:6
Symbol.h
simple::p3
static Point3 p3
Definition: testInitializePose3.cpp:53
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
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
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
TEST
TEST(ProjectionFactorRollingShutter, Constructor)
Definition: testProjectionFactorRollingShutter.cpp:57
main
int main()
Definition: testProjectionFactorRollingShutter.cpp:373
gtsam
traits
Definition: SFMdata.h:40
poseKey2
static Key poseKey2(X(2))
ProjectionFactorRollingShutter.h
Basic projection factor for rolling shutter cameras.
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
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
poseKey1
static Key poseKey1(X(1))
w
static size_t w
Definition: testProjectionFactorRollingShutter.cpp:36
camera
static const CalibratedCamera camera(kDefaultPose)
gtsam::ProjectionFactorRollingShutter::evaluateError
Vector evaluateError(const Pose3 &pose_a, const Pose3 &pose_b, const Point3 &point, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: ProjectionFactorRollingShutter.cpp:22
align_3::t
Point2 t(10, 10)
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:196
measurement
static Point2 measurement(323.0, 240.0)
Pose3.h
3D Pose
gtsam::CalibratedCamera::project
Point2 project(const Point3 &point, OptionalJacobian< 2, 6 > Dcamera={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: CalibratedCamera.cpp:188

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