testCalibratedCamera.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/CalibratedCamera.h>
19 #include <gtsam/geometry/Pose2.h>
20 #include <gtsam/base/Testable.h>
21 #include <gtsam/base/numericalDerivative.h>
22 
23 #include <CppUnitLite/TestHarness.h>
24 
25 #include <iostream>
26 
27 using namespace std::placeholders;
28 using namespace std;
29 using namespace gtsam;
30 
31 GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
32 
33 // Camera situated at 0.5 meters high, looking down
34 static const Pose3 kDefaultPose(Rot3(Vector3(1, -1, -1).asDiagonal()),
35  Point3(0, 0, 0.5));
36 
37 static const CalibratedCamera camera(kDefaultPose);
38 
39 static const Point3 point1(-0.08,-0.08, 0.0);
40 static const Point3 point2(-0.08, 0.08, 0.0);
41 static const Point3 point3( 0.08, 0.08, 0.0);
42 static const Point3 point4( 0.08,-0.08, 0.0);
43 
44 /* ************************************************************************* */
45 TEST( CalibratedCamera, constructor)
46 {
47  CHECK(assert_equal( camera.pose(), kDefaultPose));
48 }
49 
50 //******************************************************************************
51 TEST(CalibratedCamera, Create) {
52  Matrix actualH;
53  EXPECT(assert_equal(camera, CalibratedCamera::Create(kDefaultPose, actualH)));
54 
55  // Check derivative
56  std::function<CalibratedCamera(Pose3)> f = //
57  std::bind(CalibratedCamera::Create, std::placeholders::_1, nullptr);
58  Matrix numericalH = numericalDerivative11<CalibratedCamera, Pose3>(f, kDefaultPose);
59  EXPECT(assert_equal(numericalH, actualH, 1e-9));
60 }
61 
62 /* ************************************************************************* */
63 TEST( CalibratedCamera, level1)
64 {
65  // Create a level camera, looking in X-direction
66  Pose2 pose2(0.1,0.2,0);
67  CalibratedCamera camera = CalibratedCamera::Level(pose2, 0.3);
68 
69  // expected
70  Point3 x(0,-1,0),y(0,0,-1),z(1,0,0);
71  Rot3 wRc(x,y,z);
72  Pose3 expected(wRc,Point3(0.1,0.2,0.3));
73  CHECK(assert_equal( camera.pose(), expected));
74 }
75 
76 /* ************************************************************************* */
77 TEST( CalibratedCamera, level2)
78 {
79  // Create a level camera, looking in Y-direction
80  Pose2 pose2(0.4,0.3,M_PI/2.0);
81  CalibratedCamera camera = CalibratedCamera::Level(pose2, 0.1);
82 
83  // expected
84  Point3 x(1,0,0),y(0,0,-1),z(0,1,0);
85  Rot3 wRc(x,y,z);
86  Pose3 expected(wRc,Point3(0.4,0.3,0.1));
87  CHECK(assert_equal( camera.pose(), expected));
88 }
89 
90 /* ************************************************************************* */
91 TEST( CalibratedCamera, project)
92 {
93  CHECK(assert_equal( Point2(-.16, .16), camera.project(point1) ));
94  CHECK(assert_equal( Point2(-.16, -.16), camera.project(point2) ));
95  CHECK(assert_equal( Point2( .16, -.16), camera.project(point3) ));
96  CHECK(assert_equal( Point2( .16, .16), camera.project(point4) ));
97 }
98 
99 /* ************************************************************************* */
100 static Point2 Project1(const Point3& point) {
101  return PinholeBase::Project(point);
102 }
103 
104 TEST( CalibratedCamera, DProject1) {
105  Point3 pp(155, 233, 131);
106  Matrix test1;
107  CalibratedCamera::Project(pp, test1);
108  Matrix test2 = numericalDerivative11<Point2, Point3>(Project1, pp);
109  CHECK(assert_equal(test1, test2));
110 }
111 
112 /* ************************************************************************* */
113 static Point2 Project2(const Unit3& point) {
114  return PinholeBase::Project(point);
115 }
116 
117 Unit3 pointAtInfinity(0, 0, -1000);
118 TEST( CalibratedCamera, DProjectInfinity) {
119  Matrix test1;
120  CalibratedCamera::Project(pointAtInfinity, test1);
121  Matrix test2 = numericalDerivative11<Point2, Unit3>(Project2,
122  pointAtInfinity);
123  CHECK(assert_equal(test1, test2));
124 }
125 
126 /* ************************************************************************* */
127 static Point2 project2(const CalibratedCamera& camera, const Point3& point) {
128  return camera.project(point);
129 }
130 
131 
132 TEST( CalibratedCamera, Dproject_point_pose)
133 {
134  Matrix Dpose, Dpoint;
135  Point2 result = camera.project(point1, Dpose, Dpoint);
136  Matrix numerical_pose = numericalDerivative21(project2, camera, point1);
137  Matrix numerical_point = numericalDerivative22(project2, camera, point1);
138  CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transformTo(point1)));
139  CHECK(assert_equal(Point2(-.16, .16), result));
140  CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
141  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
142 }
143 
144 /* ************************************************************************* */
145 // Add a test with more arbitrary rotation
146 TEST( CalibratedCamera, Dproject_point_pose2)
147 {
148  static const Pose3 kDefaultPose(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
149  static const CalibratedCamera camera(kDefaultPose);
150  Matrix Dpose, Dpoint;
151  camera.project(point1, Dpose, Dpoint);
152  Matrix numerical_pose = numericalDerivative21(project2, camera, point1);
153  Matrix numerical_point = numericalDerivative22(project2, camera, point1);
154  CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
155  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
156 }
157 
158 /* ************************************************************************* */
159 static Point2 projectAtInfinity(const CalibratedCamera& camera, const Unit3& point) {
160  return camera.project2(point);
161 }
162 
163 TEST( CalibratedCamera, Dproject_point_pose_infinity)
164 {
165  Matrix Dpose, Dpoint;
166  Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint);
167  Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity);
168  Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity);
169  CHECK(assert_equal(Point2(0,0), result));
170  CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
171  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
172 }
173 
174 /* ************************************************************************* */
175 // Add a test with more arbitrary rotation
176 TEST( CalibratedCamera, Dproject_point_pose2_infinity)
177 {
178  static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10));
179  static const CalibratedCamera camera(pose);
180  Matrix Dpose, Dpoint;
181  camera.project2(pointAtInfinity, Dpose, Dpoint);
182  Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity);
183  Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity);
184  CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
185  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
186 }
187 
188 static Point3 BackprojectFromCamera(const CalibratedCamera& camera, const Point2& point,
189  const double& depth) {
190  return camera.BackprojectFromCamera(point, depth);
191 }
192 TEST( CalibratedCamera, DBackprojectFromCamera)
193 {
194  static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10));
195  static const CalibratedCamera camera(pose);
196  static const double depth = 5.4;
197  static const Point2 point(10.1, 50.3);
198  Matrix Dpoint, Ddepth;
199  camera.BackprojectFromCamera(point, depth, Dpoint, Ddepth);
200  Matrix numerical_point = numericalDerivative32(BackprojectFromCamera, camera, point, depth);
201  Matrix numerical_depth = numericalDerivative33(BackprojectFromCamera, camera, point, depth);
202  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
203  CHECK(assert_equal(numerical_depth, Ddepth, 1e-7));
204 }
205 
206 
207 static Point3 backproject(const Pose3& pose, const Point2& point, const double& depth) {
208  return CalibratedCamera(pose).backproject(point, depth);
209 }
210 TEST( PinholePose, DbackprojectCalibCamera)
211 {
212  Matrix36 Dpose;
213  Matrix31 Ddepth;
214  Matrix32 Dpoint;
215  const Point2 point(-100, 100);
216  const double depth(10);
217  static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10));
218  static const CalibratedCamera camera(pose);
219  camera.backproject(point, depth, Dpose, Dpoint, Ddepth);
220  Matrix expectedDpose = numericalDerivative31(backproject, pose, point, depth);
221  Matrix expectedDpoint = numericalDerivative32(backproject, pose, point, depth);
222  Matrix expectedDdepth = numericalDerivative33(backproject, pose,point, depth);
223 
224  EXPECT(assert_equal(expectedDpose, Dpose, 1e-7));
225  EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
226  EXPECT(assert_equal(expectedDdepth, Ddepth, 1e-7));
227 }
228 
229 
230 /* ************************************************************************* */
231 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
232 /* ************************************************************************* */
project2
static Point2 project2(const CalibratedCamera &camera, const Point3 &point)
Definition: testCalibratedCamera.cpp:127
CHECK
#define CHECK(condition)
Definition: Test.h:108
kDefaultPose
static const Pose3 kDefaultPose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
project
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Definition: testPinholePose.cpp:188
y
Scalar * y
Definition: level1_cplx_impl.h:124
BackprojectFromCamera
static Point3 BackprojectFromCamera(const CalibratedCamera &camera, const Point2 &point, const double &depth)
Definition: testCalibratedCamera.cpp:188
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::PinholeBase::BackprojectFromCamera
static Point3 BackprojectFromCamera(const Point2 &p, const double depth, OptionalJacobian< 3, 2 > Dpoint={}, OptionalJacobian< 3, 1 > Ddepth={})
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition: CalibratedCamera.cpp:167
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
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
M_PI
#define M_PI
Definition: main.h:106
std
Definition: BFloat16.h:88
numericalDerivative.h
Some functions to compute numerical derivatives.
test2
void test2(OptionalJacobian<-1,-1 > H={})
Definition: testOptionalJacobian.cpp:123
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
backproject
static Point3 backproject(const Pose3 &pose, const Point2 &point, const double &depth)
Definition: testCalibratedCamera.cpp:207
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
main
int main()
Definition: testCalibratedCamera.cpp:231
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
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
Project2
static Point2 Project2(const Unit3 &point)
Definition: testCalibratedCamera.cpp:113
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition: CalibratedCamera.h:154
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
TEST
TEST(CalibratedCamera, constructor)
Definition: testCalibratedCamera.cpp:45
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
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
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
point3
static const Point3 point3(0.08, 0.08, 0.0)
point2
static const Point3 point2(-0.08, 0.08, 0.0)
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
gtsam::CalibratedCamera::backproject
Point3 backproject(const Point2 &pn, double depth, OptionalJacobian< 3, 6 > Dresult_dpose={}, OptionalJacobian< 3, 2 > Dresult_dp={}, OptionalJacobian< 3, 1 > Dresult_ddepth={}) const
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition: CalibratedCamera.h:348
gtsam
traits
Definition: chartTesting.h:28
projectAtInfinity
static Point2 projectAtInfinity(const CalibratedCamera &camera, const Unit3 &point)
Definition: testCalibratedCamera.cpp:159
Project1
static Point2 Project1(const Point3 &point)
Definition: testCalibratedCamera.cpp:100
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
GTSAM_CONCEPT_MANIFOLD_INST
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
Definition: Manifold.h:177
point1
static const Point3 point1(-0.08,-0.08, 0.0)
pointAtInfinity
Unit3 pointAtInfinity(0, 0, -1000)
gtsam::PinholeBase::project2
Point2 project2(const Point3 &point, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: CalibratedCamera.cpp:116
gtsam::Pose3::transformTo
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in world coordinates and transforms it to Pose coordinates
Definition: Pose3.cpp:371
Project
Point2(* Project)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpressionFactor.cpp:40
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
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
point4
static const Point3 point4(0.08,-0.08, 0.0)
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
Pose2.h
2D Pose
camera
static const CalibratedCamera camera(kDefaultPose)
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::CalibratedCamera::project
Point2 project(const Point3 &point, OptionalJacobian< 2, 6 > Dcamera={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: CalibratedCamera.cpp:188

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:59