testStereoCamera.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 <CppUnitLite/TestHarness.h>
20 #include <gtsam/base/Testable.h>
21 #include <gtsam/base/numericalDerivative.h>
22 #include <gtsam/geometry/StereoCamera.h>
23 
24 using namespace std;
25 using namespace gtsam;
26 
27 GTSAM_CONCEPT_TESTABLE_INST(StereoCamera)
28 GTSAM_CONCEPT_MANIFOLD_INST(StereoCamera)
29 
30 /* ************************************************************************* */
31 TEST( StereoCamera, operators)
32 {
33  StereoPoint2 a(1, 2, 3), b(4, 5, 6), c(5, 7, 9), d(3, 3, 3);
34  CHECK(assert_equal(c,a+b));
35  CHECK(assert_equal(d,b-a));
36 }
37 
38 /* ************************************************************************* */
39 TEST( StereoCamera, project)
40 {
41  // create a Stereo camera at the origin with focal length 1500, baseline 0.5m
42  // and principal point 320, 240 (for a hypothetical 640x480 sensor)
43  Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
44  StereoCamera stereoCam(Pose3(), K);
45 
46  // point X Y Z in meters
47  Point3 p(0, 0, 5);
48  StereoPoint2 result = stereoCam.project(p);
49  CHECK(assert_equal(StereoPoint2(320, 320 - 150, 240), result));
50 
51  // point X Y Z in meters
52  Point3 p2(1, 1, 5);
53  StereoPoint2 result2 = stereoCam.project(p2);
54  CHECK(assert_equal(StereoPoint2(320.0+300.0,320.0+150.0,240.0+300),result2));
55 
56  // move forward 1 meter and try the same thing
57  // point X Y Z in meters
58  Point3 p3(1, 1, 6);
59  Rot3 unit = Rot3();
60  Point3 one_meter_z(0, 0, 1);
61  Pose3 camPose3(unit, one_meter_z);
62  StereoCamera stereoCam3(camPose3, K);
63  StereoPoint2 result3 = stereoCam3.project(p3);
64  CHECK(assert_equal(StereoPoint2(320.0+300.0, 320.0+150.0, 240.0+300),result3));
65 
66  // camera at (0,0,1) looking right (90deg)
67  Point3 p4(5, 1, 0);
68  Rot3 right = Rot3(0, 0, 1, 0, 1, 0, -1, 0, 0);
69  Pose3 camPose4(right, one_meter_z);
70  StereoCamera stereoCam4(camPose4, K);
71  StereoPoint2 result4 = stereoCam4.project(p4);
72  CHECK(assert_equal(StereoPoint2(320.0+300.0,320.0+150.0,240.0+300),result4));
73 }
74 
75 /* ************************************************************************* */
76 
77 static Pose3 camPose(Rot3((Matrix3() <<
78  1., 0., 0.,
79  0.,-1., 0.,
80  0., 0.,-1.
81  ).finished()),
82  Point3(0,0,6.25));
83 
84 static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
85 static StereoCamera stereoCam(camPose, K);
86 
87 // point X Y Z in meters
88 static Point3 landmark(0, 0, 5);
89 
90 /* ************************************************************************* */
91 static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_S2Stereo& K) {
92  return StereoCamera(pose, std::make_shared<Cal3_S2Stereo>(K)).project(point);
93 }
94 
95 /* ************************************************************************* */
96 TEST( StereoCamera, Dproject)
97 {
98  Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K);
99  Matrix actual1; stereoCam.project2(landmark, actual1, {});
100  CHECK(assert_equal(expected1,actual1,1e-7));
101 
102  Matrix expected2 = numericalDerivative32(project3, camPose, landmark, *K);
103  Matrix actual2; stereoCam.project2(landmark, {}, actual2);
104  CHECK(assert_equal(expected2,actual2,1e-7));
105 }
106 
107 /* ************************************************************************* */
108 TEST( StereoCamera, projectCheirality)
109 {
110  // create a Stereo camera
111  Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
112  StereoCamera stereoCam(Pose3(), K);
113 
114  // point behind the camera
115  Point3 p(0, 0, -5);
116 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
117  CHECK_EXCEPTION(stereoCam.project2(p), StereoCheiralityException);
118 #else // otherwise project should not throw the exception
119  StereoPoint2 expected = StereoPoint2(320, 470, 240);
120  CHECK(assert_equal(expected,stereoCam.project2(p),1e-7));
121 #endif
122 }
123 
124 /* ************************************************************************* */
125 TEST( StereoCamera, backproject_case1)
126 {
127  Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
128  StereoCamera stereoCam2(Pose3(), K2);
129 
130  Point3 expected(1.2, 2.3, 4.5);
131  StereoPoint2 stereo_point = stereoCam2.project(expected);
132  Point3 actual = stereoCam2.backproject(stereo_point);
133  CHECK(assert_equal(expected,actual,1e-8));
134 }
135 
136 /* ************************************************************************* */
137 TEST( StereoCamera, backproject_case2)
138 {
139  Rot3 R(0.589511291, -0.804859792, 0.0683931805,
140  -0.804435942, -0.592650676, -0.0405925523,
141  0.0732045588, -0.0310882277, -0.996832359);
142  Point3 t(53.5239823, 23.7866016, -4.42379876);
143  Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612));
144  StereoCamera camera(Pose3(R,t), K);
145  StereoPoint2 z(184.812, 129.068, 714.768);
146 
147  Point3 l = camera.backproject(z);
148  StereoPoint2 actual = camera.project(l);
149  CHECK(assert_equal(z, actual, 1e-3));
150 }
151 
152 static Point3 backproject3(const Pose3& pose, const StereoPoint2& point, const Cal3_S2Stereo& K) {
153  return StereoCamera(pose, std::make_shared<Cal3_S2Stereo>(K)).backproject(point);
154 }
155 
156 /* ************************************************************************* */
157 TEST( StereoCamera, backproject2_case1)
158 {
159  Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
160  StereoCamera stereoCam2(Pose3(), K2);
161 
162  Point3 expected_point(1.2, 2.3, 4.5);
163  StereoPoint2 stereo_point = stereoCam2.project(expected_point);
164 
165  Matrix actual_jacobian_1, actual_jacobian_2;
166  Point3 actual_point = stereoCam2.backproject2(stereo_point, actual_jacobian_1, actual_jacobian_2);
167  CHECK(assert_equal(expected_point, actual_point, 1e-8));
168 
169  Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(), stereo_point, *K2);
170  CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-6));
171 
172  Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(), stereo_point, *K2);
173  CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-6));
174 }
175 
176 TEST( StereoCamera, backproject2_case2)
177 {
178  Rot3 R(0.589511291, -0.804859792, 0.0683931805,
179  -0.804435942, -0.592650676, -0.0405925523,
180  0.0732045588, -0.0310882277, -0.996832359);
181  Point3 t(53.5239823, 23.7866016, -4.42379876);
182  Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612));
183  StereoCamera camera(Pose3(R,t), K);
184  StereoPoint2 z(184.812, 129.068, 714.768);
185 
186  Matrix actual_jacobian_1, actual_jacobian_2;
187  Point3 l = camera.backproject2(z, actual_jacobian_1, actual_jacobian_2);
188 
189  StereoPoint2 actual = camera.project(l);
190  CHECK(assert_equal(z, actual, 1e-3));
191 
192  Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(R,t), z, *K);
193  CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-6));
194 
195  Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(R,t), z, *K);
196  CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-6));
197 }
198 
199 /* ************************************************************************* */
200  int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
201 /* ************************************************************************* */
test_docs.d
d
Definition: test_docs.py:28
CHECK
#define CHECK(condition)
Definition: Test.h:108
project
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Definition: testPinholePose.cpp:188
b
Scalar * b
Definition: benchVecAdd.cpp:17
simple::p3
static Point3 p3
Definition: testInitializePose3.cpp:53
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_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
p4
KeyInt p4(x4, 4)
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
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
R
Rot2 R(Rot2::fromAngle(0.1))
std
Definition: BFloat16.h:88
numericalDerivative.h
Some functions to compute numerical derivatives.
CHECK_EXCEPTION
#define CHECK_EXCEPTION(condition, exception_name)
Definition: Test.h:118
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
landmark
static Point3 landmark(0, 0, 5)
camPose
static Pose3 camPose(Rot3((Matrix3()<< 1., 0., 0., 0.,-1., 0., 0., 0.,-1.).finished()), Point3(0, 0, 6.25))
K2
static const Cal3Bundler K2(625, 1e-3, 1e-3)
l
static const Line3 l(Rot3(), 1, 1)
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
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::StereoCamera::backproject2
Point3 backproject2(const StereoPoint2 &z, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: StereoCamera.cpp:102
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.)
gtsam::StereoCamera::project
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v)
Definition: StereoCamera.cpp:32
right
static char right
Definition: blas_interface.hh:61
main
int main()
Definition: testStereoCamera.cpp:200
gtsam::Cal3_S2Stereo::shared_ptr
std::shared_ptr< Cal3_S2Stereo > shared_ptr
Definition: Cal3_S2Stereo.h:38
K
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5))
gtsam
traits
Definition: chartTesting.h:28
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
StereoCamera.h
A Stereo Camera based on two Simple Cameras.
backproject3
static Point3 backproject3(const Pose3 &pose, const StereoPoint2 &point, const Cal3_S2Stereo &K)
Definition: testStereoCamera.cpp:152
p
float * p
Definition: Tutorial_Map_using.cpp:9
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::StereoCamera::backproject
Point3 backproject(const StereoPoint2 &z) const
back-project a measurement
Definition: StereoCamera.cpp:92
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
camera
static const CalibratedCamera camera(kDefaultPose)
TEST
TEST(StereoCamera, operators)
Definition: testStereoCamera.cpp:31
project3
static StereoPoint2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2Stereo &K)
Definition: testStereoCamera.cpp:91
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
align_3::t
Point2 t(10, 10)
stereoCam
static StereoCamera stereoCam(camPose, K)
gtsam::StereoCamera::project2
StereoPoint2 project2(const Point3 &point, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: StereoCamera.cpp:37

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:41