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 = std::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  std::function<Pose3(Camera)> f = //
69  std::bind(&Camera::getPose,_1,{});
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 std::shared_ptr<Cal3Bundler> K2 =
267  std::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 
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
point1
static const Point3 point1(-0.08,-0.08, 0.0)
gtsam::numericalDerivative41
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative41(std::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:325
Pose2.h
2D Pose
range2
static double range2(const Camera &camera, const Camera2 &camera2)
Definition: testPinholePose.cpp:269
backproject
static Point3 backproject(const Pose3 &pose, const Cal3_S2 &cal, const Point2 &p, const double &depth)
Definition: testPinholePose.cpp:207
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
TestHarness.h
Camera
Definition: camera.h:36
range1
static double range1(const Camera &camera, const Pose3 &pose)
Definition: testPinholePose.cpp:249
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
trans
static char trans
Definition: blas_interface.hh:58
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
Camera2
PinholePose< Cal3Bundler > Camera2
Definition: testPinholePose.cpp:265
Camera
PinholePose< Cal3_S2 > Camera
Definition: testPinholePose.cpp:34
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
main
int main()
Definition: testPinholePose.cpp:302
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
C2
Definition: test_operator_overloading.cpp:98
C2
static double C2[]
Definition: shichi.c:141
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
fixture::cal
const Cal3_S2 cal(focalLength, focalLength, 0.0, principalPoint.x(), principalPoint.y())
project3
static Point2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2::shared_ptr &cal)
Definition: testPinholePose.cpp:139
point1_inf
static const Unit3 point1_inf(-0.16,-0.16, -1.0)
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
camera
static const Camera camera(pose, K)
point4
static const Point3 point4(0.08,-0.08, 0.0)
numericalDerivative.h
Some functions to compute numerical derivatives.
point3_inf
static const Unit3 point3_inf(0.16, 0.16, -1.0)
camera3
static const CalibratedCamera camera3(pose1)
K2
static const std::shared_ptr< Cal3Bundler > K2
Definition: testPinholePose.cpp:266
I
#define I
Definition: main.h:112
point4_inf
static const Unit3 point4_inf(0.16,-0.16, -1.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::PinholeCamera< Cal3Bundler >
gtsam::numericalDerivative44
internal::FixedSizeMatrix< Y, X4 >::type numericalDerivative44(std::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:427
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
gtsam::PinholeCamera::pose
const Pose3 & pose() const
return pose
Definition: PinholeCamera.h:164
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
range3
static double range3(const Camera &camera, const CalibratedCamera &camera3)
Definition: testPinholePose.cpp:286
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
TEST
TEST(PinholePose, constructor)
Definition: testPinholePose.cpp:55
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
point2
static const Point3 point2(-0.08, 0.08, 0.0)
gtsam::CalibratedCamera
Definition: CalibratedCamera.h:251
camera1
static const Camera camera1(pose1, K)
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
gtsam::numericalDerivative43
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative43(std::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:393
TestResult
Definition: TestResult.h:26
camera2
static const Camera2 camera2(pose1, K2)
point2_inf
static const Unit3 point2_inf(-0.16, 0.16, -1.0)
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: SFMdata.h:40
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
gtsam::Cal3::vector
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3.h:160
gtsam::numericalDerivative42
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative42(std::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:359
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
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
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
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
K
static const Cal3_S2::shared_ptr K
Definition: testPinholePose.cpp:36
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
depth
static double depth
Definition: testSphericalCamera.cpp:64
gtsam::PinholePose
Definition: PinholePose.h:239
range0
static double range0(const Camera &camera, const Point3 &point)
Definition: testPinholePose.cpp:233
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:27
pointAtInfinity
Unit3 pointAtInfinity(0, 0, -1000)
project4
static Point2 project4(const Camera &camera, const Point3 &point)
Definition: testPinholePose.cpp:157
Cal3Bundler.h
Calibration used by Bundler.
R
Rot2 R(Rot2::fromAngle(0.1))
PinholePose.h
Pinhole camera with known calibration.
point3
static const Point3 point3(0.08, 0.08, 0.0)

gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:06