ldpk
tde4_ldp_radial_fisheye_base_deg_8.experimental.h
1 #pragma once
2 
3 #include <ldpk/ldpk_ldp_builtin.h>
5 #include <ldpk/ldpk_vec2d.h>
6 
9 
12 
13 template <class VEC2,class MAT2>
15  {
16 private:
17  typedef VEC2 vec2_type;
18  typedef MAT2 mat2_type;
20 
22 
23  static const char* _para[4];
24 
25 // This function is meant to determine the valid domain of undistort().
26 // On one hand we will know if the elomgation leaves the domain given
27 // by the projection, as this is encode in the return value. On the other
28 // hand we can also study the behaviour of the mapped undistorted elongation.
29 // If it starts flipping back, we have left the valid domain.
30  bool undistort_dn(double r_ed_dist_dn,double &r_plain_undist_dn)
31  {
32 // The literature commonly says, undistorting should be done
33 // before applying the remapping. Usually it's applied to
34 // elongation angle theta = r/f. That's what we do here.
35 // theta = r/f is the equidistant projection, so here we have g o F^-1.
36  double r_ed_undist_dn = _fl_dn * _distortion(r_ed_dist_dn / _fl_dn);
37 // Remap equidistant to gnomonic
38  if(!remap_fe2plain(r_ed_undist_dn,r_plain_undist_dn))
39  { return false; }
40  return true;
41  }
42 protected:
43  double _r_clip_factor;
44 // Largest radius for which undistort_dn() is still well-defined.
45  double _r_ed_dn_domain;
46 // The domain radius mapped to undistorted. We will try
47 // to build a simple bounding box from this.
48  double _r_plain_dn_domain;
49 // Focal length in dn-coordinates.
50  double _fl_dn;
51 
52  bool decypher(const char* name,int& i)
53  {
54  typedef base_type bt;
55  int n;
57  for(i = 0;i < n;++i)
58  {
59  if(0 == strcmp(name,_para[i]))
60  { return true; }
61  }
62  return false;
63  }
65  {
66  typedef base_type bt;
67  bt::check_builtin_parameters();
68 // Focal length in dn-coordinates.
69  _fl_dn = bt::fl_cm() / bt::r_fb_cm();
70 
71 // We do a simple brute force scan per pass, but refine the search interval
72 // after each pass. Precision should be around 1e-5 afterwards.
73 // Not quite sure which value makes sense for radius_b.
74 // Without any distortion, the result r/f for stereographic is always 2.
75 // For othographic we get 1, for equisolid sqrt(2) and for equidistant pi/2.
76  double radius_a = 0.0;
77  double radius_b = M_PI * _fl_dn;
78 // At most n samples for each pass.
79  int n = 50;
80 // A few passes
81  for(int i_pass = 0;i_pass < 5;++i_pass)
82  {
83  double r_ed_dist_dn_prev = -1.0;
84  double r_plain_undist_dn_prev = -1.0;
85 //std::cout << "radius_a / _fl_dn, radius_b / _fl_dn:" << radius_a / _fl_dn << " " << radius_b / _fl_dn << std::endl;
86 // We initialize the domain with the largest radius possible.
87 // In practice, the loop will terminate prior to reaching n,
88 // because at some point undistort_dn() wil return false,
89 // so this most likely is not relevant, but we want to make
90 // sure that _r_ed_dn_domain has some well-defined value.
91  _r_ed_dn_domain = radius_b;
92  double r_ed_dist_dn;
93  for(int i = 0;i <= n;++i)
94  {
95 // Normalized iterator.
96  double q = double(i) / n;
97 // Scan r_ed_dist_dn from radius_a to radius_b, inclusive.
98  r_ed_dist_dn = radius_a * (1.0 - q) + radius_b * q;
99  double r_plain_undist_dn;
100  if(!undistort_dn(r_ed_dist_dn,r_plain_undist_dn))
101  {
102 // undistort fails, so we use domain radius and mapped radius from the previous round.
103  _r_ed_dn_domain = r_ed_dist_dn_prev;
104  _r_plain_dn_domain = r_plain_undist_dn_prev;
105  break;
106  }
107 // Found a turning point? We are looking for the point where the Jacobian degenerates,
108 // but this criterions is faster and simpler than evaluating the Jacobian.
109  if(r_plain_undist_dn <= r_plain_undist_dn_prev)
110  {
111 // The last known radius for which undistort_dn() was well-defined.
112  _r_ed_dn_domain = r_ed_dist_dn_prev;
113  _r_plain_dn_domain = r_plain_undist_dn_prev;
114  break;
115  }
116 // r_ed_dist_dn has successfully been mapped into r_plain_undist_dn.
117 // Our current radii become the previous radii for the next round.
118  r_ed_dist_dn_prev = r_ed_dist_dn;
119  r_plain_undist_dn_prev = r_plain_undist_dn;
120 // And we can extend our domain.
121  _r_ed_dn_domain = r_ed_dist_dn;
122  _r_plain_dn_domain = r_plain_undist_dn;
123  }
124 // Refine search interval. radius_a is always in the valid domain.
125  radius_a = r_ed_dist_dn_prev;
126  radius_b = r_ed_dist_dn;
127 //std::cout << "r_domain / _fl_dn: " << _r_ed_dn_domain / _fl_dn << std::endl;
128  }
129  return true;
130  }
131  bool getNumParameters(int& n)
132  {
133  n = 4;
134  return true;
135  }
136  bool getParameterName(int i,char* identifier)
137  {
138  strcpy(identifier,_para[i]);
139  return true;
140  }
141  bool setParameterValue(const char *identifier,double v)
142  {
143  typedef base_type bt;
144  int i;
145 // Does the base class know the parameter?
146  if(bt::set_builtin_parameter_value(identifier,v))
147  { return true; }
148  if(!decypher(identifier,i))
149  { return false; }
150  if(_distortion.get_coeff(i) != v)
151  { bt::no_longer_uptodate_lut(); }
152  _distortion.set_coeff(i,v);
153  return true;
154  }
155  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)
156  {
157  typedef base_type bt;
158 // _r_plain_dn_domain represents the radius of the undistorted image in dn-coordinates.
159 // We form a square and use its corners as the bounding box for the undistorted image.
160 // vec2_type p00_dn = vec2_type(-_r_plain_dn_domain,-_r_plain_dn_domain);
161 // vec2_type p11_dn = vec2_type( _r_plain_dn_domain, _r_plain_dn_domain);
162 
163 // vec2_type p00_unit = bt::map_dn_to_unit(p00_dn);
164 // vec2_type p11_unit = bt::map_dn_to_unit(p11_dn);
165 
166 // xa_out = p00_unit[0];
167 // ya_out = p00_unit[1];
168 // xb_out = p11_unit[0];
169 // yb_out = p11_unit[1];
170 
171  xa_out = 0.0;
172  ya_out = 0.0;
173  xb_out = 1.0;
174  yb_out = 1.0;
175 
176 // std::cout << "getBoundingBoxUndistort: " << xa_out << " " << ya_out << " " << xb_out << " " << yb_out << std::endl;
177  }
178  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)
179  {
180  typedef base_type bt;
181 
182 // _r_ed_dn_domain is the radius of the valid domain of undistort() in dn-coordinates.
183 // We form a square and use its corners as the bounding box for the distorted image.
184 // vec2_type p00_dn = vec2_type(-_r_ed_dn_domain,-_r_ed_dn_domain);
185 // vec2_type p11_dn = vec2_type( _r_ed_dn_domain, _r_ed_dn_domain);
186 
187 // vec2_type p00_unit = bt::map_dn_to_unit(p00_dn);
188 // vec2_type p11_unit = bt::map_dn_to_unit(p11_dn);
189 
190 // xa_out = p00_unit[0];
191 // ya_out = p00_unit[1];
192 // xb_out = p11_unit[0];
193 // yb_out = p11_unit[1];
194 
195  xa_out = 0.0;
196  ya_out = 0.0;
197  xb_out = 1.0;
198  yb_out = 1.0;
199 
200 // std::cout << "getBoundingBoxDistort: " << xa_out << " " << ya_out << " " << xb_out << " " << yb_out << std::endl;
201  }
202 
203 
204  virtual bool remap_fe2plain(double r_ed_dn,double& r_plain_dn) = 0;
205  virtual bool remap_plain2fe(double r_plain_dn,double& r_ed_dn) = 0;
206 // Overwriting tde4_ldp_common
207  bool undistort(double x0,double y0,double &x1,double &y1)
208  {
209  typedef base_type bt;
210 
211  vec2_type p_ed_dist_dn = bt::map_unit_to_dn(vec2_type(x0,y0));
212 
213 // Distance from origin in dn-coordinates
214  double r_ed_dist_dn = norm2(p_ed_dist_dn);
215 // We have determined the domain of undistort() in initializeParameters().
216  if(r_ed_dist_dn > _r_ed_dn_domain)
217  { return false; }
218 // The literature commonly says, undistorting should be done
219 // before applying the remapping. Usually it's applied to
220 // elongation angle theta = r/f. That's what we do here.
221 // theta = r/f is the equidistant projection, so here we have g o F^-1.
222  double r_ed_undist_dn = _fl_dn * _distortion(r_ed_dist_dn / _fl_dn);
223 // Remap equidistant to gnomonic
224  double r_plain_undist_dn;
225  if(!remap_fe2plain(r_ed_undist_dn,r_plain_undist_dn))
226  { return false; }
227 // Calc undistorted point from undistorted radius
228  vec2_type p_plain_undist_dn;
229  if(dotsq(p_ed_dist_dn) == 0.0)
230  { p_plain_undist_dn = vec2_type(0,0); }
231  else
232  { p_plain_undist_dn = r_plain_undist_dn * unit(p_ed_dist_dn); }
233 // Map back to unit coordinates
234  vec2_type p_plain_undist_unit = bt::map_dn_to_unit(p_plain_undist_dn);
235  x1 = p_plain_undist_unit[0];
236  y1 = p_plain_undist_unit[1];
237  return true;
238  }
239  bool distort(double x0,double y0,double &x1,double &y1)
240  {
241  typedef base_type bt;
242 
243  vec2_type p_plain_undis_dn = bt::map_unit_to_dn(vec2_type(x0,y0));
244 // Distance from origin in dn-coordinates
245  double r_ed_undis_dn,r_plain_undis_dn = norm2(p_plain_undis_dn);
246 // We have determined the domain of undistort() in initializeParameters().
247 // Using the undistorted radius, we can formulate a criterion for distort().
248  if(r_plain_undis_dn > _r_plain_dn_domain)
249  { return false; }
250 // Remap gnomonic to equidistant
251  if(!remap_plain2fe(r_plain_undis_dn,r_ed_undis_dn))
252  { return false; }
253 // Apply distortion. We use the point itself as initial value, since coefficients will be small.
254  double r_ed_dis_dn = _fl_dn * _distortion.map_inverse(r_ed_undis_dn / _fl_dn,r_ed_undis_dn / _fl_dn);
255 // Calc distorted point from distorted radius
256  vec2_type p_ed_dis_dn;
257  if(dotsq(p_plain_undis_dn) == 0.0)
258  { p_ed_dis_dn = vec2_type(0,0); }
259  else
260  { p_ed_dis_dn = r_ed_dis_dn * unit(p_plain_undis_dn); }
261 // Map back to unit coordinates
262  vec2_type p_ed_dis_unit = bt::map_dn_to_unit(p_ed_dis_dn);
263  x1 = p_ed_dis_unit[0];
264  y1 = p_ed_dis_unit[1];
265  return true;
266  }
267 public:
268 // Mutex initialized and destroyed in baseclass.
269  tde4_ldp_radial_fisheye_base_deg_8():_r_clip_factor(50.0),_r_ed_dn_domain(0.0)
270  { }
272  { }
273  double r_clip_factor() const
274  { return _r_clip_factor; }
275  void r_clip_factor(double f)
276  { _r_clip_factor = f; }
277  virtual bool getModelName(char *name) = 0;
278  bool getParameterType(const char* identifier,tde4_ldp_ptype& ptype)
279  {
280  typedef base_type bt;
281  int i;
282  if(bt::get_builtin_parameter_type(identifier,ptype)) return true;
283  if(!decypher(identifier,i)) return false;
284  ptype = TDE4_LDP_ADJUSTABLE_DOUBLE;
285  return true;
286  }
287  bool getParameterDefaultValue(const char* identifier,double& v)
288  {
289  typedef base_type bt;
290  int i;
291  if(!decypher(identifier,i)) return false;
292  v = 0.0;
293  return true;
294  }
295  bool getParameterRange(const char* identifier,double& a,double& b)
296  {
297  typedef base_type bt;
298  int i;
299  if(!decypher(identifier,i)) return false;
300  a = -0.5;
301  b = 0.5;
302  return true;
303  }
304  };
305 
306 template <class VEC2,class MAT2>
308  "Degree 2",
309  "Degree 4",
310  "Degree 6",
311  "Degree 8"
312  };
double map_inverse(double q) const
Inverse mapping by solving the fixed point equation without providing initial values.
Definition: ldpk_generic_radial_distortion_1d.h:96
void set_coeff(int i, double q)
Set coefficient c[i], 0 <= i < N.
Definition: ldpk_generic_radial_distortion_1d.h:59
virtual bool getModelName(char *name)=0
returns a name for the model as to show up in the GUI (maximum length of "name": 100 bytes)...
bool getParameterType(const char *identifier, tde4_ldp_ptype &ptype)
returns type of given parameter... The method should return false, if the parameter addressed by iden...
Definition: tde4_ldp_radial_fisheye_base_deg_8.experimental.h:278
bool getNumParameters(int &n)
returns the number of plugin parameters...
Definition: tde4_ldp_radial_fisheye_base_deg_8.experimental.h:131
Plugin class for radial distortion. Does not compensate for decentering.
Definition: tde4_ldp_radial_fisheye_base_deg_8.experimental.h:14
bool getParameterName(int i, char *identifier)
returns "identifier" name of parameter "i" (maximum length of "identifier": 100 bytes)...
Definition: tde4_ldp_radial_fisheye_base_deg_8.experimental.h:136
bool getParameterRange(const char *identifier, double &a, double &b)
returns range for adjustable double parameters...
Definition: tde4_ldp_radial_fisheye_base_deg_8.experimental.h:295
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: tde4_ldp_radial_fisheye_base_deg_8.experimental.h:178
A polynomial radially symmetric model of degree N (even)
bool getParameterDefaultValue(const char *identifier, double &v)
returns default value for given parameter (maximum length of "char *v": 1000 bytes)......
Definition: tde4_ldp_radial_fisheye_base_deg_8.experimental.h:287
bool undistort(double x0, double y0, double &x1, double &y1)
warp/unwarp 2D points...
Definition: tde4_ldp_radial_fisheye_base_deg_8.experimental.h:207
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
bool setParameterValue(const char *identifier, double v)
set parameter values... parameters predefined by 3DE4: "tde4_focal_length_cm", "tde4_filmback_width_c...
Definition: tde4_ldp_radial_fisheye_base_deg_8.experimental.h:141
bool initializeParameters()
prepare the current set of parameters...
Definition: tde4_ldp_radial_fisheye_base_deg_8.experimental.h:64
Double-valued vector and matrix class.
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: tde4_ldp_radial_fisheye_base_deg_8.experimental.h:155
double get_coeff(int i) const
Get coefficient c[i], 0 <= i < N (i.e. coefficient power r^(2i))
Definition: ldpk_generic_radial_distortion_1d.h:53