farm-ng-core
brown_conrady.h
Go to the documentation of this file.
1 // Copyright (c) 2011, Hauke Strasdat
2 // Copyright (c) 2012, Steven Lovegrove
3 // Copyright (c) 2021, farm-ng, inc.
4 //
5 // Use of this source code is governed by an MIT-style
6 // license that can be found in the LICENSE file or at
7 // https://opensource.org/licenses/MIT.
8 
9 #pragma once
10 
11 #include "sophus/ceres/jet_helpers.h"
12 #include "sophus/common/common.h"
13 #include "sophus/sensor/camera_distortion/affine.h"
14 
15 #include <Eigen/Dense>
16 
17 namespace sophus {
18 class BrownConradyTransform {
19  public:
20  static int constexpr kNumDistortionParams = 8;
21  static int constexpr kNumParams = kNumDistortionParams + 4;
22  static constexpr const std::string_view kProjectionModel =
23  "BrownConrady: fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, k5, k6";
24 
25  template <class TScalar>
26  using ProjInCameraZ1Plane = Eigen::Matrix<TScalar, 2, 1>;
27  template <class TScalar>
28  using PixelImage = Eigen::Matrix<TScalar, 2, 1>;
29  template <class TScalar>
30  using Params = Eigen::Matrix<TScalar, kNumParams, 1>;
31  template <class TScalar>
32  using DistorationParams = Eigen::Matrix<TScalar, kNumDistortionParams, 1>;
33 
34  template <class TParamScalarT, class TPointScalarT>
35  static auto projImpl(
36  DistorationParams<TParamScalarT> const& distortion,
37  PixelImage<TPointScalarT> const& point_normalized)
38  -> PixelImage<typename Eigen::ScalarBinaryOpTraits<
39  TParamScalarT,
40  TPointScalarT>::ReturnType> {
41  using ReturnScalar = typename Eigen::
42  ScalarBinaryOpTraits<TParamScalarT, TPointScalarT>::ReturnType;
43 
44  // aliases to match opencv's implementation
45  auto x = point_normalized[0];
46  auto y = point_normalized[1];
47  auto const& k = distortion;
48 
49  // From:
50  // https://github.com/opencv/opencv/blob/63bb2abadab875fc648a572faccafee134f06fc8/modules/calib3d/src/calibration.cpp#L791
51 
52  auto r2 = x * x + y * y;
53  auto r4 = r2 * r2;
54  auto r6 = r4 * r2;
55  auto a1 = 2 * x * y;
56  auto a2 = r2 + 2 * x * x;
57  auto a3 = r2 + 2 * y * y;
58  auto cdist = 1 + k[0] * r2 + k[1] * r4 + k[4] * r6;
59  auto icdist2 = 1. / (1 + k[5] * r2 + k[6] * r4 + k[7] * r6);
60  auto xd0 = x * cdist * icdist2 + k[2] * a1 + k[3] * a2;
61  auto yd0 = y * cdist * icdist2 + k[2] * a3 + k[3] * a1;
62 
63  return PixelImage<ReturnScalar>(xd0, yd0);
64  }
65 
66  template <class TScalar>
67  static auto unprojImpl(
68  DistorationParams<TScalar> const& distortion,
69  PixelImage<TScalar> const& uv_normalized) -> PixelImage<TScalar> {
70  // We had no luck with OpenCV's undistort. It seems not to be accurate if
71  // "icdist" is close to 0.
72  // https://github.com/opencv/opencv/blob/63bb2abadab875fc648a572faccafee134f06fc8/modules/calib3d/src/undistort.dispatch.cpp#L365
73  //
74  // Hence, we derive the inverse approximation scheme from scratch.
75  //
76  //
77  // Objective: find that xy such that proj_impl(xy) = uv
78  //
79  // Using multivariate Newton scheme, by defining f and find the root of it:
80  //
81  // f: R^2 -> R^2
82  // f(xy) := proj_impl(xy) - uv
83  //
84  // xy_i+1 = xy_i + J^{-1} * f(xy) with J being the Jacobian of f.
85  //
86  // TODO(hauke): There is most likely a 1-dimensional embedding and one only
87  // need to solve a less computational heavy newton iteration...
88 
89  // initial guess
90  PixelImage<TScalar> xy = uv_normalized;
91 
92  for (int i = 0; i < 50; ++i) {
93  TScalar x = xy[0];
94  TScalar y = xy[1];
95 
96  PixelImage<TScalar> f_xy =
97  projImpl(distortion, Eigen::Matrix<TScalar, 2, 1>(x, y)) -
98  uv_normalized;
99 
100  TScalar du_dx;
101  TScalar du_dy;
102  TScalar dv_dx;
103  TScalar dv_dy;
104 
105  {
106  // Generated by brown_conrady_camera.py
107  TScalar const a = x;
108  TScalar const b = y;
109  DistorationParams<TScalar> const& d = distortion;
110 
111  TScalar const c0 = a * a; // pow(a, 2);
112  TScalar const c1 = b * b; // pow(b, 2);
113  TScalar const c2 = c0 + c1;
114  TScalar const c3 = c2 * c2; // pow(c2, 2);
115  TScalar const c4 = c3 * c2; // pow(c2, 3);
116  TScalar const c5 = c2 * d[5] + c3 * d[6] + c4 * d[7] + 1.0;
117  TScalar const c6 = c5 * c5; // pow(c5, 2);
118  TScalar const c7 = 1.0 / c6;
119  TScalar const c8 = a * d[3];
120  TScalar const c9 = 2.0 * d[2];
121  TScalar const c10 = 2 * c2;
122  TScalar const c11 = 3 * c3;
123  TScalar const c12 = c2 * d[0];
124  TScalar const c13 = c3 * d[1];
125  TScalar const c14 = c4 * d[4];
126  TScalar const c15 =
127  2.0 * (c10 * d[6] + c11 * d[7] + d[5]) * (c12 + c13 + c14 + 1.0);
128  TScalar const c16 = 2.0 * c10 * d[1] + 2.0 * c11 * d[4] + 2.0 * d[0];
129  TScalar const c17 = 1.0 * c12 + 1.0 * c13 + 1.0 * c14 + 1.0;
130  TScalar const c18 = b * d[3];
131  TScalar const c19 = a * b;
132  TScalar const c20 = -c15 * c19 + c16 * c19 * c5;
133  du_dx =
134  c7 * (-c0 * c15 + c5 * (c0 * c16 + c17) + c6 * (b * c9 + 6.0 * c8));
135  du_dy = c7 * (c20 + c6 * (a * c9 + 2 * c18));
136  dv_dx = c7 * (c20 + c6 * (2 * a * d[2] + 2.0 * c18));
137  dv_dy = c7 * (-c1 * c15 + c5 * (c1 * c16 + c17) +
138  c6 * (6.0 * b * d[2] + 2.0 * c8));
139  }
140 
141  // | du_dx du_dy | | a b |
142  // J = | | =: | |
143  // | dv_dx dv_dy | | c d |
144 
145  TScalar a = du_dx;
146  TScalar b = du_dy;
147  TScalar c = dv_dx;
148  TScalar d = dv_dy;
149 
150  // | a b | -1 1 | d -b |
151  // | | = ----- | |
152  // | c d | ad-bc | -c a |
153 
154  Eigen::Matrix<TScalar, 2, 2> m;
155  // clang-format off
156  m << d, -b,
157  -c, a;
158  // clang-format on
159 
160  Eigen::Matrix<TScalar, 2, 2> j_inv = TScalar(1) / (a * d - b * c) * m;
161  PixelImage<TScalar> step = j_inv * f_xy;
162 
163  if (abs(jet_helpers::GetValue<TScalar>::impl(step.squaredNorm())) <
164  sophus::kEpsilon<TScalar> * sophus::kEpsilon<TScalar>) {
165  break;
166  }
167  xy -= step;
168  }
169 
170  return xy;
171  }
172 
173  template <class TParamsTypeT, class TPointTypeT>
174  static auto distort(
175  Eigen::MatrixBase<TParamsTypeT> const& params,
176  Eigen::MatrixBase<TPointTypeT> const& proj_point_in_camera_z1_plane)
177  -> PixelImage<typename TPointTypeT::Scalar> {
178  using ParamScalar = typename TParamsTypeT::Scalar;
179 
180  static_assert(
181  TParamsTypeT::ColsAtCompileTime == 1, "params must be a column-vector");
182  static_assert(
183  TParamsTypeT::RowsAtCompileTime == kNumParams,
184  "params must have exactly kNumParams rows");
185  static_assert(
186  TPointTypeT::ColsAtCompileTime == 1,
187  "point_camera must be a column-vector");
188  static_assert(
189  TPointTypeT::RowsAtCompileTime == 2,
190  "point_camera must have exactly 2 columns");
191 
192  Eigen::Matrix<ParamScalar, kNumDistortionParams, 1> distortion =
193  params.template tail<kNumDistortionParams>();
194 
195  PixelImage<typename TPointTypeT::Scalar>
196  distorted_point_in_camera_z1_plane =
197  projImpl(distortion, proj_point_in_camera_z1_plane.eval());
198 
199  return AffineTransform::distort(
200  params.template head<4>(), distorted_point_in_camera_z1_plane);
201  }
202 
203  template <class TScalar>
204  static auto undistort(
205  Params<TScalar> const& params, PixelImage<TScalar> const& pixel_image)
206  -> ProjInCameraZ1Plane<TScalar> {
207  PixelImage<TScalar> proj_point_in_camera_z1_plane = unprojImpl(
208  params.template tail<kNumDistortionParams>().eval(),
209  AffineTransform::undistort(
210  params.template head<4>().eval(), pixel_image));
211 
212  return ProjInCameraZ1Plane<TScalar>(
213  proj_point_in_camera_z1_plane[0], proj_point_in_camera_z1_plane[1]);
214  }
215 
216  template <class TParamsTypeT, class TPointTypeT>
217  static auto dxDistort(
218  Eigen::MatrixBase<TParamsTypeT> const& params,
219  Eigen::MatrixBase<TPointTypeT> const& proj_point_in_camera_z1_plane)
220  -> Eigen::Matrix<typename TPointTypeT::Scalar, 2, 2> {
221  static_assert(
222  TParamsTypeT::ColsAtCompileTime == 1, "params must be a column-vector");
223  static_assert(
224  TParamsTypeT::RowsAtCompileTime == kNumParams,
225  "params must have exactly kNumParams rows");
226  static_assert(
227  TPointTypeT::ColsAtCompileTime == 1,
228  "point_camera must be a column-vector");
229  static_assert(
230  TPointTypeT::RowsAtCompileTime == 2,
231  "point_camera must have exactly 2 columns");
232  using Scalar = typename TPointTypeT::Scalar;
233 
234  Eigen::Matrix<Scalar, kNumDistortionParams, 1> d =
235  params.template tail<kNumDistortionParams>();
236 
237  Scalar a = proj_point_in_camera_z1_plane[0];
238  Scalar b = proj_point_in_camera_z1_plane[1];
239 
240  Eigen::Matrix<typename TPointTypeT::Scalar, 2, 2> dx;
241  Scalar const fx = params[0];
242  Scalar const fy = params[1];
243 
244  // Generated by brown_conrady_camera.py
245  Scalar const c0 = a * d[3];
246  Scalar const c1 = 2.0 * d[2];
247  Scalar const c2 = a * a; // pow(a, 2);
248  Scalar const c3 = b * b; // pow(b, 2);
249  Scalar const c4 = c2 + c3;
250  Scalar const c5 = c4 * c4; // pow(c4, 2);
251  Scalar const c6 = c5 * c4; // pow(c4, 3);
252  Scalar const c7 = c4 * d[5] + c5 * d[6] + c6 * d[7] + 1.0;
253  Scalar const c8 = c7 * c7; // pow(c7, 2);
254  Scalar const c9 = 2 * c4;
255  Scalar const c10 = 3 * c5;
256  Scalar const c11 = c4 * d[0];
257  Scalar const c12 = c5 * d[1];
258  Scalar const c13 = c6 * d[4];
259  Scalar const c14 =
260  2.0 * (c10 * d[7] + c9 * d[6] + d[5]) * (c11 + c12 + c13 + 1.0);
261  Scalar const c15 = 2.0 * c10 * d[4] + 2.0 * c9 * d[1] + 2.0 * d[0];
262  Scalar const c16 = 1.0 * c11 + 1.0 * c12 + 1.0 * c13 + 1.0;
263  Scalar const c17 = 1.0 / c8;
264  Scalar const c18 = c17 * fx;
265  Scalar const c19 = b * d[3];
266  Scalar const c20 = a * b;
267  Scalar const c21 = -c14 * c20 + c15 * c20 * c7;
268  Scalar const c22 = c17 * fy;
269  dx(0, 0) =
270  c18 * (-c14 * c2 + c7 * (c15 * c2 + c16) + c8 * (b * c1 + 6.0 * c0));
271  dx(0, 1) = c22 * (c21 + c8 * (2 * a * d[2] + 2.0 * c19));
272  dx(1, 0) = c18 * (c21 + c8 * (a * c1 + 2 * c19));
273  dx(1, 1) = c22 * (-c14 * c3 + c7 * (c15 * c3 + c16) +
274  c8 * (6.0 * b * d[2] + 2.0 * c0));
275 
276  return dx;
277  }
278 };
279 
280 } // namespace sophus
sophus::BrownConradyTransform::Params
Eigen::Matrix< TScalar, kNumParams, 1 > Params
Definition: brown_conrady.h:30
affine.h
sophus::BrownConradyTransform::ProjInCameraZ1Plane
Eigen::Matrix< TScalar, 2, 1 > ProjInCameraZ1Plane
Definition: brown_conrady.h:26
sophus::BrownConradyTransform::DistorationParams
Eigen::Matrix< TScalar, kNumDistortionParams, 1 > DistorationParams
Definition: brown_conrady.h:32
sophus::BrownConradyTransform::PixelImage
Eigen::Matrix< TScalar, 2, 1 > PixelImage
Definition: brown_conrady.h:28
sophus::BrownConradyTransform::kNumDistortionParams
static constexpr int kNumDistortionParams
Definition: brown_conrady.h:20
sophus::BrownConradyTransform::unprojImpl
static auto unprojImpl(DistorationParams< TScalar > const &distortion, PixelImage< TScalar > const &uv_normalized) -> PixelImage< TScalar >
Definition: brown_conrady.h:67
sophus
Image MutImage, owning images types.
Definition: num_diff.h:20
sophus::jet_helpers::GetValue
Definition: jet_helpers.h:23
sophus::BrownConradyTransform::kNumParams
static constexpr int kNumParams
Definition: brown_conrady.h:21
sophus::BrownConradyTransform::projImpl
static auto projImpl(DistorationParams< TParamScalarT > const &distortion, PixelImage< TPointScalarT > const &point_normalized) -> PixelImage< typename Eigen::ScalarBinaryOpTraits< TParamScalarT, TPointScalarT >::ReturnType >
Definition: brown_conrady.h:35
sophus::BrownConradyTransform::undistort
static auto undistort(Params< TScalar > const &params, PixelImage< TScalar > const &pixel_image) -> ProjInCameraZ1Plane< TScalar >
Definition: brown_conrady.h:204
sophus::BrownConradyTransform::distort
static auto distort(Eigen::MatrixBase< TParamsTypeT > const &params, Eigen::MatrixBase< TPointTypeT > const &proj_point_in_camera_z1_plane) -> PixelImage< typename TPointTypeT::Scalar >
Definition: brown_conrady.h:174
sophus::BrownConradyTransform::dxDistort
static auto dxDistort(Eigen::MatrixBase< TParamsTypeT > const &params, Eigen::MatrixBase< TPointTypeT > const &proj_point_in_camera_z1_plane) -> Eigen::Matrix< typename TPointTypeT::Scalar, 2, 2 >
Definition: brown_conrady.h:217
common.h
sophus::BrownConradyTransform
Definition: brown_conrady.h:18
sophus::BrownConradyTransform::kProjectionModel
static constexpr const std::string_view kProjectionModel
Definition: brown_conrady.h:22
sophus::AffineTransform::undistort
static auto undistort(Params< TScalar > const &params, PixelImage< TScalar > const &pixel_image) -> ProjInCameraZ1Plane< TScalar >
Definition: affine.h:54
jet_helpers.h
sophus::AffineTransform::distort
static auto distort(Eigen::MatrixBase< TParamsTypeT > const &params, Eigen::MatrixBase< TPointTypeT > const &proj_point_in_camera_z1_plane) -> PixelImage< typename TPointTypeT::Scalar >
Definition: affine.h:32