testPinholeCamera.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 
18 #include <gtsam/geometry/PinholeCamera.h>
19 #include <gtsam/geometry/Cal3_S2.h>
20 #include <gtsam/geometry/Cal3Bundler.h>
21 #include <gtsam/geometry/Pose2.h>
22 #include <gtsam/base/Testable.h>
23 #include <gtsam/base/numericalDerivative.h>
24 
25 #include <CppUnitLite/TestHarness.h>
26 
27 #include <cmath>
28 #include <iostream>
29 
30 using namespace std;
31 using namespace gtsam;
32 
33 typedef PinholeCamera<Cal3_S2> Camera;
34 
35 static const Cal3_S2 K(625, 625, 0, 0, 0);
36 
37 static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
38 static const Camera camera(pose, K);
39 
40 static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
41 static const Camera camera1(pose1, K);
42 
43 static const Point3 point1(-0.08,-0.08, 0.0);
44 static const Point3 point2(-0.08, 0.08, 0.0);
45 static const Point3 point3( 0.08, 0.08, 0.0);
46 static const Point3 point4( 0.08,-0.08, 0.0);
47 
48 static const Unit3 point1_inf(-0.16,-0.16, -1.0);
49 static const Unit3 point2_inf(-0.16, 0.16, -1.0);
50 static const Unit3 point3_inf( 0.16, 0.16, -1.0);
51 static const Unit3 point4_inf( 0.16,-0.16, -1.0);
52 
53 /* ************************************************************************* */
54 TEST( PinholeCamera, constructor)
55 {
56  EXPECT(assert_equal( K, camera.calibration()));
57  EXPECT(assert_equal( pose, camera.pose()));
58 }
59 
60 //******************************************************************************
61 TEST(PinholeCamera, Create) {
62 
63  Matrix actualH1, actualH2;
64  EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2)));
65 
66  // Check derivative
67  boost::function<Camera(Pose3,Cal3_S2)> f = //
68  boost::bind(Camera::Create,_1,_2,boost::none,boost::none);
69  Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
70  EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
71  Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
72  EXPECT(assert_equal(numericalH2, actualH2, 1e-8));
73 }
74 
75 //******************************************************************************
76 TEST(PinholeCamera, Pose) {
77 
78  Matrix actualH;
79  EXPECT(assert_equal(pose, camera.getPose(actualH)));
80 
81  // Check derivative
82  boost::function<Pose3(Camera)> f = //
83  boost::bind(&Camera::getPose,_1,boost::none);
84  Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
85  EXPECT(assert_equal(numericalH, actualH, 1e-9));
86 }
87 
88 /* ************************************************************************* */
89 TEST( PinholeCamera, level2)
90 {
91  // Create a level camera, looking in Y-direction
92  Pose2 pose2(0.4,0.3,M_PI/2.0);
93  Camera camera = Camera::Level(K, pose2, 0.1);
94 
95  // expected
96  Point3 x(1,0,0),y(0,0,-1),z(0,1,0);
97  Rot3 wRc(x,y,z);
98  Pose3 expected(wRc,Point3(0.4,0.3,0.1));
99  EXPECT(assert_equal( camera.pose(), expected));
100 }
101 
102 /* ************************************************************************* */
103 TEST( PinholeCamera, lookat)
104 {
105  // Create a level camera, looking in Y-direction
106  Point3 C(10,0,0);
107  Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1));
108 
109  // expected
110  Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
111  Pose3 expected(Rot3(xc,yc,zc),C);
112  EXPECT(assert_equal(camera.pose(), expected));
113 
114  Point3 C2(30,0,10);
115  Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1));
116 
117  Matrix R = camera2.pose().rotation().matrix();
118  Matrix I = trans(R)*R;
119  EXPECT(assert_equal(I, I_3x3));
120 }
121 
122 /* ************************************************************************* */
123 TEST( PinholeCamera, project)
124 {
125  EXPECT(assert_equal( camera.project(point1), Point2(-100, 100) ));
126  EXPECT(assert_equal( camera.project(point2), Point2(-100, -100) ));
127  EXPECT(assert_equal( camera.project(point3), Point2( 100, -100) ));
128  EXPECT(assert_equal( camera.project(point4), Point2( 100, 100) ));
129 }
130 
131 /* ************************************************************************* */
132 TEST( PinholeCamera, backproject)
133 {
134  EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1));
135  EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2));
136  EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3));
137  EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4));
138 }
139 
140 /* ************************************************************************* */
141 TEST( PinholeCamera, backprojectInfinity)
142 {
143  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf));
144  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf));
145  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf));
146  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf));
147 }
148 
149 /* ************************************************************************* */
150 TEST( PinholeCamera, backproject2)
151 {
152  Point3 origin(0,0,0);
153  Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
154  Camera camera(Pose3(rot, origin), K);
155 
156  Point3 actual = camera.backproject(Point2(0,0), 1.);
157  Point3 expected(0., 1., 0.);
158  pair<Point2, bool> x = camera.projectSafe(expected);
159 
160  EXPECT(assert_equal(expected, actual));
161  EXPECT(assert_equal(Point2(0,0), x.first));
162  EXPECT(x.second);
163 }
164 
165 /* ************************************************************************* */
166 TEST( PinholeCamera, backprojectInfinity2)
167 {
168  Point3 origin(0,0,0);
169  Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
170  Camera camera(Pose3(rot, origin), K);
171 
172  Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0));
173  Unit3 expected(0., 1., 0.);
174  Point2 x = camera.project(expected);
175 
176  EXPECT(assert_equal(expected, actual));
177  EXPECT(assert_equal(Point2(0,0), x));
178 }
179 
180 /* ************************************************************************* */
181 TEST( PinholeCamera, backprojectInfinity3)
182 {
183  Point3 origin(0,0,0);
184  Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity
185  Camera camera(Pose3(rot, origin), K);
186 
187  Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0));
188  Unit3 expected(0., 0., 1.);
189  Point2 x = camera.project(expected);
190 
191  EXPECT(assert_equal(expected, actual));
192  EXPECT(assert_equal(Point2(0,0), x));
193 }
194 
195 /* ************************************************************************* */
196 static Point2 project3(const Pose3& pose, const Point3& point, const Cal3_S2& cal) {
197  return Camera(pose,cal).project(point);
198 }
199 
200 /* ************************************************************************* */
201 TEST( PinholeCamera, Dproject)
202 {
203  Matrix Dpose, Dpoint, Dcal;
204  Point2 result = camera.project(point1, Dpose, Dpoint, Dcal);
205  Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K);
206  Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K);
207  Matrix numerical_cal = numericalDerivative33(project3, pose, point1, K);
208  EXPECT(assert_equal(Point2(-100, 100), result));
209  EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
210  EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
211  EXPECT(assert_equal(numerical_cal, Dcal, 1e-7));
212 }
213 
214 /* ************************************************************************* */
215 static Point2 projectInfinity3(const Pose3& pose, const Unit3& point3D, const Cal3_S2& cal) {
216  return Camera(pose,cal).project(point3D);
217 }
218 
219 TEST( PinholeCamera, Dproject_Infinity)
220 {
221  Matrix Dpose, Dpoint, Dcal;
222  Unit3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1
223 
224  // test Projection
225  Point2 actual = camera.project(point3D, Dpose, Dpoint, Dcal);
226  Point2 expected(-5.0, 5.0);
227  EXPECT(assert_equal(actual, expected, 1e-7));
228 
229  // test Jacobians
230  Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose, point3D, K);
231  Matrix Hexpected2 = numericalDerivative32(projectInfinity3, pose, point3D, K);
232  Matrix numerical_point2x2 = Hexpected2.block(0,0,2,2); // only the direction to the point matters
233  Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose, point3D, K);
234  EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
235  EXPECT(assert_equal(numerical_point2x2, Dpoint, 1e-7));
236  EXPECT(assert_equal(numerical_cal, Dcal, 1e-7));
237 }
238 
239 /* ************************************************************************* */
240 static Point2 project4(const Camera& camera, const Point3& point) {
241  return camera.project2(point);
242 }
243 
244 /* ************************************************************************* */
245 TEST( PinholeCamera, Dproject2)
246 {
247  Matrix Dcamera, Dpoint;
248  Point2 result = camera.project2(point1, Dcamera, Dpoint);
249  Matrix Hexpected1 = numericalDerivative21(project4, camera, point1);
250  Matrix Hexpected2 = numericalDerivative22(project4, camera, point1);
251  EXPECT(assert_equal(result, Point2(-100, 100) ));
252  EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7));
253  EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
254 }
255 
256 /* ************************************************************************* */
257 // Add a test with more arbitrary rotation
258 TEST( PinholeCamera, Dproject3)
259 {
260  static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
261  static const Camera camera(pose1);
262  Matrix Dpose, Dpoint;
263  camera.project2(point1, Dpose, Dpoint);
264  Matrix numerical_pose = numericalDerivative21(project4, camera, point1);
265  Matrix numerical_point = numericalDerivative22(project4, camera, point1);
266  CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
267  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
268 }
269 
270 /* ************************************************************************* */
271 static double range0(const Camera& camera, const Point3& point) {
272  return camera.range(point);
273 }
274 
275 /* ************************************************************************* */
276 TEST( PinholeCamera, range0) {
277  Matrix D1; Matrix D2;
278  double result = camera.range(point1, D1, D2);
279  Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
280  Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
281  EXPECT_DOUBLES_EQUAL(distance3(point1, camera.pose().translation()), result,
282  1e-9);
283  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
284  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
285 }
286 
287 /* ************************************************************************* */
288 static double range1(const Camera& camera, const Pose3& pose) {
289  return camera.range(pose);
290 }
291 
292 /* ************************************************************************* */
293 TEST( PinholeCamera, range1) {
294  Matrix D1; Matrix D2;
295  double result = camera.range(pose1, D1, D2);
296  Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1);
297  Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1);
298  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
299  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
300  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
301 }
302 
303 /* ************************************************************************* */
304 typedef PinholeCamera<Cal3Bundler> Camera2;
305 static const Cal3Bundler K2(625, 1e-3, 1e-3);
306 static const Camera2 camera2(pose1, K2);
307 static double range2(const Camera& camera, const Camera2& camera2) {
308  return camera.range<Cal3Bundler>(camera2);
309 }
310 
311 /* ************************************************************************* */
312 TEST( PinholeCamera, range2) {
313  Matrix D1; Matrix D2;
314  double result = camera.range<Cal3Bundler>(camera2, D1, D2);
315  Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2);
316  Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2);
317  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
318  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
319  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
320 }
321 
322 /* ************************************************************************* */
323 static const CalibratedCamera camera3(pose1);
324 static double range3(const Camera& camera, const CalibratedCamera& camera3) {
325  return camera.range(camera3);
326 }
327 
328 /* ************************************************************************* */
329 TEST( PinholeCamera, range3) {
330  Matrix D1; Matrix D2;
331  double result = camera.range(camera3, D1, D2);
332  Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3);
333  Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3);
334  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
335  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
336  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
337 }
338 
339 /* ************************************************************************* */
340 TEST( PinholeCamera, Cal3Bundler) {
341  Cal3Bundler calibration;
342  Pose3 wTc;
343  PinholeCamera<Cal3Bundler> camera(wTc, calibration);
344  Point2 p(50, 100);
345  camera.backproject(p, 10);
346 }
347 
348 /* ************************************************************************* */
349 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
350 /* ************************************************************************* */
351 
352 
project4
static Point2 project4(const Camera &camera, const Point3 &point)
Definition: testPinholeCamera.cpp:240
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:57
CHECK
#define CHECK(condition)
Definition: Test.h:109
Camera
PinholeCamera< Cal3_S2 > Camera
Definition: testPinholeCamera.cpp:33
camera
static const Camera camera(pose, K)
project
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Definition: testPinholePose.cpp:188
y
Scalar * y
Definition: level1_cplx_impl.h:102
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::PinholeBaseK::backproject
Point3 backproject(const Point2 &p, double depth, OptionalJacobian< 3, 6 > Dresult_dpose=boost::none, OptionalJacobian< 3, 2 > Dresult_dp=boost::none, OptionalJacobian< 3, 1 > Dresult_ddepth=boost::none, OptionalJacobian< 3, DimK > Dresult_dcal=boost::none) const
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition: PinholePose.h:132
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
point4_inf
static const Unit3 point4_inf(0.16,-0.16,-1.0)
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
gtsam::Point2
Vector2 Point2
Definition: Point2.h:27
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
M_PI
#define M_PI
Definition: main.h:78
R
Rot2 R(Rot2::fromAngle(0.1))
range2
static double range2(const Camera &camera, const Camera2 &camera2)
Definition: testPinholeCamera.cpp:307
std
Definition: Half.h:150
gtsam::numericalDerivative33
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative33(boost::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:285
trans
static char trans
Definition: blas_interface.hh:58
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::numericalDerivative31
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative31(boost::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:227
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const boost::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:173
point3_inf
static const Unit3 point3_inf(0.16, 0.16,-1.0)
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
point2
static const Point3 point2(-0.08, 0.08, 0.0)
backproject
static Point3 backproject(const Pose3 &pose, const Point2 &point, const double &depth)
Definition: testCalibratedCamera.cpp:206
gtsam::distance3
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition: Point3.cpp:26
camera3
static const CalibratedCamera camera3(pose1)
example::point
Point3 point(10, 0,-5)
K2
static const Cal3Bundler K2(625, 1e-3, 1e-3)
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::numericalDerivative32
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative32(boost::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:256
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(boost::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:199
result
Values result
Definition: OdometryOptimize.cpp:8
pose
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
camera2
static const Camera2 camera2(pose1, K2)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:211
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Cal3_S2
Definition: Cal3_S2.h:34
gtsam::lago::I
static const Matrix I
Definition: lago.cpp:35
range0
static double range0(const Camera &camera, const Point3 &point)
Definition: testPinholeCamera.cpp:271
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:37
point3
static const Point3 point3(0.08, 0.08, 0.0)
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
main
int main()
Definition: testPinholeCamera.cpp:349
point1
static const Point3 point1(-0.08,-0.08, 0.0)
K
static const Cal3_S2 K(625, 625, 0, 0, 0)
TEST
TEST(PinholeCamera, constructor)
Definition: testPinholeCamera.cpp:54
range1
static double range1(const Camera &camera, const Pose3 &pose)
Definition: testPinholeCamera.cpp:288
p
float * p
Definition: Tutorial_Map_using.cpp:9
range3
static double range3(const Camera &camera, const CalibratedCamera &camera3)
Definition: testPinholeCamera.cpp:324
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
pose2
static const Pose3 pose2
Definition: testTriangulation.cpp:48
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
point2_inf
static const Unit3 point2_inf(-0.16, 0.16,-1.0)
Pose2.h
2D Pose
Camera
Definition: camera.h:36
Camera2
PinholeCamera< Cal3Bundler > Camera2
Definition: testPinholeCamera.cpp:304
projectInfinity3
static Point2 projectInfinity3(const Pose3 &pose, const Unit3 &point3D, const Cal3_S2 &cal)
Definition: testPinholeCamera.cpp:215
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
project3
static Point2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2 &cal)
Definition: testPinholeCamera.cpp:196
camera1
static const Camera camera1(pose1, K)
point1_inf
static const Unit3 point1_inf(-0.16,-0.16,-1.0)
Cal3Bundler.h
Calibration used by Bundler.
point4
static const Point3 point4(0.08,-0.08, 0.0)
Cal3_S2.h
The most common 5DOF 3D->2D calibration.

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:48:34