testCal3Bundler.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 
17 #include <CppUnitLite/TestHarness.h>
18 #include <gtsam/base/Testable.h>
19 #include <gtsam/base/TestableAssertions.h>
20 #include <gtsam/base/numericalDerivative.h>
21 #include <gtsam/geometry/Cal3Bundler.h>
22 
23 using namespace gtsam;
24 
25 GTSAM_CONCEPT_TESTABLE_INST(Cal3Bundler)
26 GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler)
27 
28 static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
29 static Point2 p(2, 3);
30 
31 /* ************************************************************************* */
32 TEST(Cal3Bundler, vector) {
33  Cal3Bundler K;
34  Vector expected(3);
35  expected << 1, 0, 0;
36  CHECK(assert_equal(expected, K.vector()));
37 }
38 
39 /* ************************************************************************* */
40 TEST(Cal3Bundler, uncalibrate) {
41  Vector v = K.vector();
42  double r = p.x() * p.x() + p.y() * p.y();
43  double g = v[0] * (1 + v[1] * r + v[2] * r * r);
44  Point2 expected(1000 + g * p.x(), 2000 + g * p.y());
45  Point2 actual = K.uncalibrate(p);
46  CHECK(assert_equal(expected, actual));
47 }
48 
49 TEST(Cal3Bundler, calibrate) {
50  Point2 pn(0.5, 0.5);
51  Point2 pi = K.uncalibrate(pn);
52  Point2 pn_hat = K.calibrate(pi);
53  CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
54 }
55 
56 /* ************************************************************************* */
57 Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) {
58  return k.uncalibrate(pt);
59 }
60 
61 Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
62  return k.calibrate(pt);
63 }
64 
65 /* ************************************************************************* */
66 TEST(Cal3Bundler, DuncalibrateDefault) {
67  Cal3Bundler trueK(1, 0, 0);
68  Matrix Dcal, Dp;
69  Point2 actual = trueK.uncalibrate(p, Dcal, Dp);
70  Point2 expected = p;
71  CHECK(assert_equal(expected, actual, 1e-7));
72  Matrix numerical1 = numericalDerivative21(uncalibrate_, trueK, p);
73  Matrix numerical2 = numericalDerivative22(uncalibrate_, trueK, p);
74  CHECK(assert_equal(numerical1, Dcal, 1e-7));
75  CHECK(assert_equal(numerical2, Dp, 1e-7));
76 }
77 
78 /* ************************************************************************* */
79 TEST(Cal3Bundler, DcalibrateDefault) {
80  Cal3Bundler trueK(1, 0, 0);
81  Matrix Dcal, Dp;
82  Point2 pn(0.5, 0.5);
83  Point2 pi = trueK.uncalibrate(pn);
84  Point2 actual = trueK.calibrate(pi, Dcal, Dp);
85  CHECK(assert_equal(pn, actual, 1e-7));
86  Matrix numerical1 = numericalDerivative21(calibrate_, trueK, pi);
87  Matrix numerical2 = numericalDerivative22(calibrate_, trueK, pi);
88  CHECK(assert_equal(numerical1, Dcal, 1e-5));
89  CHECK(assert_equal(numerical2, Dp, 1e-5));
90 }
91 
92 /* ************************************************************************* */
93 TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
94  Cal3Bundler K(5, 0, 0, 2, 2);
95  Matrix Dcal, Dp;
96  Point2 actual = K.uncalibrate(p, Dcal, Dp);
97  Point2 expected(12, 17);
98  CHECK(assert_equal(expected, actual, 1e-7));
99  Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
100  Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
101  CHECK(assert_equal(numerical1, Dcal, 1e-7));
102  CHECK(assert_equal(numerical2, Dp, 1e-7));
103 }
104 
105 /* ************************************************************************* */
106 TEST(Cal3Bundler, DcalibratePrincipalPoint) {
107  Cal3Bundler K(2, 0, 0, 2, 2);
108  Matrix Dcal, Dp;
109  Point2 pn(0.5, 0.5);
110  Point2 pi = K.uncalibrate(pn);
111  Point2 actual = K.calibrate(pi, Dcal, Dp);
112  CHECK(assert_equal(pn, actual, 1e-7));
113  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
114  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
115  CHECK(assert_equal(numerical1, Dcal, 1e-5));
116  CHECK(assert_equal(numerical2, Dp, 1e-5));
117 }
118 
119 /* ************************************************************************* */
120 TEST(Cal3Bundler, Duncalibrate) {
121  Matrix Dcal, Dp;
122  Point2 actual = K.uncalibrate(p, Dcal, Dp);
123  Point2 expected(2182, 3773);
124  CHECK(assert_equal(expected, actual, 1e-7));
125  Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
126  Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
127  CHECK(assert_equal(numerical1, Dcal, 1e-7));
128  CHECK(assert_equal(numerical2, Dp, 1e-7));
129 }
130 
131 /* ************************************************************************* */
132 TEST(Cal3Bundler, Dcalibrate) {
133  Matrix Dcal, Dp;
134  Point2 pn(0.5, 0.5);
135  Point2 pi = K.uncalibrate(pn);
136  Point2 actual = K.calibrate(pi, Dcal, Dp);
137  CHECK(assert_equal(pn, actual, 1e-7));
138  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
139  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
140  CHECK(assert_equal(numerical1, Dcal, 1e-5));
141  CHECK(assert_equal(numerical2, Dp, 1e-5));
142 }
143 
144 /* ************************************************************************* */
145 TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); }
146 
147 /* ************************************************************************* */
148 TEST(Cal3Bundler, retract) {
149  Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
150  EXPECT_LONGS_EQUAL(3, expected.dim());
151 
152  EXPECT_LONGS_EQUAL(Cal3Bundler::Dim(), 3);
153  EXPECT_LONGS_EQUAL(expected.dim(), 3);
154 
155  Vector3 d;
156  d << 10, 1e-3, 1e-3;
157  Cal3Bundler actual = K.retract(d);
158  CHECK(assert_equal(expected, actual, 1e-7));
159  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
160 }
161 
162 /* ************************************************************************* */
163 TEST(Cal3Bundler, Print) {
164  Cal3Bundler cal(1, 2, 3, 4, 5);
165  std::stringstream os;
166  os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
167  << ", px: " << cal.px() << ", py: " << cal.py();
168 
169  EXPECT(assert_stdout_equal(os.str(), cal));
170 }
171 
172 /* ************************************************************************* */
173 int main() {
174  TestResult tr;
175  return TestRegistry::runAllTests(tr);
176 }
177 /* ************************************************************************* */
gtsam::assert_stdout_equal
bool assert_stdout_equal(const std::string &expected, const V &actual)
Definition: TestableAssertions.h:330
test_docs.d
d
Definition: test_docs.py:28
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::Cal3Bundler::localCoordinates
Vector3 localCoordinates(const Cal3Bundler &T2) const
Calculate local coordinates to another calibration.
Definition: Cal3Bundler.h:150
CHECK
#define CHECK(condition)
Definition: Test.h:108
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::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Cal3Bundler::vector
Vector3 vector() const
Definition: Cal3Bundler.cpp:42
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
main
int main()
Definition: testCal3Bundler.cpp:173
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
numericalDerivative.h
Some functions to compute numerical derivatives.
pt
static const Point3 pt(1.0, 2.0, 3.0)
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
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
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
K
static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000)
gtsam::Cal3Bundler::retract
Cal3Bundler retract(const Vector &d) const
Update calibration with tangent space delta.
Definition: Cal3Bundler.h:145
gtsam::Cal3Bundler::calibrate
Point2 calibrate(const Point2 &pi, OptionalJacobian< 2, 3 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3Bundler.cpp:95
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Cal3Bundler::k1
double k1() const
distorsion parameter k1
Definition: Cal3Bundler.h:87
gtsam::Cal3::fx
double fx() const
focal length x
Definition: Cal3.h:139
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
uncalibrate_
Point2 uncalibrate_(const Cal3Bundler &k, const Point2 &pt)
Definition: testCal3Bundler.cpp:57
gtsam::Cal3Bundler::dim
size_t dim() const override
return DOF, dimensionality of tangent space
Definition: Cal3Bundler.h:139
gtsam
traits
Definition: chartTesting.h:28
gtsam::Cal3Bundler::k2
double k2() const
distorsion parameter k2
Definition: Cal3Bundler.h:90
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.cal
cal
Definition: test_ProjectionFactorRollingShutter.py:27
GTSAM_CONCEPT_MANIFOLD_INST
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
Definition: Manifold.h:177
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
p
static Point2 p(2, 3)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
os
ofstream os("timeSchurFactors.csv")
gtsam::uncalibrate
Point2_ uncalibrate(const Expression< CALIBRATION > &K, const Point2_ &xy_hat)
Definition: slam/expressions.h:172
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::Cal3Bundler::px
double px() const
image center in x
Definition: Cal3Bundler.h:93
gtsam::Cal3Bundler::uncalibrate
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 3 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
: convert intrinsic coordinates xy to image coordinates uv Version of uncalibrate with derivatives ...
Definition: Cal3Bundler.cpp:66
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
gtsam::Cal3Bundler::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition: Cal3Bundler.h:142
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:62
Cal3Bundler.h
Calibration used by Bundler.
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
gtsam::Cal3Bundler::py
double py() const
image center in y
Definition: Cal3Bundler.h:96
calibrate_
Point2 calibrate_(const Cal3Bundler &k, const Point2 &pt)
Definition: testCal3Bundler.cpp:61

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