testCal3DFisheye.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>
22 #include <gtsam/geometry/Point3.h>
23 
25 
26 using namespace gtsam;
27 
30 
31 static const double fx = 250, fy = 260, s = 0.1, u0 = 320, v0 = 240;
32 static Cal3Fisheye K(fx, fy, s, u0, v0, -0.013721808247486035,
33  0.020727425669427896, -0.012786476702685545,
34  0.0025242267320687625);
35 static Point2 kTestPoint2(2, 3);
36 
37 /* ************************************************************************* */
39 
40 /* ************************************************************************* */
41 TEST(Cal3Fisheye, retract) {
42  Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
43  K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
44  K.k4() + 9);
45 
47  EXPECT_LONGS_EQUAL(expected.dim(), 9);
48 
49  Vector9 d;
50  d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
51  Cal3Fisheye actual = K.retract(d);
52  CHECK(assert_equal(expected, actual, 1e-7));
53  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
54 }
55 
56 /* ************************************************************************* */
57 TEST(Cal3Fisheye, uncalibrate1) {
58  // Calculate the solution
59  const double xi = kTestPoint2.x(), yi = kTestPoint2.y();
60  const double r = sqrt(xi * xi + yi * yi);
61  const double t = atan(r);
62  const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
63  const double td =
64  t * (1 + K.k1() * tt + K.k2() * t4 + K.k3() * t6 + K.k4() * t8);
65  Vector3 pd(td / r * xi, td / r * yi, 1);
66  Vector3 v = K.K() * pd;
67 
68  Point2 uv_sol(v[0] / v[2], v[1] / v[2]);
69 
71  CHECK(assert_equal(uv, uv_sol));
72 }
73 
74 /* ************************************************************************* */
75 // For numerical derivatives
76 Point2 f(const Cal3Fisheye& k, const Point2& pt) { return k.uncalibrate(pt); }
77 
78 /* ************************************************************************* */
79 TEST(Cal3Fisheye, Derivatives) {
80  Matrix H1, H2;
81  K.uncalibrate(kTestPoint2, H1, H2);
84 }
85 
86 /* ************************************************************************* */
87 // Check that a point at (0,0) projects to the image center.
88 TEST(Cal3Fisheye, uncalibrate2) {
89  Point2 pz(0, 0);
90  Matrix H1, H2;
91  auto uv = K.uncalibrate(pz, H1, H2);
92  CHECK(assert_equal(uv, Point2(u0, v0)));
93  CHECK(assert_equal(numericalDerivative21(f, K, pz, 1e-7), H1, 1e-5));
94  // TODO(frank): the second jacobian is all NaN for the image center!
95  // CHECK(assert_equal(numericalDerivative22(f, K, pz, 1e-7), H2, 1e-5));
96 }
97 
98 /* ************************************************************************* */
99 // This test uses cv2::fisheye::projectPoints to test that uncalibrate
100 // properly projects a point into the image plane. One notable difference
101 // between opencv and the Cal3Fisheye::uncalibrate function is the skew
102 // parameter. The equivalence is alpha = s/fx.
103 //
104 // Python script to project points with fisheye model in OpenCv
105 // (script run with OpenCv version 4.2.0 and Numpy version 1.18.2)
106 // clang-format off
107 /*
108 ===========================================================
109 
110 import numpy as np
111 import cv2
112 
113 objpts = np.float64([[23,27,31]]).reshape(1,-1,3)
114 
115 cameraMatrix = np.float64([
116  [250, 0, 320],
117  [0, 260, 240],
118  [0,0,1]
119 ])
120 alpha = 0.1/250
121 distCoeffs = np.float64([-0.013721808247486035, 0.020727425669427896,-0.012786476702685545, 0.0025242267320687625])
122 
123 rvec = np.float64([[0.,0.,0.]])
124 tvec = np.float64([[0.,0.,0.]]);
125 imagePoints, jacobian = cv2.fisheye.projectPoints(objpts, rvec, tvec, cameraMatrix, distCoeffs, alpha=alpha)
126 np.set_printoptions(precision=14)
127 print(imagePoints)
128 
129 ===========================================================
130  * Script output: [[[457.82638130304935 408.18905848512986]]]
131  */
132 // clang-format on
133 TEST(Cal3Fisheye, uncalibrate3) {
134  Point3 p3(23, 27, 31);
135  Point2 xi(p3.x() / p3.z(), p3.y() / p3.z());
136  auto uv = K.uncalibrate(xi);
137  CHECK(assert_equal(uv, Point2(457.82638130304935, 408.18905848512986)));
138 }
139 
140 /* ************************************************************************* */
141 TEST(Cal3Fisheye, calibrate1) {
142  Point2 pi;
143  Point2 uv;
144  Point2 pi_hat;
145 
146  pi = Point2(0.5, 0.5); // point in intrinsic coordinates
147  uv = K.uncalibrate(pi); // map intrinsic coord to image plane (pi)
148  pi_hat = K.calibrate(uv); // map image coords (pi) back to intrinsic coords
149  CHECK(traits<Point2>::Equals(pi, pi_hat,
150  1e-5)); // check that the inv mapping works
151 
152  pi = Point2(-0.7, -1.2);
153  uv = K.uncalibrate(pi);
154  pi_hat = K.calibrate(uv);
155  CHECK(traits<Point2>::Equals(pi, pi_hat, 1e-5));
156 
157  pi = Point2(-3, 5);
158  uv = K.uncalibrate(pi);
159  pi_hat = K.calibrate(uv);
160  CHECK(traits<Point2>::Equals(pi, pi_hat, 1e-5));
161 
162  pi = Point2(7, -12);
163  uv = K.uncalibrate(pi);
164  pi_hat = K.calibrate(uv);
165  CHECK(traits<Point2>::Equals(pi, pi_hat, 1e-5));
166 }
167 
168 /* ************************************************************************* */
169 // Check that calibrate returns (0,0) for the image center
170 TEST(Cal3Fisheye, calibrate2) {
171  Point2 uv(u0, v0);
172  auto xi_hat = K.calibrate(uv);
173  CHECK(assert_equal(xi_hat, Point2(0, 0)))
174 }
175 
176 /* ************************************************************************* */
177 // Run calibrate on OpenCv test from uncalibrate3
178 // (script shown above)
179 // 3d point: (23, 27, 31)
180 // 2d point in image plane: (457.82638130304935, 408.18905848512986)
181 TEST(Cal3Fisheye, calibrate3) {
182  Point3 p3(23, 27, 31);
183  Point2 xi(p3.x() / p3.z(), p3.y() / p3.z());
184  Point2 uv(457.82638130304935, 408.18905848512986);
185  auto xi_hat = K.calibrate(uv);
186  CHECK(assert_equal(xi_hat, xi));
187 }
188 
189 Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) {
190  return k.calibrate(pt);
191 }
192 
193 /* ************************************************************************* */
194 TEST(Cal3Fisheye, Dcalibrate) {
195  Point2 p(0.5, 0.5);
196  Point2 pi = K.uncalibrate(p);
197  Matrix Dcal, Dp;
198  K.calibrate(pi, Dcal, Dp);
199  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
200  CHECK(assert_equal(numerical1, Dcal, 1e-5));
201  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
202  CHECK(assert_equal(numerical2, Dp, 1e-5));
203 }
204 
205 /* ************************************************************************* */
207  Cal3Fisheye cal(1, 2, 3, 4, 5, 6, 7, 8, 9);
208  std::stringstream os;
209  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
210  << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
211  << ", k2: " << cal.k2() << ", k3: " << cal.k3() << ", k4: " << cal.k4();
212 
213  EXPECT(assert_stdout_equal(os.str(), cal));
214 }
215 
216 /* ************************************************************************* */
217 int main() {
218  TestResult tr;
219  return TestRegistry::runAllTests(tr);
220 }
221 /* ************************************************************************* */
bool assert_stdout_equal(const std::string &expected, const V &actual)
Provides additional testing facilities for common data structures.
static const double fx
#define CHECK(condition)
Definition: Test.h:108
Point2 f(const Cal3Fisheye &k, const Point2 &pt)
static Point3 p3
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
Matrix expected
Definition: testMatrix.cpp:971
Vector2 Point2
Definition: Point2.h:32
static size_t Dim()
Return dimensions of calibration manifold object.
Definition: Cal3Fisheye.h:160
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
double py() const
image center in y
Definition: Cal3.h:154
Some functions to compute numerical derivatives.
Vector localCoordinates(const Cal3Fisheye &T2) const
Given a different calibration, calculate update to obtain it.
Definition: Cal3Fisheye.h:168
static const Point3 pt(1.0, 2.0, 3.0)
static Cal3Fisheye K(fx, fy, s, u0, v0, -0.013721808247486035, 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625)
double k3() const
First tangential distortion coefficient.
Definition: Cal3Fisheye.h:102
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_DEVICE_FUNC const AtanReturnType atan() const
static const double fy
#define EXPECT(condition)
Definition: Test.h:150
double fx() const
focal length x
Definition: Cal3.h:139
Array< int, Dynamic, 1 > v
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double s
double k2() const
Second distortion coefficient.
Definition: Cal3Fisheye.h:99
static const double u0
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
convert intrinsic coordinates [x_i; y_i] to (distorted) image coordinates [u; v]
Definition: Cal3Fisheye.cpp:47
Point2 calibrate_(const Cal3Fisheye &k, const Point2 &pt)
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
Vector xi
Definition: testPose2.cpp:148
traits
Definition: chartTesting.h:28
double k4() const
Second tangential distortion coefficient.
Definition: Cal3Fisheye.h:105
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
Definition: Manifold.h:177
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
ofstream os("timeSchurFactors.csv")
int main()
double k1() const
First distortion coefficient.
Definition: Cal3Fisheye.h:96
static const double v0
3D Point
float * p
static Point2 kTestPoint2(2, 3)
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Calibration of a fisheye camera.
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Vector3 Point3
Definition: Point3.h:38
TEST(SmartFactorBase, Pinhole)
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:62
Cal3Fisheye retract(const Vector &d) const
Given delta vector, update calibration.
Definition: Cal3Fisheye.h:163
Calibration of a fisheye camera.
Definition: Cal3Fisheye.h:51
double fy() const
focal length y
Definition: Cal3.h:142
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
Point2 t(10, 10)
double px() const
image center in x
Definition: Cal3.h:151


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