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 distortion = 1.0 + v[1] * r + v[2] * r * r;
44  double u = K.px() + v[0] * distortion * p.x();
45  double v_coord = K.py() + v[0] * distortion * p.y();
46  Point2 expected(u, v_coord);
47  Point2 actual = K.uncalibrate(p);
48  CHECK(assert_equal(expected, actual));
49 }
50 
51 /* ************************************************************************* */
52 TEST(Cal3Bundler, Calibrate) {
53  Point2 pn(0.5, 0.5);
54  Point2 pi = K.uncalibrate(pn);
55  Point2 pn_hat = K.calibrate(pi);
56  CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
57 }
58 
59 /* ************************************************************************* */
60 Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) {
61  return k.uncalibrate(pt);
62 }
63 
64 Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
65  return k.calibrate(pt);
66 }
67 
68 /* ************************************************************************* */
69 TEST(Cal3Bundler, DUncalibrateDefault) {
70  Cal3Bundler trueK(1, 0, 0);
71  Matrix Dcal, Dp;
72  Point2 actual = trueK.uncalibrate(p, Dcal, Dp);
73  Point2 expected(p); // Since K is identity, uncalibrate should return p
74  CHECK(assert_equal(expected, actual, 1e-7));
75  Matrix numerical1 = numericalDerivative21(uncalibrate_, trueK, p);
76  Matrix numerical2 = numericalDerivative22(uncalibrate_, trueK, p);
77  CHECK(assert_equal(numerical1, Dcal, 1e-7));
78  CHECK(assert_equal(numerical2, Dp, 1e-7));
79 }
80 
81 /* ************************************************************************* */
82 TEST(Cal3Bundler, DCalibrateDefault) {
83  Cal3Bundler trueK(1, 0, 0);
84  Matrix Dcal, Dp;
85  Point2 pn(0.5, 0.5);
86  Point2 pi = trueK.uncalibrate(pn);
87  Point2 actual = trueK.calibrate(pi, Dcal, Dp);
88  CHECK(assert_equal(pn, actual, 1e-7));
89  Matrix numerical1 = numericalDerivative21(calibrate_, trueK, pi);
90  Matrix numerical2 = numericalDerivative22(calibrate_, trueK, pi);
91  CHECK(assert_equal(numerical1, Dcal, 1e-5));
92  CHECK(assert_equal(numerical2, Dp, 1e-5));
93 }
94 
95 /* ************************************************************************* */
96 TEST(Cal3Bundler, DUncalibratePrincipalPoint) {
97  Cal3Bundler K(5, 0, 0, 2, 2);
98  Matrix Dcal, Dp;
99  Point2 actual = K.uncalibrate(p, Dcal, Dp);
100  Point2 expected(2.0 + 5.0 * p.x(), 2.0 + 5.0 * p.y());
101  CHECK(assert_equal(expected, actual, 1e-7));
102  Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
103  Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
104  CHECK(assert_equal(numerical1, Dcal, 1e-7));
105  CHECK(assert_equal(numerical2, Dp, 1e-7));
106 }
107 
108 /* ************************************************************************* */
109 TEST(Cal3Bundler, DCalibratePrincipalPoint) {
110  Cal3Bundler K(2, 0, 0, 2, 2);
111  Matrix Dcal, Dp;
112  Point2 pn(0.5, 0.5);
113  Point2 pi = K.uncalibrate(pn);
114  Point2 actual = K.calibrate(pi, Dcal, Dp);
115  CHECK(assert_equal(pn, actual, 1e-7));
116  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
117  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
118  CHECK(assert_equal(numerical1, Dcal, 1e-5));
119  CHECK(assert_equal(numerical2, Dp, 1e-5));
120 }
121 
122 /* ************************************************************************* */
123 TEST(Cal3Bundler, DUncalibrate) {
124  Matrix Dcal, Dp;
125  Point2 actual = K.uncalibrate(p, Dcal, Dp);
126  // Compute expected value manually
127  Vector v = K.vector();
128  double r2 = p.x() * p.x() + p.y() * p.y();
129  double distortion = 1.0 + v[1] * r2 + v[2] * r2 * r2;
130  Point2 expected(
131  K.px() + v[0] * distortion * p.x(),
132  K.py() + v[0] * distortion * p.y());
133  CHECK(assert_equal(expected, actual, 1e-7));
134 
135  Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
136  Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
137  CHECK(assert_equal(numerical1, Dcal, 1e-7));
138  CHECK(assert_equal(numerical2, Dp, 1e-7));
139 }
140 
141 /* ************************************************************************* */
142 TEST(Cal3Bundler, DCalibrate) {
143  Matrix Dcal, Dp;
144  Point2 pn(0.5, 0.5);
145  Point2 pi = K.uncalibrate(pn);
146  Point2 actual = K.calibrate(pi, Dcal, Dp);
147  CHECK(assert_equal(pn, actual, 1e-7));
148  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
149  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
150  CHECK(assert_equal(numerical1, Dcal, 1e-5));
151  CHECK(assert_equal(numerical2, Dp, 1e-5));
152 }
153 
154 /* ************************************************************************* */
155 TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); }
156 
157 /* ************************************************************************* */
158 TEST(Cal3Bundler, Retract) {
159  Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
160  EXPECT_LONGS_EQUAL(3, expected.dim());
161 
162  EXPECT_LONGS_EQUAL(Cal3Bundler::Dim(), 3);
163  EXPECT_LONGS_EQUAL(expected.dim(), 3);
164 
165  Vector3 d;
166  d << 10, 1e-3, 1e-3;
167  Cal3Bundler actual = K.retract(d);
168  CHECK(assert_equal(expected, actual, 1e-7));
169  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
170 }
171 
172 /* ************************************************************************* */
173 TEST(Cal3Bundler, Print) {
174  Cal3Bundler cal(1, 2, 3, 4, 5);
175  std::stringstream os;
176  os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
177  << ", px: " << cal.px() << ", py: " << cal.py();
178 
179  EXPECT(assert_stdout_equal(os.str(), cal));
180 }
181 
182 /* ************************************************************************* */
183 int main() {
184  TestResult tr;
185  return TestRegistry::runAllTests(tr);
186 }
187 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Cal3Bundler::retract
Cal3Bundler retract(const Vector &d) const
Update calibration with tangent space delta.
Definition: Cal3Bundler.h:134
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
gtsam::Cal3_S2::calibrate
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3_S2.cpp:53
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Cal3_S2::uncalibrate
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3_S2.cpp:44
uncalibrate_
Point2 uncalibrate_(const Cal3Bundler &k, const Point2 &pt)
Definition: testCal3Bundler.cpp:60
TestHarness.h
r2
static const double r2
Definition: testSmartRangeFactor.cpp:32
gtsam::Cal3::py
double py() const
image center in y
Definition: Cal3.h:154
gtsam::Cal3Bundler::localCoordinates
Vector3 localCoordinates(const Cal3Bundler &T2) const
Calculate local coordinates to another calibration.
Definition: Cal3Bundler.h:139
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
pt
static const Point3 pt(1.0, 2.0, 3.0)
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Cal3Bundler::vector
Vector3 vector() const
Definition: Cal3Bundler.cpp:42
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
K
static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000)
fixture::cal
const Cal3_S2 cal(focalLength, focalLength, 0.0, principalPoint.x(), principalPoint.y())
TestableAssertions.h
Provides additional testing facilities for common data structures.
calibrate_
Point2 calibrate_(const Cal3Bundler &k, const Point2 &pt)
Definition: testCal3Bundler.cpp:64
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:64
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::assert_stdout_equal
bool assert_stdout_equal(const std::string &expected, const V &actual)
Definition: TestableAssertions.h:330
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
main
int main()
Definition: testCal3Bundler.cpp:183
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
TestResult
Definition: TestResult.h:26
gtsam::Cal3::fx
double fx() const
focal length x
Definition: Cal3.h:139
example1::trueK
Cal3_S2 trueK
Definition: testEssentialMatrixFactor.cpp:42
gtsam
traits
Definition: SFMdata.h:40
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
gtsam::traits
Definition: Group.h:36
CHECK
#define CHECK(condition)
Definition: Test.h:108
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::Cal3Bundler::Dim
static size_t Dim()
Return DOF, dimensionality of tangent space.
Definition: Cal3Bundler.h:131
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:65
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
GTSAM_CONCEPT_MANIFOLD_INST
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
‍**
Definition: Manifold.h:177
gtsam::Cal3::px
double px() const
image center in x
Definition: Cal3.h:151
gtsam::Cal3Bundler::calibrate
Point2 calibrate(const Point2 &pi, OptionalJacobian< 2, 3 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3Bundler.cpp:93
Cal3Bundler.h
Calibration used by Bundler.
p
static Point2 p(2, 3)

gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:07:19