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 
61  bool _uptodate_lut;
62 
65  para_builtin_enum find_builtin_parameter_by_name(const char* identifier) const;
66 
72  static int compute_extremal_parameters_of_spline( const vec2_type& qa,const vec2_type& qb,
73  const vec2_type& dqa,const vec2_type& dqb,
74  double* t_out)
75  {
76  vec2_type b = dqa;
77  vec2_type c = -3.0 * qa + 3.0 * qb - 2.0 * dqa - dqb;
78  vec2_type d = 2.0 * qa - 2.0 * qb + dqa + dqb;
79  vec2_type disc;
80  int i_out = 0;
81 
82  for(int i = 0;i < 2;++i)
83  {
84  if(d[i] != 0.0)
85  {
86  double d_mul_3 = 3.0 * d[i];
87  disc[i] = (c[i] * c[i]) / (d_mul_3 * d_mul_3) - b[i] / d_mul_3;
88 // equal 0 means saddle point, i.e. not an extreme value, less than 0 means no extremum and np saddle point.
89 // We are only interested in local extrema, therefore we check against 0.0 and 1.0.
90  if(disc[i] > 0.0)
91  {
92  double t,r = sqrt(disc[i]);
93  t = -c[i] / d_mul_3 + r;
94  if((t > 0.0) && (t < 1.0))
95  { t_out[i_out++] = t; }
96  t = -c[i] / d_mul_3 - r;
97  if((t > 0.0) && (t < 1.0))
98  { t_out[i_out++] = t; }
99  }
100  }
101  else if(c[i] != 0.0)
102  {
103 // Not a cubic polynomial? Treat as quadratic.
104  double t = b[i] / (-2.0 * c[i]);
105  if((t > 0.0) && (t < 1.0))
106  { t_out[i_out++] = t; }
107  }
108  }
109  return i_out;
110  }
111  template <int I> void extend_box_by_spline( const vec2_type& pa,const vec2_type& pb,
112  const vec2_type& qa,const vec2_type& qb,
113  const mat2_type& ja,const mat2_type& jb,
114  box2_type& bbox)
115  {
116  double t_out[4];
117  vec2_type dqa = ja * (pb - pa);
118  vec2_type dqb = jb * (pb - pa);
119  int n_out = compute_extremal_parameters_of_spline(qa,qb,dqa,dqb,t_out);
120  for(int i = 0;i < n_out;++i)
121  {
122  if((t_out[i] <= 0.0) || (t_out[i] >= 1.0))
123  { std::cerr << "ldpk::ldp_builtin: extend_box_by_spline: implementation error, needs fix." << std::endl; }
124  vec2_type qt,pt;
125  pt = (1.0 - t_out[i]) * pa + t_out[i] * pb;
126  qt = apply_mapping<I>(pt);
127  bbox.extend(qt);
128  }
129  }
131  template <int I> mat2_type get_jacobian_matrix(const vec2_type& p,const vec2_type& q)
132  {
133  mat2_type j;
134  if(I)
135  {
136  getJacobianMatrix(p[0],p[1],j[0][0],j[0][1],j[1][0],j[1][1]);
137  return j;
138  }
139  else
140  {
141 // Checked with difference quotients. This is called the Inverse Function Theorem.
142  getJacobianMatrix(q[0],q[1],j[0][0],j[0][1],j[1][0],j[1][1]);
143  return invert(j);
144  }
145  }
147  template <int I> vec2_type apply_mapping(const vec2_type& p)
148  {
149  vec2_type q;
150  if(I)
151  { undistort(p[0],p[1],q[0],q[1]); }
152  else
153  { distort(p[0],p[1],q[0],q[1]); }
154  return q;
155  }
156  template <int I> void get_bounding_box( double xa_in,double ya_in,double xb_in,double yb_in,
157  double& xa_out,double& ya_out,double& xb_out,double& yb_out,
158  int nx,int ny)
159  {
160  double dx_in = xb_in - xa_in,dy_in = yb_in - ya_in;
161  box2_type bbox;
162 
163  vec2_type pa,pb;
164  vec2_type qa,qb;
165  mat2_type ja,jb;
166 // ya_in
167  pb = vec2_type(xa_in,ya_in);
168  qb = apply_mapping<I>(pb);
169  jb = get_jacobian_matrix<I>(pb,qb);
170  bbox.extend(qb);
171  for(int ix = 1;ix <= nx;++ix)
172  {
173  double x_in = xa_in + dx_in * double(ix) / nx;
174  double y_in = ya_in;
175 // original
176  pa = pb;qa = qb;ja = jb;
177  pb = vec2_type(x_in,y_in);
178  qb = apply_mapping<I>(pb);
179  jb = get_jacobian_matrix<I>(pb,qb);
180  extend_box_by_spline<I>(pa,pb,qa,qb,ja,jb,bbox);
181  bbox.extend(qb);
182  }
183 // yb_in
184  pb = vec2_type(xa_in,yb_in);
185  qb = apply_mapping<I>(pb);
186  jb = get_jacobian_matrix<I>(pb,qb);
187  bbox.extend(qb);
188  for(int ix = 1;ix <= nx;++ix)
189  {
190  double x_in = xa_in + dx_in * double(ix) / nx;
191  double y_in = yb_in;
192 // copy-paste
193  pa = pb;qa = qb;ja = jb;
194  pb = vec2_type(x_in,y_in);
195  qb = apply_mapping<I>(pb);
196  jb = get_jacobian_matrix<I>(pb,qb);
197  extend_box_by_spline<I>(pa,pb,qa,qb,ja,jb,bbox);
198  bbox.extend(qb);
199  }
200 // xa_in
201  pb = vec2_type(xa_in,ya_in);
202  qb = apply_mapping<I>(pb);
203  jb = get_jacobian_matrix<I>(pb,qb);
204  bbox.extend(qb);
205  for(int iy = 1;iy <= ny;++iy)
206  {
207  double x_in = xa_in;
208  double y_in = ya_in + dy_in * double(iy) / ny;
209 // copy-paste
210  pa = pb;qa = qb;ja = jb;
211  pb = vec2_type(x_in,y_in);
212  qb = apply_mapping<I>(pb);
213  jb = get_jacobian_matrix<I>(pb,qb);
214  extend_box_by_spline<I>(pa,pb,qa,qb,ja,jb,bbox);
215  bbox.extend(qb);
216  }
217 // xb_in
218  pb = vec2_type(xb_in,ya_in);
219  qb = apply_mapping<I>(pb);
220  jb = get_jacobian_matrix<I>(pb,qb);
221  bbox.extend(qb);
222  for(int iy = 1;iy <= ny;++iy)
223  {
224  double x_in = xb_in;
225  double y_in = ya_in + dy_in * double(iy) / ny;
226 // copy-paste
227  pa = pb;qa = qb;ja = jb;
228  pb = vec2_type(x_in,y_in);
229  qb = apply_mapping<I>(pb);
230  jb = get_jacobian_matrix<I>(pb,qb);
231  extend_box_by_spline<I>(pa,pb,qa,qb,ja,jb,bbox);
232  bbox.extend(qb);
233  }
234  xa_out = bbox.a()[0];
235  ya_out = bbox.a()[1];
236  xb_out = bbox.b()[0];
237  yb_out = bbox.b()[1];
238  }
239  protected:
241 #ifdef _WINDOWS
242  CRITICAL_SECTION _critsec;
243 #else
244  pthread_mutex_t _mutex;
245 #endif
246 
249 
250  bool is_uptodate_lut() const
253  { return _uptodate_lut; }
257  { _uptodate_lut = false; }
259  void set_uptodate_lut(bool u)
260  { _uptodate_lut = u; }
263  virtual void update_lut();
266  { return _lut; }
268 
270  int get_num_builtin_parameters() const
272  { return 7; }
274  bool set_builtin_parameter_value(const char* identifier,double v);
277  bool get_builtin_parameter_type(const char* identifier,tde4_ldp_ptype& ptype) const
278  {
279  if(find_builtin_parameter_by_name(identifier) != para_not_a_builtin)
280  {
281  ptype = TDE4_LDP_DOUBLE;
282  return true;
283  }
284  return false;
285  }
291  {
292  if(w_fb_cm() == 0)
293  { std::cerr << "ldp_builtin: filmback width is 0." << std::endl; }
294  if(h_fb_cm() == 0)
295  { std::cerr << "ldp_builtin: filmback height is 0." << std::endl; }
296  if(fl_cm() == 0)
297  { std::cerr << "ldp_builtin: focal length is 0." << std::endl; }
298  if(pa() == 0)
299  { std::cerr << "ldp_builtin: pixel aspect is 0." << std::endl; }
300  _r_fb_cm = sqrt(w_fb_cm() * w_fb_cm() + h_fb_cm() * h_fb_cm()) / 2.0;
301  }
303  vec2_type map_unit_to_dn(const vec2_type& p_unit) const
304  {
305  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());
306  return p_cm / r_fb_cm();
307  }
308  vec2_type map_dn_to_unit(const vec2_type& p_dn) const
309  {
310  vec2_type p_cm(p_dn * r_fb_cm());
311  p_cm += vec2_type(w_fb_cm() / 2 + x_lco_cm(),h_fb_cm() / 2 + y_lco_cm());
312  return vec2_type(p_cm[0] / w_fb_cm(),p_cm[1] / h_fb_cm());
313  }
314  void lock()
315  {
316 #ifdef _WINDOWS
317  EnterCriticalSection(&_critsec);
318 #else
319  pthread_mutex_lock(&this->_mutex);
320 #endif
321  }
322  void unlock()
323  {
324 #ifdef _WINDOWS
325  LeaveCriticalSection(&_critsec);
326 #else
327  pthread_mutex_unlock(&this->_mutex);
328 #endif
329  }
330  public:
331  ldp_builtin():_uptodate_lut(false)
332  {
333 #ifdef _WINDOWS
334  InitializeCriticalSection(&_critsec);
335 #else
336  int r = pthread_mutex_init(&_mutex,NULL);
337  if(r)
338  { std::cerr << "ldpk::ldp_builtin::pthread_mutex_init: " << strerror(r) << std::endl; }
339 #endif
340 // We set all built-in parameters to zero. That way we can reliably find out
341 // if the application initializies properly (i.e. pixel aspect needs to be set).
342  for(int i = 0;i < get_num_builtin_parameters();++i)
343  { _value_builtin[i] = 0; }
344  }
345  virtual ~ldp_builtin()
346  {
347 #ifdef _WINDOWS
348  DeleteCriticalSection(&_critsec);
349 #else
350  int r = pthread_mutex_destroy(&_mutex);
351  if(r)
352  { std::cerr << "ldpk::ldp_builtin::pthread_mutex_destroy: " << strerror(r) << std::endl; }
353 #endif
354  }
357 
358  double w_fb_cm() const
359  { return _value_builtin[para_filmback_width]; }
360  double h_fb_cm() const
361  { return _value_builtin[para_filmback_height]; }
362  double x_lco_cm() const
363  { return _value_builtin[para_lens_center_offset_x]; }
364  double y_lco_cm() const
365  { return _value_builtin[para_lens_center_offset_y]; }
366  double fl_cm() const
367  { return _value_builtin[para_focal_length]; }
368  double pa() const
369  { return _value_builtin[para_pixel_aspect]; }
370  double fd_cm() const
371  { return _value_builtin[para_custom_focus_distance]; }
372 
373  double r_fb_cm() const
374  { return _r_fb_cm; }
376 
382  virtual bool undistort_gnomonic(double x0, double y0, double &x1, double &y1)
383  { return undistort(x0,y0,x1,y1); }
384  virtual bool distort_gnomonic(double x0, double y0, double &x1, double &y1)
385  { return distort(x0,y0,x1,y1); }
386  virtual bool distort_gnomonic(double x0, double y0, double x1_start , double y1_start, double &x1, double &y1)
387  { return distort(x0,y0,x1_start,y1_start,x1,y1); }
389 
391  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)
392  { get_bounding_box<1>(xa_in,ya_in,xb_in,yb_in,xa_out,ya_out,xb_out,yb_out,nx,ny); }
393  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)
394  { get_bounding_box<0>(xa_in,ya_in,xb_in,yb_in,xa_out,ya_out,xb_out,yb_out,nx,ny); }
396  };
397 
398 // Built-in parameter names. Do not change.
399  template <class VEC2,class MAT2>
400  const char* ldp_builtin<VEC2,MAT2>::_para_builtin[7] = {
401  "tde4_focal_length_cm",
402  "tde4_filmback_width_cm",
403  "tde4_filmback_height_cm",
404  "tde4_lens_center_offset_x_cm",
405  "tde4_lens_center_offset_y_cm",
406  "tde4_pixel_aspect",
407  "tde4_custom_focus_distance_cm"
408  };
409  template <class VEC2,class MAT2>
411  {
412  for(int i = 0;i < get_num_builtin_parameters();++i)
413  {
414  if(strcmp(_para_builtin[i],identifier) == 0)
415  {
416  return para_builtin_enum(i);
417  }
418  }
419  return para_not_a_builtin;
420  }
421  template <class VEC2,class MAT2>
422  bool ldp_builtin<VEC2,MAT2>::set_builtin_parameter_value(const char* identifier,double v)
423  {
424  int i = find_builtin_parameter_by_name(identifier);
425  if(i < 0)
426  { return false; }
427  if(_value_builtin[i] != v)
428  {
429 // Whenever a parameter is changed, the lookup table becomes obsolete.
431  _value_builtin[i] = v;
432  }
433  return true;
434  }
435  template <class VEC2,class MAT2>
437  {
438  _lut.init(45,45);
439  _lut.reset();
440  while(_lut.next())
441  {
442  vec2_type qs = _lut.get_current_initial_value();
443  vec2_type q,p = _lut.get_p_current_fov();
444 // Call the inverse model function with initial values.
445  distort_gnomonic(p[0],p[1],qs[0],qs[1],q[0],q[1]);
446  _lut.set_q_current_fov(q);
447  }
448  set_uptodate_lut(true);
449  }
450  }
451 
452 #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:393
bool next()
False means no value to be generated, loop is done. See example.
Definition: ldpk_lookup_table.h:203
virtual void update_lut()
Update the lookup table. The derived class will do this, if necessary, when distort() without initial...
Definition: ldpk_ldp_builtin.h:436
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:256
int get_num_builtin_parameters() const
There are seven built-in parameters.
Definition: ldpk_ldp_builtin.h:271
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)
Bounding box.
Definition: ldpk_ldp_builtin.h:391
CRITICAL_SECTION _critsec
The Mutex, used in derived classes, initialized in constructor.
Definition: ldpk_ldp_builtin.h:242
The namespace of (most of the) things related to the Lens Distortion Plugin Kit.
Definition: ldpk.h:19
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
void set_uptodate_lut(bool u)
If a derived class computes the LUT, it must have write-access to this flag.
Definition: ldpk_ldp_builtin.h:259
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:290
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:265
virtual bool undistort_gnomonic(double x0, double y0, double &x1, double &y1)
Model function Our base class presents the model functions along with the projection. Internally, we have to carefully keep apart the model function and the projection. For most distortion models, this is no problem, yet for fisheye models it is, so all non-gnomonic distortion classes will have to overwrite these.
Definition: ldpk_ldp_builtin.h:382
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:132
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:422
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:277
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:252