testCal3_S2.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/base/Testable.h>
21 #include <gtsam/geometry/Cal3_S2.h>
22 
23 using namespace gtsam;
24 
27 
28 static Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2);
29 static Point2 p(1, -2);
30 static Point2 p_uv(1320.3, 1740);
31 static Point2 p_xy(2, 3);
32 
33 /* ************************************************************************* */
34 TEST(Cal3_S2, Constructor) {
35  Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2);
36 
37  double fov = 60; // degrees
38  size_t w = 640, h = 480;
39  Cal3_S2 actual(fov, w, h);
40 
41  CHECK(assert_equal(expected, actual, 1e-3));
42 }
43 
44 /* ************************************************************************* */
45 TEST(Cal3_S2, Calibrate) {
46  Point2 intrinsic(2, 3);
47  Point2 expectedimage(1320.3, 1740);
48  Point2 imagecoordinates = K.uncalibrate(intrinsic);
49  CHECK(assert_equal(expectedimage, imagecoordinates));
50  CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates)));
51 }
52 
53 /* ************************************************************************* */
54 TEST(Cal3_S2, CalibrateHomogeneous) {
55  Vector3 intrinsic(2, 3, 1);
56  Vector3 image(1320.3, 1740, 1);
57  CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
58 }
59 
60 /* ************************************************************************* */
61 Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) {
62  return k.uncalibrate(pt);
63 }
64 
65 TEST(Cal3_S2, Duncalibrate1) {
66  Matrix25 computed;
67  K.uncalibrate(p, computed, {});
69  CHECK(assert_equal(numerical, computed, 1e-8));
70 }
71 
72 /* ************************************************************************* */
73 TEST(Cal3_S2, Duncalibrate2) {
74  Matrix computed;
75  K.uncalibrate(p, {}, computed);
77  CHECK(assert_equal(numerical, computed, 1e-9));
78 }
79 
80 Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {
81  return k.calibrate(pt);
82 }
83 
84 /* ************************************************************************* */
85 TEST(Cal3_S2, Dcalibrate1) {
86  Matrix computed;
87  Point2 expected = K.calibrate(p_uv, computed, {});
89  CHECK(assert_equal(expected, p_xy, 1e-8));
90  CHECK(assert_equal(numerical, computed, 1e-8));
91 }
92 
93 /* ************************************************************************* */
94 TEST(Cal3_S2, Dcalibrate2) {
95  Matrix computed;
96  Point2 expected = K.calibrate(p_uv, {}, computed);
98  CHECK(assert_equal(expected, p_xy, 1e-8));
99  CHECK(assert_equal(numerical, computed, 1e-8));
100 }
101 
102 /* ************************************************************************* */
103 TEST(Cal3_S2, Equal) {
104  CHECK(assert_equal(K, K, 1e-9));
105 
106  Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2);
107  CHECK(assert_equal(K, K1, 1e-9));
108 }
109 
110 /* ************************************************************************* */
111 TEST(Cal3_S2, Retract) {
112  Cal3_S2 expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5);
113 
115  EXPECT_LONGS_EQUAL(expected.dim(), 5);
116 
117  Vector5 d;
118  d << 1, 2, 3, 4, 5;
119  Cal3_S2 actual = K.retract(d);
120  CHECK(assert_equal(expected, actual, 1e-7));
121  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
122 }
123 
124 /* ************************************************************************* */
126  Cal3_S2 k1(5, 5, 5, 5, 5), k2(5, 6, 7, 8, 9);
127  Matrix H1, H2;
128 
129  EXPECT(assert_equal(Cal3_S2(0, 1, 2, 3, 4), k1.between(k2, H1, H2)));
130  EXPECT(assert_equal(-I_5x5, H1));
131  EXPECT(assert_equal(I_5x5, H2));
132 }
133 
134 /* ************************************************************************* */
136  Cal3_S2 cal(5, 5, 5, 5, 5);
137  std::stringstream os;
138  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
139  << ", px: " << cal.px() << ", py: " << cal.py();
140 
141  EXPECT(assert_stdout_equal(os.str(), cal));
142 }
143 
144 /* ************************************************************************* */
145 int main() {
146  TestResult tr;
147  return TestRegistry::runAllTests(tr);
148 }
149 /* ************************************************************************* */
bool assert_stdout_equal(const std::string &expected, const V &actual)
Provides additional testing facilities for common data structures.
#define CHECK(condition)
Definition: Test.h:108
Concept check for values that can be used in unit tests.
double skew() const
skew
Definition: Cal3.h:148
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
static Point2 p(1, -2)
Matrix expected
Definition: testMatrix.cpp:971
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
Vector5 localCoordinates(const Cal3_S2 &T2) const
Unretraction for the calibration.
Definition: Cal3_S2.h:126
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
static Point2 p_uv(1320.3, 1740)
double py() const
image center in y
Definition: Cal3.h:154
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3_S2.cpp:53
virtual size_t dim() const
return DOF, dimensionality of tangent space
Definition: Cal3.h:177
Some functions to compute numerical derivatives.
static const Point3 pt(1.0, 2.0, 3.0)
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)
Eigen::VectorXd Vector
Definition: Vector.h:38
#define EXPECT(condition)
Definition: Test.h:150
double fx() const
focal length x
Definition: Cal3.h:139
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static Cal3_S2::shared_ptr K1(new Cal3_S2(fov, w, h))
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
RowVector3d w
traits
Definition: chartTesting.h:28
int main()
Cal3_S2 retract(const Vector &d) const
Given 5-dim tangent vector, create new calibration.
Definition: Cal3_S2.h:121
const double h
Point2 uncalibrate_(const Cal3_S2 &k, const Point2 &pt)
Definition: testCal3_S2.cpp:61
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
Definition: Manifold.h:177
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
ofstream os("timeSchurFactors.csv")
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Point2 calibrate_(const Cal3_S2 &k, const Point2 &pt)
Definition: testCal3_S2.cpp:80
TEST(SmartFactorBase, Pinhole)
static size_t Dim()
return DOF, dimensionality of tangent space
Definition: Cal3_S2.h:118
static Point2 p_xy(2, 3)
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Cal3_S2 between(const Cal3_S2 &q, OptionalJacobian< 5, 5 > H1={}, OptionalJacobian< 5, 5 > H2={}) const
"Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
Definition: Cal3_S2.h:104
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:62
static double fov
double fy() const
focal length y
Definition: Cal3.h:142
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
The most common 5DOF 3D->2D calibration.
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3_S2.cpp:44
double px() const
image center in x
Definition: Cal3.h:151


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