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 
20 #include <gtsam/geometry/Cal3_S2.h>
21 #include <gtsam/geometry/Pose2.h>
23 #include <gtsam/base/Testable.h>
25 
27 
28 #include <cmath>
29 #include <iostream>
30 
31 using namespace std;
32 using namespace gtsam;
33 
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 /* ************************************************************************* */
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 /* ************************************************************************* */
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 /* ************************************************************************* */
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 /* ************************************************************************* */
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 /* ************************************************************************* */
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 /* ************************************************************************* */
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 /* ************************************************************************* */
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 /* ************************************************************************* */
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 /* ************************************************************************* */
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 
Pinhole camera with known calibration.
static const Point3 point1(-0.08,-0.08, 0.0)
int EIGEN_BLAS_FUNC() rot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps)
#define CHECK(condition)
Definition: Test.h:108
static const CalibratedCamera camera3(pose1)
#define I
Definition: main.h:112
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
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)
static Point2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2::shared_ptr &cal)
Concept check for values that can be used in unit tests.
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
TEST(PinholePose, constructor)
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
Matrix expected
Definition: testMatrix.cpp:971
static const Camera camera1(pose1, K)
static double depth
Vector2 Point2
Definition: Point2.h:32
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Rot2 R(Rot2::fromAngle(0.1))
static double range2(const Camera &camera, const Camera2 &camera2)
PinholePose< Cal3_S2 > Camera
Definition: BFloat16.h:88
static char trans
Some functions to compute numerical derivatives.
static const Cal3_S2::shared_ptr K
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
static double range1(const Camera &camera, const Pose3 &pose)
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)
static const Unit3 point3_inf(0.16, 0.16, -1.0)
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
static const Unit3 point2_inf(-0.16, 0.16, -1.0)
static const Point3 point3(0.08, 0.08, 0.0)
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
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)
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
Values result
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)
#define EXPECT(condition)
Definition: Test.h:150
static Point3 backproject(const Pose3 &pose, const Cal3_S2 &cal, const Point2 &p, const double &depth)
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)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
static const Point3 point2(-0.08, 0.08, 0.0)
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
PinholePose< Cal3Bundler > Camera2
static double range3(const Camera &camera, const CalibratedCamera &camera3)
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
static const Camera2 camera2(pose1, K2)
traits
Definition: chartTesting.h:28
int main()
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)
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
static const Unit3 point4_inf(0.16,-0.16, -1.0)
static double range0(const Camera &camera, const Point3 &point)
Unit3 pointAtInfinity(0, 0, -1000)
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)
float * p
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3.h:160
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
static const Camera camera(pose, K)
Vector3 Point3
Definition: Point3.h:38
static const Point3 point4(0.08,-0.08, 0.0)
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition: Point3.cpp:27
2D Pose
Definition: camera.h:36
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
Calibration used by Bundler.
static Point2 project4(const Camera &camera, const Point3 &point)
The most common 5DOF 3D->2D calibration.
static const std::shared_ptr< Cal3Bundler > K2
static const Unit3 point1_inf(-0.16,-0.16, -1.0)


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