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

gtsam
Author(s):
autogenerated on Tue Jun 25 2024 03:06:23