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 
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:79
CHECK
#define CHECK(condition)
Definition: Test.h:108
camera3
static const CalibratedCamera camera3(pose1)
I
#define I
Definition: main.h:112
project
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Definition: testPinholePose.cpp:188
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
project3
static Point2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2::shared_ptr &cal)
Definition: testPinholePose.cpp:139
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
TEST
TEST(PinholePose, constructor)
Definition: testPinholePose.cpp:55
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
camera1
static const Camera camera1(pose1, K)
depth
static double depth
Definition: testSphericalCamera.cpp:57
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
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
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: BFloat16.h:88
trans
static char trans
Definition: blas_interface.hh:58
numericalDerivative.h
Some functions to compute numerical derivatives.
K
static const Cal3_S2::shared_ptr K
Definition: testPinholePose.cpp:36
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
range1
static double range1(const Camera &camera, const Pose3 &pose)
Definition: testPinholePose.cpp:249
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
point3_inf
static const Unit3 point3_inf(0.16, 0.16, -1.0)
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
point2_inf
static const Unit3 point2_inf(-0.16, 0.16, -1.0)
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::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
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
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
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
backproject
static Point3 backproject(const Pose3 &pose, const Cal3_S2 &cal, const Point2 &p, const double &depth)
Definition: testPinholePose.cpp:207
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.)
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
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
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:50
camera2
static const Camera2 camera2(pose1, K2)
gtsam
traits
Definition: chartTesting.h:28
main
int main()
Definition: testPinholePose.cpp:302
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
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.cal
cal
Definition: test_ProjectionFactorRollingShutter.py:27
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
point4_inf
static const Unit3 point4_inf(0.16,-0.16, -1.0)
range0
static double range0(const Camera &camera, const Point3 &point)
Definition: testPinholePose.cpp:233
pointAtInfinity
Unit3 pointAtInfinity(0, 0, -1000)
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
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Cal3::vector
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3.h:160
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
camera
static const Camera camera(pose, K)
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
point4
static const Point3 point4(0.08,-0.08, 0.0)
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
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
Cal3Bundler.h
Calibration used by Bundler.
project4
static Point2 project4(const Camera &camera, const Point3 &point)
Definition: testPinholePose.cpp:157
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
K2
static const std::shared_ptr< Cal3Bundler > K2
Definition: testPinholePose.cpp:266
point1_inf
static const Unit3 point1_inf(-0.16,-0.16, -1.0)

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:55