testSphericalCamera.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/Testable.h>
21 #include <gtsam/base/numericalDerivative.h>
22 #include <gtsam/geometry/SphericalCamera.h>
23 
24 #include <cmath>
25 #include <iostream>
26 
27 #if defined(__i686__) || defined(__i386__)
28 // See issue discussion: https://github.com/borglab/gtsam/issues/1605
29 constexpr double TEST_THRESHOLD = 1e-5;
30 #else
31 constexpr double TEST_THRESHOLD = 1e-7;
32 #endif
33 
34 using namespace std::placeholders;
35 using namespace std;
36 using namespace gtsam;
37 
38 typedef SphericalCamera Camera;
39 
40 // static const Cal3_S2 K(625, 625, 0, 0, 0);
41 //
42 static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()),
43  Point3(0, 0, 0.5));
44 static const Camera camera(pose);
45 //
46 static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
47 static const Camera camera1(pose1);
48 
49 static const Point3 point1(-0.08, -0.08, 0.0);
50 static const Point3 point2(-0.08, 0.08, 0.0);
51 static const Point3 point3(0.08, 0.08, 0.0);
52 static const Point3 point4(0.08, -0.08, 0.0);
53 
54 // manually computed in matlab
55 static const Unit3 bearing1(-0.156054862928174, 0.156054862928174,
56  0.975342893301088);
57 static const Unit3 bearing2(-0.156054862928174, -0.156054862928174,
58  0.975342893301088);
59 static const Unit3 bearing3(0.156054862928174, -0.156054862928174,
60  0.975342893301088);
61 static const Unit3 bearing4(0.156054862928174, 0.156054862928174,
62  0.975342893301088);
63 
64 static double depth = 0.512640224719052;
65 /* ************************************************************************* */
66 TEST(SphericalCamera, constructor) {
67  EXPECT(assert_equal(pose, camera.pose()));
68 }
69 
70 /* ************************************************************************* */
71 TEST(SphericalCamera, project) {
72  // expected from manual calculation in Matlab
73  EXPECT(assert_equal(camera.project(point1), bearing1));
74  EXPECT(assert_equal(camera.project(point2), bearing2));
75  EXPECT(assert_equal(camera.project(point3), bearing3));
76  EXPECT(assert_equal(camera.project(point4), bearing4));
77 }
78 
79 /* ************************************************************************* */
80 TEST(SphericalCamera, backproject) {
81  EXPECT(assert_equal(camera.backproject(bearing1, depth), point1));
82  EXPECT(assert_equal(camera.backproject(bearing2, depth), point2));
83  EXPECT(assert_equal(camera.backproject(bearing3, depth), point3));
84  EXPECT(assert_equal(camera.backproject(bearing4, depth), point4));
85 }
86 
87 /* ************************************************************************* */
88 TEST(SphericalCamera, backproject2) {
89  Point3 origin(0, 0, 0);
90  Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
91  Camera camera(Pose3(rot, origin));
92 
93  Point3 actual = camera.backproject(Unit3(0, 0, 1), 1.);
94  Point3 expected(0., 1., 0.);
95  pair<Unit3, bool> x = camera.projectSafe(expected);
96 
97  EXPECT(assert_equal(expected, actual));
98  EXPECT(assert_equal(Unit3(0, 0, 1), x.first));
99  EXPECT(x.second);
100 }
101 
102 /* ************************************************************************* */
103 static Unit3 project3(const Pose3& pose, const Point3& point) {
104  return Camera(pose).project(point);
105 }
106 
107 /* ************************************************************************* */
108 TEST(SphericalCamera, Dproject) {
109  Matrix Dpose, Dpoint;
110  Unit3 result = camera.project(point1, Dpose, Dpoint);
111  Matrix numerical_pose = numericalDerivative21(project3, pose, point1);
112  Matrix numerical_point = numericalDerivative22(project3, pose, point1);
113  EXPECT(assert_equal(bearing1, result));
114  EXPECT(assert_equal(numerical_pose, Dpose, TEST_THRESHOLD));
115  EXPECT(assert_equal(numerical_point, Dpoint, TEST_THRESHOLD));
116 }
117 
118 /* ************************************************************************* */
119 static Vector2 reprojectionError2(const Pose3& pose, const Point3& point,
120  const Unit3& measured) {
121  return Camera(pose).reprojectionError(point, measured);
122 }
123 
124 /* ************************************************************************* */
125 TEST(SphericalCamera, reprojectionError) {
126  Matrix Dpose, Dpoint;
127  Vector2 result = camera.reprojectionError(point1, bearing1, Dpose, Dpoint);
128  Matrix numerical_pose =
129  numericalDerivative31(reprojectionError2, pose, point1, bearing1);
130  Matrix numerical_point =
131  numericalDerivative32(reprojectionError2, pose, point1, bearing1);
132  EXPECT(assert_equal(Vector2(0.0, 0.0), result));
133  EXPECT(assert_equal(numerical_pose, Dpose, TEST_THRESHOLD));
134  EXPECT(assert_equal(numerical_point, Dpoint, TEST_THRESHOLD));
135 }
136 
137 /* ************************************************************************* */
138 TEST(SphericalCamera, reprojectionError_noisy) {
139  Matrix Dpose, Dpoint;
140  Unit3 bearing_noisy = bearing1.retract(Vector2(0.01, 0.05));
141  Vector2 result =
142  camera.reprojectionError(point1, bearing_noisy, Dpose, Dpoint);
143  Matrix numerical_pose =
144  numericalDerivative31(reprojectionError2, pose, point1, bearing_noisy);
145  Matrix numerical_point =
146  numericalDerivative32(reprojectionError2, pose, point1, bearing_noisy);
147  EXPECT(assert_equal(Vector2(-0.050282, 0.00833482), result, 1e2*TEST_THRESHOLD));
148  EXPECT(assert_equal(numerical_pose, Dpose, TEST_THRESHOLD));
149  EXPECT(assert_equal(numerical_point, Dpoint, TEST_THRESHOLD));
150 }
151 
152 /* ************************************************************************* */
153 // Add a test with more arbitrary rotation
154 TEST(SphericalCamera, Dproject2) {
155  static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
156  static const Camera camera(pose1);
157  Matrix Dpose, Dpoint;
158  camera.project2(point1, Dpose, Dpoint);
159  Matrix numerical_pose = numericalDerivative21(project3, pose1, point1);
160  Matrix numerical_point = numericalDerivative22(project3, pose1, point1);
161  CHECK(assert_equal(numerical_pose, Dpose, TEST_THRESHOLD));
162  CHECK(assert_equal(numerical_point, Dpoint, TEST_THRESHOLD));
163 }
164 
165 /* ************************************************************************* */
166 int main() {
167  TestResult tr;
168  return TestRegistry::runAllTests(tr);
169 }
170 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
Camera
SphericalCamera Camera
Definition: testSphericalCamera.cpp:38
camera1
static const Camera camera1(pose1)
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
SphericalCamera.h
Calibrated camera with spherical projection.
project3
static Unit3 project3(const Pose3 &pose, const Point3 &point)
Definition: testSphericalCamera.cpp:103
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
TestHarness.h
Camera
Definition: camera.h:36
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::SphericalCamera
Definition: SphericalCamera.h:74
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:195
measured
Point2 measured(-17, 30)
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
point1
static const Point3 point1(-0.08, -0.08, 0.0)
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
backproject
static Point3 backproject(const Pose3 &pose, const Point2 &point, const double &depth)
Definition: testCalibratedCamera.cpp:207
result
Values result
Definition: OdometryOptimize.cpp:8
rot
int EIGEN_BLAS_FUNC() rot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps)
Definition: level1_real_impl.h:79
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::SphericalCamera::project
Unit3 project(const Point3 &point, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: SphericalCamera.cpp:83
point3
static const Point3 point3(0.08, 0.08, 0.0)
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::numericalDerivative32
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative32(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:259
gtsam::Pose3
Definition: Pose3.h:37
gtsam::numericalDerivative31
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative31(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:226
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
point2
static const Point3 point2(-0.08, 0.08, 0.0)
Vector2
Definition: test_operator_overloading.cpp:18
bearing3
static const Unit3 bearing3(0.156054862928174, -0.156054862928174, 0.975342893301088)
gtsam::SphericalCamera::reprojectionError
Vector2 reprojectionError(const Point3 &point, const Unit3 &measured, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: SphericalCamera.cpp:90
gtsam::Unit3::retract
Unit3 retract(const Vector2 &v, OptionalJacobian< 2, 2 > H={}) const
The retract function.
Definition: Unit3.cpp:255
TestResult
Definition: TestResult.h:26
main
int main()
Definition: testSphericalCamera.cpp:166
bearing2
static const Unit3 bearing2(-0.156054862928174, -0.156054862928174, 0.975342893301088)
gtsam
traits
Definition: chartTesting.h:28
project
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Definition: testPinholePose.cpp:188
constructor
Definition: init.h:200
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:166
reprojectionError2
static Vector2 reprojectionError2(const Pose3 &pose, const Point3 &point, const Unit3 &measured)
Definition: testSphericalCamera.cpp:119
std
Definition: BFloat16.h:88
origin
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set origin
Definition: gnuplot_common_settings.hh:45
TEST_THRESHOLD
constexpr double TEST_THRESHOLD
Definition: testSphericalCamera.cpp:31
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
TEST
TEST(SphericalCamera, constructor)
Definition: testSphericalCamera.cpp:66
bearing1
static const Unit3 bearing1(-0.156054862928174, 0.156054862928174, 0.975342893301088)
camera
static const Camera camera(pose)
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
depth
static double depth
Definition: testSphericalCamera.cpp:64
point4
static const Point3 point4(0.08, -0.08, 0.0)
bearing4
static const Unit3 bearing4(0.156054862928174, 0.156054862928174, 0.975342893301088)

gtsam
Author(s):
autogenerated on Tue Jun 25 2024 03:07:00