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 /* ************************************************************************* */
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 /* ************************************************************************* */
gtsam::Cal3::fy
double fy() const
focal length y
Definition: Cal3.h:142
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.cal
cal
Definition: test_ProjectionFactorRollingShutter.py:27
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
atan
const EIGEN_DEVICE_FUNC AtanReturnType atan() const
Definition: ArrayCwiseUnaryOps.h:283
d
static const double d[K][N]
Definition: igam.h:11
v0
static const double v0
Definition: testCal3DFisheye.cpp:31
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
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
gtsam::Cal3Fisheye::calibrate
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3Fisheye.cpp:111
fy
static const double fy
Definition: testCal3DFisheye.cpp:31
gtsam::Cal3::py
double py() const
image center in y
Definition: Cal3.h:154
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::utils.numerical_derivative.retract
def retract(a, np.ndarray xi)
Definition: numerical_derivative.py:44
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Cal3Fisheye::k1
double k1() const
First distortion coefficient.
Definition: Cal3Fisheye.h:96
Point3.h
3D Point
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
os
ofstream os("timeSchurFactors.csv")
f
Point2 f(const Cal3Fisheye &k, const Point2 &pt)
Definition: testCal3DFisheye.cpp:76
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::Cal3Fisheye::k4
double k4() const
Second tangential distortion coefficient.
Definition: Cal3Fisheye.h:105
main
int main()
Definition: testCal3DFisheye.cpp:217
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
gtsam::Cal3Fisheye::retract
Cal3Fisheye retract(const Vector &d) const
Given delta vector, update calibration.
Definition: Cal3Fisheye.h:163
kTestPoint2
static Point2 kTestPoint2(2, 3)
gtsam::Cal3Fisheye::k2
double k2() const
Second distortion coefficient.
Definition: Cal3Fisheye.h:99
gtsam::Cal3Fisheye
Calibration of a fisheye camera.
Definition: Cal3Fisheye.h:51
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::Cal3Fisheye::k3
double k3() const
First tangential distortion coefficient.
Definition: Cal3Fisheye.h:102
s
static const double s
Definition: testCal3DFisheye.cpp:31
K
static Cal3Fisheye K(fx, fy, s, u0, v0, -0.013721808247486035, 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625)
simple::p3
static Point3 p3
Definition: testInitializePose3.cpp:53
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::Cal3::K
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
gtsam::Cal3Fisheye::Dim
static size_t Dim()
Return dimensions of calibration manifold object.
Definition: Cal3Fisheye.h:160
TestResult
Definition: TestResult.h:26
u0
static const double u0
Definition: testCal3DFisheye.cpp:31
calibrate_
Point2 calibrate_(const Cal3Fisheye &k, const Point2 &pt)
Definition: testCal3DFisheye.cpp:189
gtsam::Cal3::fx
double fx() const
focal length x
Definition: Cal3.h:139
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
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Cal3::skew
double skew() const
skew
Definition: Cal3.h:148
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::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::Cal3Fisheye::uncalibrate
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
gtsam::Cal3Fisheye::localCoordinates
Vector localCoordinates(const Cal3Fisheye &T2) const
Given a different calibration, calculate update to obtain it.
Definition: Cal3Fisheye.h:168
gtsam::Cal3::px
double px() const
image center in x
Definition: Cal3.h:151
align_3::t
Point2 t(10, 10)
fx
static const double fx
Definition: testCal3DFisheye.cpp:31
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Cal3Fisheye.h
Calibration of a fisheye camera.


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:46