ldpk
ldpk_ldp_builtin.h
1 #ifndef ldp_builtin_sdv
2 #define ldp_builtin_sdv
3 
4 #include <ldpk/ldpk_vec2d.h>
5 #include <ldpk/tde4_ld_plugin.h>
7 #include <algorithm>
8 #include <string.h>
9 
10 namespace ldpk
11  {
14  {
15  para_not_a_builtin = -1,
16  para_focal_length = 0,
17  para_filmback_width = 1,
18  para_filmback_height = 2,
19  para_lens_center_offset_x = 3,
20  para_lens_center_offset_y = 4,
21  para_pixel_aspect = 5,
22  para_custom_focus_distance = 6
23  };
24 
30  template <class VEC2,class MAT2>
32  {
33  private:
34  typedef VEC2 vec2_type;
35  typedef MAT2 mat2_type;
36  typedef box<VEC2> box2_type;
37 
39  static const char* _para_builtin[7];
41  double _value_builtin[7];
42 
44  double _r_fb_cm;
45 
52  bool _uptodate_lut;
53 
56  para_builtin_enum find_builtin_parameter_by_name(const char* identifier) const;
57 
63  static int compute_extremal_parameters_of_spline( const vec2_type& qa,const vec2_type& qb,
64  const vec2_type& dqa,const vec2_type& dqb,
65  double* t_out)
66  {
67  vec2_type b = dqa;
68  vec2_type c = -3.0 * qa + 3.0 * qb - 2.0 * dqa - dqb;
69  vec2_type d = 2.0 * qa - 2.0 * qb + dqa + dqb;
70  vec2_type disc;
71  int i_out = 0;
72 
73  for(int i = 0;i < 2;++i)
74  {
75  if(d[i] != 0.0)
76  {
77  double d_mul_3 = 3.0 * d[i];
78  disc[i] = (c[i] * c[i]) / (d_mul_3 * d_mul_3) - b[i] / d_mul_3;
79 // equal 0 means saddle point, i.e. not an extreme value, less than 0 means no extremum and np saddle point.
80 // We are only interested in local extrema, therefore we check against 0.0 and 1.0.
81  if(disc[i] > 0.0)
82  {
83  double t,r = sqrt(disc[i]);
84  t = -c[i] / d_mul_3 + r;
85  if((t > 0.0) && (t < 1.0))
86  { t_out[i_out++] = t; }
87  t = -c[i] / d_mul_3 - r;
88  if((t > 0.0) && (t < 1.0))
89  { t_out[i_out++] = t; }
90  }
91  }
92  else if(c[i] != 0.0)
93  {
94 // Not a cubic polynomial? Treat as quadratic.
95  double t = b[i] / (-2.0 * c[i]);
96  if((t > 0.0) && (t < 1.0))
97  { t_out[i_out++] = t; }
98  }
99  }
100  return i_out;
101  }
102  template <int I> void extend_box_by_spline( const vec2_type& pa,const vec2_type& pb,
103  const vec2_type& qa,const vec2_type& qb,
104  const mat2_type& ja,const mat2_type& jb,
105  box2_type& bbox)
106  {
107  double t_out[4];
108  vec2_type dqa = ja * (pb - pa);
109  vec2_type dqb = jb * (pb - pa);
110  int n_out = compute_extremal_parameters_of_spline(qa,qb,dqa,dqb,t_out);
111  for(int i = 0;i < n_out;++i)
112  {
113  if((t_out[i] <= 0.0) || (t_out[i] >= 1.0))
114  { std::cerr << "ldpk::ldp_builtin: extend_box_by_spline: implementation error, needs fix." << std::endl; }
115  vec2_type qt,pt;
116  pt = (1.0 - t_out[i]) * pa + t_out[i] * pb;
117  qt = apply_mapping<I>(pt);
118  bbox.extend(qt);
119  }
120  }
122  template <int I> mat2_type get_jacobian_matrix(const vec2_type& p,const vec2_type& q)
123  {
124  mat2_type j;
125  if(I)
126  {
127  getJacobianMatrix(p[0],p[1],j[0][0],j[0][1],j[1][0],j[1][1]);
128  return j;
129  }
130  else
131  {
132 // Checked with difference quotients. This is called the Inverse Function Theorem.
133  getJacobianMatrix(q[0],q[1],j[0][0],j[0][1],j[1][0],j[1][1]);
134  return invert(j);
135  }
136  }
138  template <int I> vec2_type apply_mapping(const vec2_type& p)
139  {
140  vec2_type q;
141  if(I)
142  { undistort(p[0],p[1],q[0],q[1]); }
143  else
144  { distort(p[0],p[1],q[0],q[1]); }
145  return q;
146  }
147  template <int I> void get_bounding_box( double xa_in,double ya_in,double xb_in,double yb_in,
148  double& xa_out,double& ya_out,double& xb_out,double& yb_out,
149  int nx,int ny)
150  {
151  double dx_in = xb_in - xa_in,dy_in = yb_in - ya_in;
152  box2_type bbox;
153 
154  vec2_type pa,pb;
155  vec2_type qa,qb;
156  mat2_type ja,jb;
157 // ya_in
158  pb = vec2_type(xa_in,ya_in);
159  qb = apply_mapping<I>(pb);
160  jb = get_jacobian_matrix<I>(pb,qb);
161  bbox.extend(qb);
162  for(int ix = 1;ix <= nx;++ix)
163  {
164  double x_in = xa_in + dx_in * double(ix) / nx;
165  double y_in = ya_in;
166 // original
167  pa = pb;qa = qb;ja = jb;
168  pb = vec2_type(x_in,y_in);
169  qb = apply_mapping<I>(pb);
170  jb = get_jacobian_matrix<I>(pb,qb);
171  extend_box_by_spline<I>(pa,pb,qa,qb,ja,jb,bbox);
172  bbox.extend(qb);
173  }
174 // yb_in
175  pb = vec2_type(xa_in,yb_in);
176  qb = apply_mapping<I>(pb);
177  jb = get_jacobian_matrix<I>(pb,qb);
178  bbox.extend(qb);
179  for(int ix = 1;ix <= nx;++ix)
180  {
181  double x_in = xa_in + dx_in * double(ix) / nx;
182  double y_in = yb_in;
183 // copy-paste
184  pa = pb;qa = qb;ja = jb;
185  pb = vec2_type(x_in,y_in);
186  qb = apply_mapping<I>(pb);
187  jb = get_jacobian_matrix<I>(pb,qb);
188  extend_box_by_spline<I>(pa,pb,qa,qb,ja,jb,bbox);
189  bbox.extend(qb);
190  }
191 // xa_in
192  pb = vec2_type(xa_in,ya_in);
193  qb = apply_mapping<I>(pb);
194  jb = get_jacobian_matrix<I>(pb,qb);
195  bbox.extend(qb);
196  for(int iy = 1;iy <= ny;++iy)
197  {
198  double x_in = xa_in;
199  double y_in = ya_in + dy_in * double(iy) / ny;
200 // copy-paste
201  pa = pb;qa = qb;ja = jb;
202  pb = vec2_type(x_in,y_in);
203  qb = apply_mapping<I>(pb);
204  jb = get_jacobian_matrix<I>(pb,qb);
205  extend_box_by_spline<I>(pa,pb,qa,qb,ja,jb,bbox);
206  bbox.extend(qb);
207  }
208 // xb_in
209  pb = vec2_type(xb_in,ya_in);
210  qb = apply_mapping<I>(pb);
211  jb = get_jacobian_matrix<I>(pb,qb);
212  bbox.extend(qb);
213  for(int iy = 1;iy <= ny;++iy)
214  {
215  double x_in = xb_in;
216  double y_in = ya_in + dy_in * double(iy) / ny;
217 // copy-paste
218  pa = pb;qa = qb;ja = jb;
219  pb = vec2_type(x_in,y_in);
220  qb = apply_mapping<I>(pb);
221  jb = get_jacobian_matrix<I>(pb,qb);
222  extend_box_by_spline<I>(pa,pb,qa,qb,ja,jb,bbox);
223  bbox.extend(qb);
224  }
225  xa_out = bbox.a()[0];
226  ya_out = bbox.a()[1];
227  xb_out = bbox.b()[0];
228  yb_out = bbox.b()[1];
229  }
230  protected:
232 #ifdef _WINDOWS
233  CRITICAL_SECTION _critsec;
234 #else
235  pthread_mutex_t _mutex;
236 #endif
237 
240 
241  bool is_uptodate_lut() const
244  { return _uptodate_lut; }
248  { _uptodate_lut = false; }
251  void update_lut();
254  { return _lut; }
256 
258  int get_num_builtin_parameters() const
260  { return 7; }
262  bool set_builtin_parameter_value(const char* identifier,double v);
265  bool get_builtin_parameter_type(const char* identifier,tde4_ldp_ptype& ptype) const
266  {
267  if(find_builtin_parameter_by_name(identifier) != para_not_a_builtin)
268  {
269  ptype = TDE4_LDP_DOUBLE;
270  return true;
271  }
272  return false;
273  }
279  {
280  if(w_fb_cm() == 0)
281  { std::cerr << "ldp_builtin: filmback width is 0." << std::endl; }
282  if(h_fb_cm() == 0)
283  { std::cerr << "ldp_builtin: filmback height is 0." << std::endl; }
284  if(fl_cm() == 0)
285  { std::cerr << "ldp_builtin: focal length is 0." << std::endl; }
286  if(pa() == 0)
287  { std::cerr << "ldp_builtin: pixel aspect is 0." << std::endl; }
288  _r_fb_cm = sqrt(w_fb_cm() * w_fb_cm() + h_fb_cm() * h_fb_cm()) / 2.0;
289  }
291  vec2_type map_unit_to_dn(const vec2_type& p_unit) const
292  {
293  vec2_type p_cm((p_unit[0] - 1.0/2.0) * w_fb_cm() - x_lco_cm(),(p_unit[1] - 1.0/2.0) * h_fb_cm() - y_lco_cm());
294  return p_cm / r_fb_cm();
295  }
296  vec2_type map_dn_to_unit(const vec2_type& p_dn) const
297  {
298  vec2_type p_cm(p_dn * r_fb_cm());
299  p_cm += vec2_type(w_fb_cm() / 2 + x_lco_cm(),h_fb_cm() / 2 + y_lco_cm());
300  return vec2_type(p_cm[0] / w_fb_cm(),p_cm[1] / h_fb_cm());
301  }
302  void lock()
303  {
304 #ifdef _WINDOWS
305  EnterCriticalSection(&_critsec);
306 #else
307  pthread_mutex_lock(&this->_mutex);
308 #endif
309  }
310  void unlock()
311  {
312 #ifdef _WINDOWS
313  LeaveCriticalSection(&_critsec);
314 #else
315  pthread_mutex_unlock(&this->_mutex);
316 #endif
317  }
318  public:
319  ldp_builtin():_uptodate_lut(false)
320  {
321 #ifdef _WINDOWS
322  InitializeCriticalSection(&_critsec);
323 #else
324  int r = pthread_mutex_init(&_mutex,NULL);
325  if(r)
326  { std::cerr << "ldpk::ldp_builtin::pthread_mutex_init: " << strerror(r) << std::endl; }
327 #endif
328 // We set all built-in parameters to zero. That way we can reliably find out
329 // if the application initializies properly (i.e. pixel aspect needs to be set).
330  for(int i = 0;i < get_num_builtin_parameters();++i)
331  { _value_builtin[i] = 0; }
332  }
333  virtual ~ldp_builtin()
334  {
335 #ifdef _WINDOWS
336  DeleteCriticalSection(&_critsec);
337 #else
338  int r = pthread_mutex_destroy(&_mutex);
339  if(r)
340  { std::cerr << "ldpk::ldp_builtin::pthread_mutex_destroy: " << strerror(r) << std::endl; }
341 #endif
342  }
345 
346  double w_fb_cm() const
347  { return _value_builtin[para_filmback_width]; }
348  double h_fb_cm() const
349  { return _value_builtin[para_filmback_height]; }
350  double x_lco_cm() const
351  { return _value_builtin[para_lens_center_offset_x]; }
352  double y_lco_cm() const
353  { return _value_builtin[para_lens_center_offset_y]; }
354  double fl_cm() const
355  { return _value_builtin[para_focal_length]; }
356  double pa() const
357  { return _value_builtin[para_pixel_aspect]; }
358  double fd_cm() const
359  { return _value_builtin[para_custom_focus_distance]; }
360 
361  double r_fb_cm() const
362  { return _r_fb_cm; }
364  virtual void getBoundingBoxUndistort(double xa_in,double ya_in,double xb_in,double yb_in,double& xa_out,double& ya_out,double& xb_out,double& yb_out,int nx,int ny)
365  { get_bounding_box<1>(xa_in,ya_in,xb_in,yb_in,xa_out,ya_out,xb_out,yb_out,nx,ny); }
366  virtual void getBoundingBoxDistort(double xa_in,double ya_in,double xb_in,double yb_in,double& xa_out,double& ya_out,double& xb_out,double& yb_out,int nx,int ny)
367  { get_bounding_box<0>(xa_in,ya_in,xb_in,yb_in,xa_out,ya_out,xb_out,yb_out,nx,ny); }
368  };
369 
370 // Built-in parameter names. Do not change.
371  template <class VEC2,class MAT2>
372  const char* ldp_builtin<VEC2,MAT2>::_para_builtin[7] = {
373  "tde4_focal_length_cm",
374  "tde4_filmback_width_cm",
375  "tde4_filmback_height_cm",
376  "tde4_lens_center_offset_x_cm",
377  "tde4_lens_center_offset_y_cm",
378  "tde4_pixel_aspect",
379  "tde4_custom_focus_distance_cm"
380  };
381  template <class VEC2,class MAT2>
383  {
384  for(int i = 0;i < get_num_builtin_parameters();++i)
385  {
386  if(strcmp(_para_builtin[i],identifier) == 0)
387  {
388  return para_builtin_enum(i);
389  }
390  }
391  return para_not_a_builtin;
392  }
393  template <class VEC2,class MAT2>
394  bool ldp_builtin<VEC2,MAT2>::set_builtin_parameter_value(const char* identifier,double v)
395  {
396  int i = find_builtin_parameter_by_name(identifier);
397  if(i < 0)
398  { return false; }
399  if(_value_builtin[i] != v)
400  {
401 // Whenever a parameter is changed, the lookup table becomes obsolete.
403  _value_builtin[i] = v;
404  }
405  return true;
406  }
407  template <class VEC2,class MAT2>
409  {
410 // _lut.init(21,15);
411  _lut.init(45,45);
412  _lut.reset();
413  while(_lut.next())
414  {
415  vec2_type qs = _lut.get_current_initial_value();
416  vec2_type q,p = _lut.get_p_current_fov();
417 // Call the inverse distortion function with initial values
418  distort(p[0],p[1],qs[0],qs[1],q[0],q[1]);
419  _lut.set_q_current_fov(q);
420  }
421  _uptodate_lut = true;
422  }
423  }
424 
425 #endif
virtual bool undistort(double x0, double y0, double &x1, double &y1)=0
warp/unwarp 2D points...
virtual void getBoundingBoxDistort(double xa_in, double ya_in, double xb_in, double yb_in, double &xa_out, double &ya_out, double &xb_out, double &yb_out, int nx, int ny)
Iterate around the specified box, distort the points and compute the bounding box.
Definition: ldpk_ldp_builtin.h:366
bool next()
False means no value to be generated, loop is done. See example.
Definition: ldpk_lookup_table.h:203
void update_lut()
Update the lookup table. The derived class will do this, if necessary, when distort() without initial...
Definition: ldpk_ldp_builtin.h:408
void no_longer_uptodate_lut()
This class and the derived class mark the lookup table as obsolete when some parameter was changed...
Definition: ldpk_ldp_builtin.h:247
int get_num_builtin_parameters() const
There are seven built-in parameters.
Definition: ldpk_ldp_builtin.h:259
virtual void getBoundingBoxUndistort(double xa_in, double ya_in, double xb_in, double yb_in, double &xa_out, double &ya_out, double &xb_out, double &yb_out, int nx, int ny)
Iterate around the specified box, undistort the points and compute the bounding box.
Definition: ldpk_ldp_builtin.h:364
CRITICAL_SECTION _critsec
The Mutex, used in derived classes, initialized in constructor.
Definition: ldpk_ldp_builtin.h:233
The namespace of (most of the) things related to the Lens Distortion Plugin Kit.
Definition: ldpk.h:180
A simple box class for double precision points in 2d. We will extend this as needed.
Definition: ldpk_vec2d.h:192
Lens Distortion Plugin Base Class.
Definition: tde4_ld_plugin.h:29
void reset()
Use reset() and next() to build a loop for generating positions. See example.
Definition: ldpk_lookup_table.h:191
para_builtin_enum
An enum-type for built-in parameters. Also used to address elements in parameter name array...
Definition: ldpk_ldp_builtin.h:13
void init(int nx, int ny)
Initialize a grid with nx * ny sample points. Implies reset().
Definition: ldpk_lookup_table.h:72
This class handles the built-in parameter and the lookup table. You may find it useful for your own d...
Definition: ldpk_ldp_builtin.h:31
void check_builtin_parameters()
This method should be invoked by the derived classes in initializeParameters(). It write error messag...
Definition: ldpk_ldp_builtin.h:278
vec2_type get_current_initial_value() const
Appropriate initial value for calculating warp at current position obtained with get_p_current_fov()...
Definition: ldpk_lookup_table.h:235
vec2_type get_p_current_fov() const
Current position in normalized FOV-coordinates. This is the position you want to warp next...
Definition: ldpk_lookup_table.h:109
const ldpk::lookup_table< vec2_type > & get_lut() const
The derived class has constant access, since it needs to consult it.
Definition: ldpk_ldp_builtin.h:253
Double-valued vector and matrix class.
virtual bool getJacobianMatrix(double x0, double y0, double &m00, double &m01, double &m10, double &m11)
calculate the Jacobian matrix of the undistort()-Method. Overwrite this, if you know the Jacobian for...
Definition: tde4_ld_plugin.h:128
bool set_builtin_parameter_value(const char *identifier, double v)
The return values indicates, if &#39;identifier&#39; refers to a built-in parameter.
Definition: ldpk_ldp_builtin.h:394
void set_q_current_fov(const vec2_type &q_fov)
Set value for given index pair. Index pairs out of bounds will raise an exception. Use this method to insert the warped position from warping p_current_fov.
Definition: ldpk_lookup_table.h:115
bool get_builtin_parameter_type(const char *identifier, tde4_ldp_ptype &ptype) const
The return values indicates, if &#39;identifier&#39; refers to a built-in parameter. Currently, all built-in parameters are double-valued.
Definition: ldpk_ldp_builtin.h:265
bool is_uptodate_lut() const
The derived class uses this in order to check if the lookup table needs an update.
Definition: ldpk_ldp_builtin.h:243