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 using namespace std::placeholders;
28 using namespace std;
29 using namespace gtsam;
30 
31 typedef SphericalCamera Camera;
32 
33 // static const Cal3_S2 K(625, 625, 0, 0, 0);
34 //
35 static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()),
36  Point3(0, 0, 0.5));
37 static const Camera camera(pose);
38 //
39 static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
40 static const Camera camera1(pose1);
41 
42 static const Point3 point1(-0.08, -0.08, 0.0);
43 static const Point3 point2(-0.08, 0.08, 0.0);
44 static const Point3 point3(0.08, 0.08, 0.0);
45 static const Point3 point4(0.08, -0.08, 0.0);
46 
47 // manually computed in matlab
48 static const Unit3 bearing1(-0.156054862928174, 0.156054862928174,
49  0.975342893301088);
50 static const Unit3 bearing2(-0.156054862928174, -0.156054862928174,
51  0.975342893301088);
52 static const Unit3 bearing3(0.156054862928174, -0.156054862928174,
53  0.975342893301088);
54 static const Unit3 bearing4(0.156054862928174, 0.156054862928174,
55  0.975342893301088);
56 
57 static double depth = 0.512640224719052;
58 /* ************************************************************************* */
59 TEST(SphericalCamera, constructor) {
60  EXPECT(assert_equal(pose, camera.pose()));
61 }
62 
63 /* ************************************************************************* */
64 TEST(SphericalCamera, project) {
65  // expected from manual calculation in Matlab
66  EXPECT(assert_equal(camera.project(point1), bearing1));
67  EXPECT(assert_equal(camera.project(point2), bearing2));
68  EXPECT(assert_equal(camera.project(point3), bearing3));
69  EXPECT(assert_equal(camera.project(point4), bearing4));
70 }
71 
72 /* ************************************************************************* */
73 TEST(SphericalCamera, backproject) {
74  EXPECT(assert_equal(camera.backproject(bearing1, depth), point1));
75  EXPECT(assert_equal(camera.backproject(bearing2, depth), point2));
76  EXPECT(assert_equal(camera.backproject(bearing3, depth), point3));
77  EXPECT(assert_equal(camera.backproject(bearing4, depth), point4));
78 }
79 
80 /* ************************************************************************* */
81 TEST(SphericalCamera, backproject2) {
82  Point3 origin(0, 0, 0);
83  Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
84  Camera camera(Pose3(rot, origin));
85 
86  Point3 actual = camera.backproject(Unit3(0, 0, 1), 1.);
87  Point3 expected(0., 1., 0.);
88  pair<Unit3, bool> x = camera.projectSafe(expected);
89 
90  EXPECT(assert_equal(expected, actual));
91  EXPECT(assert_equal(Unit3(0, 0, 1), x.first));
92  EXPECT(x.second);
93 }
94 
95 /* ************************************************************************* */
96 static Unit3 project3(const Pose3& pose, const Point3& point) {
97  return Camera(pose).project(point);
98 }
99 
100 /* ************************************************************************* */
101 TEST(SphericalCamera, Dproject) {
102  Matrix Dpose, Dpoint;
103  Unit3 result = camera.project(point1, Dpose, Dpoint);
104  Matrix numerical_pose = numericalDerivative21(project3, pose, point1);
105  Matrix numerical_point = numericalDerivative22(project3, pose, point1);
106  EXPECT(assert_equal(bearing1, result));
107  EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
108  EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
109 }
110 
111 /* ************************************************************************* */
112 static Vector2 reprojectionError2(const Pose3& pose, const Point3& point,
113  const Unit3& measured) {
114  return Camera(pose).reprojectionError(point, measured);
115 }
116 
117 /* ************************************************************************* */
118 TEST(SphericalCamera, reprojectionError) {
119  Matrix Dpose, Dpoint;
120  Vector2 result = camera.reprojectionError(point1, bearing1, Dpose, Dpoint);
121  Matrix numerical_pose =
122  numericalDerivative31(reprojectionError2, pose, point1, bearing1);
123  Matrix numerical_point =
124  numericalDerivative32(reprojectionError2, pose, point1, bearing1);
125  EXPECT(assert_equal(Vector2(0.0, 0.0), result));
126  EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
127  EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
128 }
129 
130 /* ************************************************************************* */
131 TEST(SphericalCamera, reprojectionError_noisy) {
132  Matrix Dpose, Dpoint;
133  Unit3 bearing_noisy = bearing1.retract(Vector2(0.01, 0.05));
134  Vector2 result =
135  camera.reprojectionError(point1, bearing_noisy, Dpose, Dpoint);
136  Matrix numerical_pose =
137  numericalDerivative31(reprojectionError2, pose, point1, bearing_noisy);
138  Matrix numerical_point =
139  numericalDerivative32(reprojectionError2, pose, point1, bearing_noisy);
140  EXPECT(assert_equal(Vector2(-0.050282, 0.00833482), result, 1e-5));
141  EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
142  EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
143 }
144 
145 /* ************************************************************************* */
146 // Add a test with more arbitrary rotation
147 TEST(SphericalCamera, Dproject2) {
148  static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
149  static const Camera camera(pose1);
150  Matrix Dpose, Dpoint;
151  camera.project2(point1, Dpose, Dpoint);
152  Matrix numerical_pose = numericalDerivative21(project3, pose1, point1);
153  Matrix numerical_point = numericalDerivative22(project3, pose1, point1);
154  CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
155  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
156 }
157 
158 /* ************************************************************************* */
159 int main() {
160  TestResult tr;
161  return TestRegistry::runAllTests(tr);
162 }
163 /* ************************************************************************* */
measured
Point2 measured(-17, 30)
bearing4
static const Unit3 bearing4(0.156054862928174, 0.156054862928174, 0.975342893301088)
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
CHECK
#define CHECK(condition)
Definition: Test.h:108
project
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Definition: testPinholePose.cpp:188
gtsam::SphericalCamera::reprojectionError
Vector2 reprojectionError(const Point3 &point, const Unit3 &measured, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: SphericalCamera.cpp:90
Testable.h
Concept check for values that can be used in unit tests.
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
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:12
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
point1
static const Point3 point1(-0.08, -0.08, 0.0)
depth
static double depth
Definition: testSphericalCamera.cpp:57
reprojectionError2
static Vector2 reprojectionError2(const Pose3 &pose, const Point3 &point, const Unit3 &measured)
Definition: testSphericalCamera.cpp:112
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
point4
static const Point3 point4(0.08, -0.08, 0.0)
Camera
SphericalCamera Camera
Definition: testSphericalCamera.cpp:31
SphericalCamera.h
Calibrated camera with spherical projection.
std
Definition: BFloat16.h:88
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
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
backproject
static Point3 backproject(const Pose3 &pose, const Point2 &point, const double &depth)
Definition: testCalibratedCamera.cpp:207
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
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
TEST
TEST(SphericalCamera, constructor)
Definition: testSphericalCamera.cpp:59
bearing3
static const Unit3 bearing3(0.156054862928174, -0.156054862928174, 0.975342893301088)
result
Values result
Definition: OdometryOptimize.cpp:8
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
point3
static const Point3 point3(0.08, 0.08, 0.0)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
bearing2
static const Unit3 bearing2(-0.156054862928174, -0.156054862928174, 0.975342893301088)
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
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
camera
static const Camera camera(pose)
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
gtsam
traits
Definition: chartTesting.h:28
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
gtsam::Unit3::retract
Unit3 retract(const Vector2 &v, OptionalJacobian< 2, 2 > H={}) const
The retract function.
Definition: Unit3.cpp:255
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
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
project3
static Unit3 project3(const Pose3 &pose, const Point3 &point)
Definition: testSphericalCamera.cpp:96
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
point2
static const Point3 point2(-0.08, 0.08, 0.0)
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
bearing1
static const Unit3 bearing1(-0.156054862928174, 0.156054862928174, 0.975342893301088)
camera1
static const Camera camera1(pose1)
main
int main()
Definition: testSphericalCamera.cpp:159

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:41