testPinholePose.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 <gtsam/geometry/PinholePose.h>
20 #include <gtsam/geometry/Cal3_S2.h>
21 #include <gtsam/geometry/Pose2.h>
22 #include <gtsam/geometry/Cal3Bundler.h>
23 #include <gtsam/base/Testable.h>
24 #include <gtsam/base/numericalDerivative.h>
25 
26 #include <CppUnitLite/TestHarness.h>
27 
28 #include <cmath>
29 #include <iostream>
30 
31 using namespace std;
32 using namespace gtsam;
33 
34 typedef PinholePose<Cal3_S2> Camera;
35 
36 static const Cal3_S2::shared_ptr K = boost::make_shared<Cal3_S2>(625, 625, 0, 0, 0);
37 
38 static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
39 static const Camera camera(pose, K);
40 
41 static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
42 static const Camera camera1(pose1, K);
43 
44 static const Point3 point1(-0.08,-0.08, 0.0);
45 static const Point3 point2(-0.08, 0.08, 0.0);
46 static const Point3 point3( 0.08, 0.08, 0.0);
47 static const Point3 point4( 0.08,-0.08, 0.0);
48 
49 static const Unit3 point1_inf(-0.16,-0.16, -1.0);
50 static const Unit3 point2_inf(-0.16, 0.16, -1.0);
51 static const Unit3 point3_inf( 0.16, 0.16, -1.0);
52 static const Unit3 point4_inf( 0.16,-0.16, -1.0);
53 
54 /* ************************************************************************* */
55 TEST( PinholePose, constructor)
56 {
57  EXPECT(assert_equal( pose, camera.pose()));
58 }
59 
60 //******************************************************************************
61 /* Already in testPinholeCamera???
62 TEST(PinholeCamera, Pose) {
63 
64  Matrix actualH;
65  EXPECT(assert_equal(pose, camera.getPose(actualH)));
66 
67  // Check derivative
68  boost::function<Pose3(Camera)> f = //
69  boost::bind(&Camera::getPose,_1,boost::none);
70  Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
71  EXPECT(assert_equal(numericalH, actualH, 1e-9));
72 }
73 */
74 
75 /* ************************************************************************* */
76 TEST( PinholePose, lookat)
77 {
78  // Create a level camera, looking in Y-direction
79  Point3 C(10,0,0);
80  Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1));
81 
82  // expected
83  Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
84  Pose3 expected(Rot3(xc,yc,zc),C);
85  EXPECT(assert_equal( camera.pose(), expected));
86 
87  Point3 C2(30,0,10);
88  Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1));
89 
90  Matrix R = camera2.pose().rotation().matrix();
91  Matrix I = trans(R)*R;
92  EXPECT(assert_equal(I, I_3x3));
93 }
94 
95 /* ************************************************************************* */
96 TEST( PinholePose, project)
97 {
98  EXPECT(assert_equal( camera.project2(point1), Point2(-100, 100) ));
99  EXPECT(assert_equal( camera.project2(point2), Point2(-100, -100) ));
100  EXPECT(assert_equal( camera.project2(point3), Point2( 100, -100) ));
101  EXPECT(assert_equal( camera.project2(point4), Point2( 100, 100) ));
102 }
103 
104 /* ************************************************************************* */
105 TEST( PinholePose, backproject)
106 {
107  EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1));
108  EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2));
109  EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3));
110  EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4));
111 }
112 
113 /* ************************************************************************* */
114 TEST( PinholePose, backprojectInfinity)
115 {
116  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf));
117  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf));
118  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf));
119  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf));
120 }
121 
122 /* ************************************************************************* */
123 TEST( PinholePose, backproject2)
124 {
125  Point3 origin(0,0,0);
126  Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
127  Camera camera(Pose3(rot, origin), K);
128 
129  Point3 actual = camera.backproject(Point2(0,0), 1.);
130  Point3 expected(0., 1., 0.);
131  pair<Point2, bool> x = camera.projectSafe(expected);
132 
133  EXPECT(assert_equal(expected, actual));
134  EXPECT(assert_equal(Point2(0,0), x.first));
135  EXPECT(x.second);
136 }
137 
138 /* ************************************************************************* */
139 static Point2 project3(const Pose3& pose, const Point3& point,
140  const Cal3_S2::shared_ptr& cal) {
141  return Camera(pose, cal).project2(point);
142 }
143 
144 /* ************************************************************************* */
145 TEST( PinholePose, Dproject)
146 {
147  Matrix Dpose, Dpoint;
148  Point2 result = camera.project2(point1, Dpose, Dpoint);
149  Matrix expectedDcamera = numericalDerivative31(project3, pose, point1, K);
150  Matrix expectedDpoint = numericalDerivative32(project3, pose, point1, K);
151  EXPECT(assert_equal(Point2(-100, 100), result));
152  EXPECT(assert_equal(expectedDcamera, Dpose, 1e-7));
153  EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
154 }
155 
156 /* ************************************************************************* */
157 static Point2 project4(const Camera& camera, const Point3& point) {
158  return camera.project2(point);
159 }
160 
161 /* ************************************************************************* */
162 TEST( PinholePose, Dproject2)
163 {
164  Matrix Dcamera, Dpoint;
165  Point2 result = camera.project2(point1, Dcamera, Dpoint);
166  Matrix expectedDcamera = numericalDerivative21(project4, camera, point1);
167  Matrix expectedDpoint = numericalDerivative22(project4, camera, point1);
168  EXPECT(assert_equal(result, Point2(-100, 100) ));
169  EXPECT(assert_equal(expectedDcamera, Dcamera, 1e-7));
170  EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
171 }
172 
173 /* ************************************************************************* */
174 // Add a test with more arbitrary rotation
175 TEST( CalibratedCamera, Dproject3)
176 {
177  static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
178  static const Camera camera(pose1);
179  Matrix Dpose, Dpoint;
180  camera.project2(point1, Dpose, Dpoint);
181  Matrix expectedDcamera = numericalDerivative21(project4, camera, point1);
182  Matrix numerical_point = numericalDerivative22(project4, camera, point1);
183  CHECK(assert_equal(expectedDcamera, Dpose, 1e-7));
184  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
185 }
186 
187 /* ************************************************************************* */
188 static Point2 project(const Pose3& pose, const Unit3& pointAtInfinity,
189  const Cal3_S2::shared_ptr& cal) {
190  return Camera(pose, cal).project(pointAtInfinity);
191 }
192 
193 /* ************************************************************************* */
194 TEST( PinholePose, DprojectAtInfinity2)
195 {
196  Unit3 pointAtInfinity(0,0,-1000);
197  Matrix Dpose, Dpoint;
198  Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint);
199  Matrix expectedDcamera = numericalDerivative31(project, pose, pointAtInfinity, K);
200  Matrix expectedDpoint = numericalDerivative32(project, pose, pointAtInfinity, K);
201  EXPECT(assert_equal(Point2(0,0), result));
202  EXPECT(assert_equal(expectedDcamera, Dpose, 1e-7));
203  EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
204 }
205 
206 
207 static Point3 backproject(const Pose3& pose, const Cal3_S2& cal,
208  const Point2& p, const double& depth) {
209  return Camera(pose, cal.vector()).backproject(p, depth);
210 }
211 
212 TEST( PinholePose, DbackprojectRegCamera)
213 {
214  Matrix36 Dpose;
215  Matrix31 Ddepth;
216  Matrix32 Dpoint;
217  Matrix Dcal;
218  const Point2 point(-100, 100);
219  const double depth(10);
220  camera.backproject(point, depth, Dpose, Dpoint, Ddepth, Dcal);
221  Matrix expectedDpose = numericalDerivative41(backproject, pose, *K, point, depth);
222  Matrix expectedDcal = numericalDerivative42(backproject, pose, *K, point, depth);
223  Matrix expectedDpoint = numericalDerivative43(backproject, pose, *K, point, depth);
224  Matrix expectedDdepth = numericalDerivative44(backproject, pose, *K, point, depth);
225 
226  EXPECT(assert_equal(expectedDpose, Dpose, 1e-7));
227  EXPECT(assert_equal(expectedDcal, Dcal, 1e-7));
228  EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
229  EXPECT(assert_equal(expectedDdepth, Ddepth, 1e-7));
230 }
231 
232 /* ************************************************************************* */
233 static double range0(const Camera& camera, const Point3& point) {
234  return camera.range(point);
235 }
236 
237 /* ************************************************************************* */
238 TEST( PinholePose, range0) {
239  Matrix D1; Matrix D2;
240  double result = camera.range(point1, D1, D2);
241  Matrix expectedDcamera = numericalDerivative21(range0, camera, point1);
242  Matrix expectedDpoint = numericalDerivative22(range0, camera, point1);
243  EXPECT_DOUBLES_EQUAL(distance3(point1, camera.pose().translation()), result, 1e-9);
244  EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
245  EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
246 }
247 
248 /* ************************************************************************* */
249 static double range1(const Camera& camera, const Pose3& pose) {
250  return camera.range(pose);
251 }
252 
253 /* ************************************************************************* */
254 TEST( PinholePose, range1) {
255  Matrix D1; Matrix D2;
256  double result = camera.range(pose1, D1, D2);
257  Matrix expectedDcamera = numericalDerivative21(range1, camera, pose1);
258  Matrix expectedDpoint = numericalDerivative22(range1, camera, pose1);
259  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
260  EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
261  EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
262 }
263 
264 /* ************************************************************************* */
265 typedef PinholePose<Cal3Bundler> Camera2;
266 static const boost::shared_ptr<Cal3Bundler> K2 =
267  boost::make_shared<Cal3Bundler>(625, 1e-3, 1e-3);
268 static const Camera2 camera2(pose1, K2);
269 static double range2(const Camera& camera, const Camera2& camera2) {
270  return camera.range<Cal3Bundler>(camera2);
271 }
272 
273 /* ************************************************************************* */
274 TEST( PinholePose, range2) {
275  Matrix D1; Matrix D2;
276  double result = camera.range<Cal3Bundler>(camera2, D1, D2);
277  Matrix expectedDcamera = numericalDerivative21(range2, camera, camera2);
278  Matrix expectedDpoint = numericalDerivative22(range2, camera, camera2);
279  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
280  EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
281  EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
282 }
283 
284 /* ************************************************************************* */
285 static const CalibratedCamera camera3(pose1);
286 static double range3(const Camera& camera, const CalibratedCamera& camera3) {
287  return camera.range(camera3);
288 }
289 
290 /* ************************************************************************* */
291 TEST( PinholePose, range3) {
292  Matrix D1; Matrix D2;
293  double result = camera.range(camera3, D1, D2);
294  Matrix expectedDcamera = numericalDerivative21(range3, camera, camera3);
295  Matrix expectedDpoint = numericalDerivative22(range3, camera, camera3);
296  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
297  EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
298  EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
299 }
300 
301 /* ************************************************************************* */
302 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
303 /* ************************************************************************* */
304 
305 
PinholePose.h
Pinhole camera with known calibration.
point1
static const Point3 point1(-0.08,-0.08, 0.0)
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
camera3
static const CalibratedCamera camera3(pose1)
project
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Definition: testPinholePose.cpp:188
project3
static Point2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2::shared_ptr &cal)
Definition: testPinholePose.cpp:139
point2_inf
static const Unit3 point2_inf(-0.16, 0.16,-1.0)
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
point4_inf
static const Unit3 point4_inf(0.16,-0.16,-1.0)
TEST
TEST(PinholePose, constructor)
Definition: testPinholePose.cpp:55
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
pose
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
camera1
static const Camera camera1(pose1, K)
gtsam::Point2
Vector2 Point2
Definition: Point2.h:27
point1_inf
static const Unit3 point1_inf(-0.16,-0.16,-1.0)
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
R
Rot2 R(Rot2::fromAngle(0.1))
range2
static double range2(const Camera &camera, const Camera2 &camera2)
Definition: testPinholePose.cpp:269
Camera
PinholePose< Cal3_S2 > Camera
Definition: testPinholePose.cpp:34
std
Definition: Half.h:150
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
gtsam::numericalDerivative42
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative42(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Definition: numericalDerivative.h:342
K
static const Cal3_S2::shared_ptr K
Definition: testPinholePose.cpp:36
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
range1
static double range1(const Camera &camera, const Pose3 &pose)
Definition: testPinholePose.cpp:249
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
pointAtInfinity
Unit3 pointAtInfinity(0, 0,-1000)
example::point
Point3 point(10, 0,-5)
point3
static const Point3 point3(0.08, 0.08, 0.0)
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
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
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
backproject
static Point3 backproject(const Pose3 &pose, const Cal3_S2 &cal, const Point2 &p, const double &depth)
Definition: testPinholePose.cpp:207
K2
static const boost::shared_ptr< Cal3Bundler > K2
Definition: testPinholePose.cpp:266
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
point2
static const Point3 point2(-0.08, 0.08, 0.0)
gtsam::Cal3_S2
Definition: Cal3_S2.h:34
gtsam::Cal3::vector
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3.h:160
gtsam::lago::I
static const Matrix I
Definition: lago.cpp:35
Camera2
PinholePose< Cal3Bundler > Camera2
Definition: testPinholePose.cpp:265
range3
static double range3(const Camera &camera, const CalibratedCamera &camera3)
Definition: testPinholePose.cpp:286
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:37
gtsam::numericalDerivative43
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative43(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Definition: numericalDerivative.h:370
camera2
static const Camera2 camera2(pose1, K2)
gtsam
traits
Definition: chartTesting.h:28
gtsam::numericalDerivative41
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative41(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Definition: numericalDerivative.h:314
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
main
int main()
Definition: testPinholePose.cpp:302
range0
static double range0(const Camera &camera, const Point3 &point)
Definition: testPinholePose.cpp:233
p
float * p
Definition: Tutorial_Map_using.cpp:9
point3_inf
static const Unit3 point3_inf(0.16, 0.16,-1.0)
camera
static const Camera camera(pose, K)
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
point4
static const Point3 point4(0.08,-0.08, 0.0)
Pose2.h
2D Pose
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::Cal3_S2::shared_ptr
boost::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
Cal3Bundler.h
Calibration used by Bundler.
project4
static Point2 project4(const Camera &camera, const Point3 &point)
Definition: testPinholePose.cpp:157
gtsam::numericalDerivative44
internal::FixedSizeMatrix< Y, X4 >::type numericalDerivative44(boost::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
Definition: numericalDerivative.h:398
Cal3_S2.h
The most common 5DOF 3D->2D calibration.

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