ldpk
ldpk_radial_decentered_distortion_opencv.h
Go to the documentation of this file.
1 #pragma once
2 
6 
8 #include <iostream>
9 
10 namespace ldpk
11  {
14  template <class VEC2,class MAT2>
16  {
17  public:
19  typedef VEC2 vec2_type;
20  typedef MAT2 mat2_type;
21  private:
22 // union allows to access coefficients by index.
23 // Original OpenCV coefficients for OpenCV-coordinates.
24  union
25  {
26  struct
27  {
28 // p2 in OpenCV corresponds to our u2,
29 // p1 in OpenCV corresponds to our v2.
30 // This is the typical order of coefficients in OpenCV
31  double _k1;
32  double _k2;
33  double _p1;
34  double _p2;
35  double _k3;
36  };
37  double _c_ocv[5];
38  };
39 // Coefficients in SDV-coordinates.
40  union
41  {
42  struct
43  {
44 // We reproduce the order of coefficients from OpenCV in our notation.
45  double _c2;
46  double _c4;
47  double _v2;
48  double _u2;
49  double _c6;
50  };
51  double _c[5];
52  };
53 // OpenCV uses different coordinates than we do. Internally, we will use
54 // our own coordinate system, but get_coeff() and set_coeff() will
55 // work with OpenCV-coordinates. For each of the five coefficients
56 // there is a factor. In reparametrization we call this rho, and this here
57 // is quite similar.
58  union
59  {
60  struct
61  {
62  double _rho_pow_c2,_rho_pow_u2,_rho_pow_v2;
63  double _rho_pow_c4;
64  double _rho_pow_c6;
65  };
66  double _rho_pow[5];
67  };
68  double _rho;
69  public:
71  {
72  _c2 = _u2 = _v2 = 0.0;
73  _c4 = 0.0;
74  _c6 = 0.0;
75  _rho_pow_c2 = 1.0;
76  _rho_pow_u2 = 1.0;
77  _rho_pow_v2 = 1.0;
78  _rho_pow_c4 = 1.0;
79  _rho_pow_c6 = 1.0;
80  _rho = 1.0;
81  }
82  void set_rho(double rho)
83  {
84  _rho = rho;
85 // For c2
86  _rho_pow[0] = std::pow(rho,2.0);
87 // For c4
88  _rho_pow[1] = std::pow(rho,4.0);
89 // For v2 and u2
90  _rho_pow[2] = std::pow(rho,1.0);
91  _rho_pow[3] = std::pow(rho,1.0);
92 // For c6
93  _rho_pow[4] = std::pow(rho,6.0);
94 
95  for(int i = 0;i < 5;++i)
96  { _c[i] = _c_ocv[i] * _rho_pow[i]; }
97  }
99  double get_coeff(int i) const
100  {
102  return _c_ocv[i];
103  }
105  void set_coeff(int i,double q)
106  {
108  _c_ocv[i] = q;
109  }
112  vec2_type operator()(const vec2_type& p_dn) const
113  {
114  double x_dn,y_dn;
115  double x = p_dn[0];
116  double y = p_dn[1];
117  double x2 = x * x;
118  double y2 = y * y;
119  double xy = x * y;
120  double r2 = x2 + y2;
121  double r4 = r2 * r2;
122  double r6 = r2 * r4;
123 // OpenCV has Y-down-coordinates
124  x_dn = x * (1.0 + _c2 * r2 + _c4 * r4 + _c6 * r6)
125  + (r2 + 2.0 * x2) * (_u2)
126  + 2.0 * xy * (-_v2);
127 
128  y_dn = y * (1.0 + _c2 * r2 + _c4 * r4 + _c6 * r6)
129  + (r2 + 2.0 * y2) * (-_v2)
130  + 2.0 * xy * (_u2);
131  return vec2_type(x_dn,y_dn);
132  }
135  mat2_type jacobi(const vec2_type& p_dn) const
136  {
137  mat2_type m;
138  double x = p_dn[0];
139  double y = p_dn[1];
140  double x2 = x * x;
141  double y2 = y * y;
142  double x3 = x2 * x;
143  double y3 = y2 * y;
144  double xy = x * y;
145  double x2y = xy * x;
146  double xy2 = xy * y;
147  double r2 = x2 + y2;
148  double r4 = r2 * r2;
149 
150 // long implementation for testing.
151 // m[0][0] = 1 + _c2 * (3.0 * x2 + y2) + _c4 * (5.0 * x2 + y2) * r2 + _c6 * (7.0 * x2 + y2) * r4
152 // + 6.0 * _u2 * x
153 // + 2.0 * (-_v2) * y;
154 // m[1][1] = 1 + _c2 * (x2 + 3.0 * y2) + _c4 * (x2 + 5.0 * y2) * r2 + _c6 * (x2 + 7.0 * y2) * r4
155 // + 6.0 * (-_v2) * y
156 // + 2.0 * _u2 * x;
157 //
158 // m[0][1] = 2.0 * _c2 * xy + 4.0 * _c4 * xy * r2 + _c6 * xy * r4
159 // + 2.0 * _u2 * y
160 // + 2.0 * (-_v2) * x;
161 // m[1][0] = 2.0 * _c2 * xy + 4.0 * _c4 * xy * r2 + _c6 * xy * r4
162 // + 2.0 * _u2 * y
163 // + 2.0 * (-_v2) * x;
164 // slightly condensed implementation
165  double u2x = _u2 * x;
166  double v2y = (-_v2) * y;
167  double c4r2 = _c4 * r2;
168  double c6r4 = _c6 * r4;
169  m[0][0] = 1.0 + _c2 * (3.0 * x2 + y2) + c4r2 * (5.0 * x2 + y2) + c6r4 * (7.0 * x2 + y2)
170  + 6.0 * u2x
171  + 2.0 * v2y;
172  m[1][1] = 1.0 + _c2 * (x2 + 3.0 * y2) + c4r2 * (x2 + 5.0 * y2) + c6r4 * (x2 + 7.0 * y2)
173  + 6.0 * v2y
174  + 2.0 * u2x;
175 
176  double m_off_diag_common = 2.0 * _c2 * xy + 4.0 * _c4 * xy * r2 + 6.0 * _c6 * xy * r4
177  + 2.0 * _u2 * y + 2.0 * (-_v2) * x;
178  m[0][1] = m_off_diag_common;
179  m[1][0] = m_off_diag_common;
180 
181  return m;
182  }
186  void derive(double* dg,int n_parameters,const vec2_type& p_dn) const
187  {
188  int size = 2 * n_parameters;
189  double x = p_dn[0];
190  double y = p_dn[1];
191  double x2 = p_dn[0] * p_dn[0];
192  double y2 = p_dn[1] * p_dn[1];
193  double xy = p_dn[0] * p_dn[1];
194  double r2 = x2 + y2;
195  double r4 = r2 * r2;
196  double r6 = r2 * r4;
197 // OpenCV order. Since we have defined c_sdv = c_ocv * rho**j,
198 // the derivatives d/dc_ocv are d/dc_sdv * rho**j.
199  int k = 0;
200 // c2 (= k1)
201  dg[k++] = _rho_pow[0] * (x * r2);
202  dg[k++] = _rho_pow[0] * (y * r2);
203  if(k == size) return;
204 // c4 (= k2)
205  dg[k++] = _rho_pow[1] * (x * r4);
206  dg[k++] = _rho_pow[1] * (y * r4);
207  if(k == size) return;
208 // v2 (= p1), mind the sign due to OpenCV's Y-down-coordinates.
209  dg[k++] = -_rho_pow[2] * (2.0 * xy);
210  dg[k++] = -_rho_pow[2] * (r2 + 2.0 * y2);
211  if(k == size) return;
212 // u2 (= p2)
213  dg[k++] = _rho_pow[3] * (r2 + 2.0 * x2);
214  dg[k++] = _rho_pow[3] * (2.0 * xy);
215  if(k == size) return;
216 // c6 (= k3)
217  dg[k++] = _rho_pow[4] * (x * r6);
218  dg[k++] = _rho_pow[4] * (y * r6);
219  if(k == size) return;
220 // Unreachable
221  std::cerr << "radial_decentered_distortion_opencv: n_parameters out of range" << std::endl;
222  }
223  std::ostream& out(std::ostream& cout) const
224  {
225  int p = int(cout.precision());
226  cout.precision(5);
227  cout << "c2: " << _c2 << std::endl;
228  cout << "c4: " << _c4 << std::endl;
229  cout << "v2: " << _v2 << std::endl;
230  cout << "u2: " << _u2 << std::endl;
231  cout << "c6: " << _c6 << std::endl;
232  cout.precision(p);
233  return cout;
234  }
235  };
236  }
237 
mat2_type jacobi(const vec2_type &p_dn) const
Analytic version of the Jacobi-matrix. By definition, we are working in dn-coordinates here...
Definition: ldpk_radial_decentered_distortion_opencv.h:135
Base class for a distortion model with N parameters. You may find it useful to derive your own distor...
Definition: ldpk_generic_distortion_base.h:20
double get_coeff(int i) const
Get coefficient c[i], 0 <= i < 5.
Definition: ldpk_radial_decentered_distortion_opencv.h:99
void check_range(int i) const
A derived class may check if the index is valid.
Definition: ldpk_generic_distortion_base.h:36
std::ostream & out(std::ostream &cout) const
The derived class implements a method for printing values inside 3DE4&#39;s matrix tool dialog...
Definition: ldpk_radial_decentered_distortion_opencv.h:223
void derive(double *dg, int n_parameters, const vec2_type &p_dn) const
Derivative wrt distortion coefficients. dg points to an array with N / 2 Elements Not tested! ...
Definition: ldpk_radial_decentered_distortion_opencv.h:186
The namespace of (most of the) things related to the Lens Distortion Plugin Kit.
Definition: ldpk.h:19
Base class for distortion models.
vec2_type operator()(const vec2_type &p_dn) const
Remove distortion. p_dn is a point in diagonally normalized coordinates. We compute in dn-coordinates...
Definition: ldpk_radial_decentered_distortion_opencv.h:112
A polynomial radially symmetric model of degree 6 with decentering degree 2 as defined in OpenCV...
Definition: ldpk_radial_decentered_distortion_opencv.h:15
void set_coeff(int i, double q)
Set coefficient c[i], 0 <= i < 6.
Definition: ldpk_radial_decentered_distortion_opencv.h:105