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 
20 #include <gtsam/base/Testable.h>
23 
24 using namespace std;
25 using namespace gtsam;
26 
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);
35  CHECK(assert_equal(d,b-a));
36 }
37 
38 /* ************************************************************************* */
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));
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));
113 
114  // point behind the camera
115  Point3 p(0, 0, -5);
116 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
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 /* ************************************************************************* */
#define CHECK(condition)
Definition: Test.h:108
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Scalar * b
Definition: benchVecAdd.cpp:17
static Point3 p3
Concept check for values that can be used in unit tests.
static int runAllTests(TestResult &result)
Matrix expected
Definition: testMatrix.cpp:971
KeyInt p4(x4, 4)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
Rot2 R(Rot2::fromAngle(0.1))
Definition: BFloat16.h:88
Some functions to compute numerical derivatives.
#define CHECK_EXCEPTION(condition, exception_name)
Definition: Test.h:118
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
static Point3 landmark(0, 0, 5)
static Pose3 camPose(Rot3((Matrix3()<< 1., 0., 0., 0.,-1., 0., 0., 0.,-1.).finished()), Point3(0, 0, 6.25))
static const Cal3Bundler K2(625, 1e-3, 1e-3)
static const Line3 l(Rot3(), 1, 1)
Values result
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
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)
Point3 backproject2(const StereoPoint2 &z, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
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.)
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v)
static char right
int main()
std::shared_ptr< Cal3_S2Stereo > shared_ptr
Definition: Cal3_S2Stereo.h:38
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5))
traits
Definition: chartTesting.h:28
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
Definition: Manifold.h:177
A Stereo Camera based on two Simple Cameras.
static Point3 backproject3(const Pose3 &pose, const StereoPoint2 &point, const Cal3_S2Stereo &K)
float * p
Point3 backproject(const StereoPoint2 &z) const
back-project a measurement
static Point3 p2
Vector3 Point3
Definition: Point3.h:38
static const CalibratedCamera camera(kDefaultPose)
TEST(StereoCamera, operators)
static StereoPoint2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2Stereo &K)
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
Point2 t(10, 10)
static StereoCamera stereoCam(camPose, K)
StereoPoint2 project2(const Point3 &point, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const


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