vgl_quadric_3d.hxx
Go to the documentation of this file.
1 // This is core/vgl/vgl_quadric_3d.hxx
2 #ifndef vgl_quadric_3d_hxx_
3 #define vgl_quadric_3d_hxx_
4 #include <iostream>
5 #include <algorithm>
6 #include <functional>
7 #include "vgl_quadric_3d.h"
8 #include "vgl_tolerance.h"
9 #define RANK_FACTOR 100000
10 #define DET_FACTOR 100
11 static const char* quadric_class_name[] =
12 {
13  "invalid_quadric",
14  "real_ellipsoid",
15  "imaginary_ellipsoid",
16  "hyperboloid_of_one_sheet",
17  "hyperboloid_of_two_sheets",
18  "real_elliptic_cone",
19  "imaginary_elliptic_cone",
20  "elliptic_paraboloid",
21  "hyperbolic_paraboloid",
22  "real_elliptic_cylinder",
23  "imaginary_elliptic_cylinder",
24  "hyperbolic_cylinder",
25  "real_intersecting_planes",
26  "imaginary_intersecting_planes",
27  "parabolic_cylinder",
28  "real_parallel_planes",
29  "imaginary_parallel_planes",
30  "coincident_planes"
31 };
32 template <class T>
33 std::string vgl_quadric_3d<T>::type_by_number(vgl_quadric_type const& type) { return quadric_class_name[(size_t)type]; }
34 
35 template <class T>
36 typename vgl_quadric_3d<T>::vgl_quadric_type vgl_quadric_3d<T>::type_by_name(std::string const& name){
37  for(size_t i = 1; i<num_quadric_types; i++){
38  if(quadric_class_name[i]==name)
39  return static_cast<vgl_quadric_type>(i);
40  }
41  return no_type;
42 }
43 
44 template <class T>
45 vgl_quadric_3d<T>::vgl_quadric_3d(T a, T b, T c, T d, T e, T f, T g, T h, T i, T j):det_zero_(false){
46  this->set(a, b, c, d, e, f, g, h, i, j);
47 }
48 template <class T>
49 vgl_quadric_3d<T>::vgl_quadric_3d(T const coeff[]):det_zero_(false){
50  this->set(coeff[0],coeff[1],coeff[2],coeff[3],coeff[4],coeff[5],coeff[6],coeff[7],coeff[8],coeff[9]);
51 }
52  // _ _
53  // | a d/2 e/2 g/2 |
54  // | d/2 b f/2 h/2 |
55  // Q = | e/2 f/2 c i/2 |
56  // | g/2 h/2 i/2 j |
57  // - -
58  //
59 template <class T>
60 vgl_quadric_3d<T>::vgl_quadric_3d(std::vector<std::vector<T> > const& Q):det_zero_(false){
61  this->set(Q[0][0], Q[1][1], Q[2][2], T(2)*Q[0][1], T(2)*Q[0][2],
62  T(2)*Q[1][2], T(2)*Q[0][3], T(2)*Q[1][3], T(2)*Q[2][3], Q[3][3]);
63 }
64 template <class T>
65 vgl_quadric_3d<T>::vgl_quadric_3d(std::vector<std::vector<T> > const& canonical_quadric,
66  std::vector<std::vector<T> > const& H){
67  //The 4x4 coefficient matrix, Qg, in the globa frame is given by
68  // Qg = H^-t Qc H^-1
69  // where Qc = canonical_quadric;
70  std::vector<std::vector<T> > R(3,std::vector<T>(3, T(0)));
71  std::vector<std::vector<T> > Qg(4, std::vector<T>(4, T(0)));
72  std::vector<std::vector<T> > qr(3,std::vector<T>(3, T(0)));
73  std::vector<T> qu(3,T(0)), t(3, T(0)), c(3,T(0)), Rc(3,T(0)),
74  RqRt(3, T(0)), Rtrt(3,T(0));
75  for(size_t r = 0; r<3; ++r){
76  t[r]=H[r][3];
77  qu[r] = canonical_quadric[r][r];
78  c[r]=canonical_quadric[r][3];
79  for(size_t c = 0; c<3; ++c)
80  R[r][c] = H[r][c];
81  }
82  qr[0][0]=R[0][0]*qu[0]*R[0][0] +R[0][1]*qu[1]*R[0][1] +R[0][2]*qu[2]*R[0][2];
83  qr[1][0]=R[1][0]*qu[0]*R[0][0] +R[1][1]*qu[1]*R[0][1] +R[1][2]*qu[2]*R[0][2];
84  qr[2][0]=R[2][0]*qu[0]*R[0][0] +R[2][1]*qu[1]*R[0][1] +R[2][2]*qu[2]*R[0][2];
85  qr[1][1]=R[1][0]*qu[0]*R[1][0] +R[1][1]*qu[1]*R[1][1] +R[1][2]*qu[2]*R[1][2];
86  qr[2][1]=R[2][0]*qu[0]*R[1][0] +R[2][1]*qu[1]*R[1][1] +R[2][2]*qu[2]*R[1][2];
87  qr[2][2]=R[2][0]*qu[0]*R[2][0] +R[2][1]*qu[1]*R[2][1] +R[2][2]*qu[2]*R[2][2];
88  qr[0][1]=qr[1][0]; qr[0][2]=qr[2][0]; qr[2][1]=qr[1][2];
89 
90  Rc[0] = R[0][0]*c[0] + R[0][1]*c[1] + R[0][2]*c[2];
91  Rc[1] = R[1][0]*c[0] + R[1][1]*c[1] + R[1][2]*c[2];
92  Rc[2] = R[2][0]*c[0] + R[2][1]*c[1] + R[2][2]*c[2];
93 
94  Rtrt[0]=R[0][0]*t[0] + R[1][0]*t[1] + R[2][0]*t[2];
95  Rtrt[1]=R[0][1]*t[0] + R[1][1]*t[1] + R[2][1]*t[2];
96  Rtrt[2]=R[0][2]*t[0] + R[1][2]*t[1] + R[2][2]*t[2];
97 
98  RqRt[0] = (qr[0][0]*t[0] + qr[0][1]*t[1] + qr[0][2]*t[2]);
99  RqRt[1] = (qr[1][0]*t[0] + qr[1][1]*t[1] + qr[1][2]*t[2]);
100  RqRt[2] = (qr[2][0]*t[0] + qr[2][1]*t[1] + qr[2][2]*t[2]);
101 
102  T tRqRt = t[0]*RqRt[0] + t[1]*RqRt[1] + t[2]*RqRt[2];
103  T tRc = t[0]*Rc[0] + t[1]*Rc[1] + t[2]*Rc[2];
104  T ctrRtrt = c[0]*Rtrt[0] + c[1]*Rtrt[1] + c[2]*Rtrt[2];
105 
106  Qg[0][3] = Rc[0]-RqRt[0];
107  Qg[1][3] = Rc[1]-RqRt[1];
108  Qg[2][3] = Rc[2]-RqRt[2];
109  Qg[3][0] = Qg[0][3]; Qg[3][1] = Qg[1][3]; Qg[3][2] = Qg[2][3];
110 
111  for(size_t r = 0; r<3; ++r)
112  Qg[r][r] = qr[r][r];
113 
114  Qg[1][0] = qr[1][0]; Qg[2][0] = qr[2][0]; Qg[2][1] = qr[2][1];
115  Qg[0][1] = Qg[1][0]; Qg[0][2] = Qg[2][0]; Qg[1][2] = Qg[2][1];
116  Qg[3][3] = canonical_quadric[3][3]- ctrRtrt + tRqRt - tRc;
117  *this = vgl_quadric_3d<T>(Qg);
118 }
119 template <class T>
120 vgl_quadric_3d<T>::vgl_quadric_3d(std::vector<T> const& diag,
121  std::vector<std::vector<T> > const& H){
122  std::vector<std::vector<T> > Qg(4, std::vector<T>(4, T(0)));
123  Qg[0][0] = diag[0];Qg[1][1] = diag[1];Qg[2][2] = diag[2];Qg[3][3] = diag[3];
124  *this = vgl_quadric_3d<T>(Qg, H);
125 }
126  template <class T>
127 void vgl_quadric_3d<T>::set(T a, T b, T c, T d, T e, T f, T g, T h, T i, T j){
128  a_ = a; b_ = b; c_ = c; d_ = d; e_ = e; f_ = f;
129  g_ = g; h_ = h; i_ = i; j_ = j;
130  this->compute_type();
131 }
132 template <class T>
133 void vgl_quadric_3d<T>::set(std::vector<std::vector<T> > const& Q){
134  this->set(Q[0][0], Q[1][1], Q[2][2], T(2)*Q[0][1],
135  T(2)*Q[0][2], T(2)*Q[1][2], T(2)*Q[0][3],
136  T(2)*Q[1][3], T(2)*Q[2][3], Q[3][3]);
137 }
138 template <class T>
139 std::vector<std::vector<T> > vgl_quadric_3d<T>::coef_matrix() const{
140  std::vector<std::vector<T> > Q(4,std::vector<T>(4,T(0)));
141  Q[0][0]=a_;Q[1][1]=b_; Q[2][2]=c_; Q[3][3]=j_;
142  Q[0][1]= Q[1][0]=d_/T(2); Q[0][2]= Q[2][0]=e_/T(2);
143  Q[0][3]= Q[3][0]=g_/T(2); Q[1][2]= Q[2][1]=f_/T(2);
144  Q[1][3]= Q[3][1]=h_/T(2); Q[3][2]=Q[2][3] =i_/T(2);
145  return Q;
146 }
147 template <class T>
148 T vgl_quadric_3d<T>::sampson_dist(vgl_homg_point_3d<T> const& pt) const{
149  T x = pt.x(), y = pt.y(), z = pt.z(), w = pt.w();
150  T algebraic_dist = a_*x*x + b_*y*y + c_*z*z + d_*x*y + e_*x*z + f_*y*z + g_*x*w + h_*y*w + i_*z*w +j_*w*w;
151  T grad_x = (T(2)*a_*x + d_*y * e_*z + g_*w);
152  T grad_y = (T(2)*b_*y + d_*x * f_*z + h_*w);
153  T grad_z = (T(2)*c_*z + e_*x * f_*y + i_*w );
154  T grad_mag_sqrd = grad_x*grad_x + grad_y*grad_y + grad_z*grad_z;
155  T sampson_dist_sqrd = (algebraic_dist*algebraic_dist)/grad_mag_sqrd;
156  return sqrt(sampson_dist_sqrd);
157 }
158 template <class T>
159 bool vgl_quadric_3d<T>::on(vgl_homg_point_3d<T> const& pt, T tol) const{
160  T d = this->sampson_dist(pt);
161  if(d < tol)
162  return true;
163  return false;
164 }
165 template <class T>
166 bool vgl_quadric_3d<T>::center(vgl_point_3d<T>& center) const{
167  if(!(type_ == real_ellipsoid || type_==real_elliptic_cone ||
168  type_ == hyperboloid_of_one_sheet || type_ == hyperboloid_of_two_sheets)
169  )
170  return false;
171  T h = T(1)/T(2);
172  // _ _
173  // | a d/2 e/2|
174  // upper3x3 = | d/2 b f/2|
175  // | e/2 f/2 c |
176  // - -
177  T upper_det = a_*b_*c_ - a_*h*f_*h*f_ - h*d_*h*d_*c_ + d_*h*f_*h*e_*h + e_*h*d_*h*f_*h - e_*h*b_*e_*h;
178  if(fabs(upper_det)<T(DET_FACTOR)*vgl_tolerance<T>::position){
179  return false;
180  }
181  T det_inv = T(1)/upper_det;
182 
183  T d[9];// inverse of upper 3x3
184  d[0] = (b_*c_ -f_*f_*h*h)*det_inv;
185  d[1] = (f_*h*e_*h -c_*d_*h)*det_inv;
186  d[2] = (d_*h*f_*h -e_*h*b_)*det_inv;
187  d[3] = (f_*h*e_*h -d_*h*c_)*det_inv;
188  d[4] = (a_*c_ -e_*h*e_*h)*det_inv;
189  d[5] = (d_*h*e_*h -f_*h*a_)*det_inv;
190  d[6] = (d_*h*f_*h -b_*e_*h)*det_inv;
191  d[7] = (d_*h*e_*h -a_*f_*h)*det_inv;
192  d[8] = (a_*b_ -d_*h*d_*h)*det_inv;
193 
194  // tx 1 d0 d1 d2 g_
195  // ty = - d3 d4 d5 h_
196  // tz 2 d6 d7 d8 i_
197 
198  T tx = h*(d[0]*g_ + d[1]*h_ + d[2]*i_);
199  T ty = h*(d[3]*g_ + d[4]*h_ + d[5]*i_);
200  T tz = h*(d[6]*g_ + d[7]*h_ + d[8]*i_);
201  center.set(-tx, -ty, -tz);
202  return true;
203 }
204  template <class T>
205 void vgl_quadric_3d<T>::compute_type(){
206  type_ = no_type;
207  T tol = vgl_tolerance<T>::position;
208  std::vector<std::vector<T> > Q = this->coef_matrix();
209  T m[4][4]; T l[4]; T vc[4][4];
210  for(size_t r = 0; r<4; ++r)
211  for(size_t c = 0; c<4; ++c)
212  m[r][c] = Q[r][c];
213 
214  T mu[3][3]; T lu[3]; T vcu[3][3];
215  for(size_t r = 0; r<3; ++r)
216  for(size_t c = 0; c<3; ++c)
217  mu[r][c] = Q[r][c];
218 
219  // the vector l contains the eigenvalues of the coeficient matrix
220  eigen<T, 4>( m, l, vc);
221 std::vector<T> lv;
222  //determine the rank of Q
223  T rank_tol = T(RANK_FACTOR)*tol;
224  std::vector<T> eig_vals;
225  for(auto & i : l){
226  eig_vals.push_back(fabs(i));
227  lv.push_back(i);
228 }
229 
230  std::sort(eig_vals.begin(), eig_vals.end(), std::greater<T>());
231  T largest_eig_val = eig_vals[0];
232  size_t r4 = 0;
233  for (size_t i = 0; i < 4; ++i) {
234  eig_vals[i] /= largest_eig_val;
235  if (eig_vals[i] > rank_tol)
236  r4++;
237  }
238  std::vector<T> non_zero_vals;
239  for(size_t i = 0 ; i<4; ++i){
240  if(fabs(lv[i])/largest_eig_val <= rank_tol)
241  continue;
242  non_zero_vals.push_back(lv[i]);
243  }
244  T z = T(0);
245  size_t nz = non_zero_vals.size();
246  if(nz == 0)
247  return;
248  bool sign = true;
249  if(nz == 2)
250  sign =(non_zero_vals[0]>z && non_zero_vals[1]>z)||
251  (non_zero_vals[0]<z && non_zero_vals[1]<z);
252  else if(nz == 3)
253  sign = (non_zero_vals[0]>z && non_zero_vals[1]>z && non_zero_vals[2]>z) ||
254  (non_zero_vals[0]<z&& non_zero_vals[1]< z&& non_zero_vals[2]< z);
255  else if(nz == 4)
256  sign=(non_zero_vals[0]>z&&non_zero_vals[1]>z&&non_zero_vals[2]>z&&non_zero_vals[3]>z)||
257  (non_zero_vals[0]<z&& non_zero_vals[1]< z&& non_zero_vals[2]< z&& non_zero_vals[3]< z);
258 
259 
260  //determine the rank of upper 3x3 of Q
261  std::vector<T> lvu;
262  eigen<T, 3>( mu, lu, vcu);
263  std::vector<T> upper_eig_vals;
264  for(auto & i : lu){
265  upper_eig_vals.push_back(fabs(i));
266  lvu.push_back(i);
267  }
268  std::sort(upper_eig_vals.begin(), upper_eig_vals.end(), std::greater<T>());
269  largest_eig_val = upper_eig_vals[0];
270  size_t r3 = 0;
271  for (size_t i = 0; i < 3; ++i) {
272  upper_eig_vals[i] /= largest_eig_val;
273  if (upper_eig_vals[i] > rank_tol){
274  r3++;
275  }
276  }
277  std::vector<T> non_zero_uvals;
278  for(size_t i = 0 ; i<3; ++i){
279  if(fabs(lvu[i])/largest_eig_val <= rank_tol)
280  continue;
281  non_zero_uvals.push_back(lvu[i]);
282  }
283  nz = non_zero_uvals.size();
284  if(nz == 0)
285  return;
286  bool signu = true;
287  if(nz == 2)
288  signu =(non_zero_uvals[0]>z && non_zero_uvals[1]>z)||
289  (non_zero_uvals[0]<z && non_zero_uvals[1]<z);
290  else if(nz == 3)
291  signu = (non_zero_uvals[0]>z && non_zero_uvals[1]>z && non_zero_uvals[2]>z) ||
292  (non_zero_uvals[0]<z&& non_zero_uvals[1]< z&& non_zero_uvals[2]< z);
293 
294  //compute the determinant of Q
295 T det = Q[0][0]*Q[1][1]*Q[2][2]*Q[3][3]
296  - Q[0][0]*Q[1][1]*Q[3][2]*Q[2][3]
297  - Q[0][0]*Q[2][1]*Q[1][2]*Q[3][3]
298  + Q[0][0]*Q[2][1]*Q[3][2]*Q[1][3]
299  + Q[0][0]*Q[3][1]*Q[1][2]*Q[2][3]
300  - Q[0][0]*Q[3][1]*Q[2][2]*Q[1][3]
301  - Q[1][0]*Q[0][1]*Q[2][2]*Q[3][3]
302  + Q[1][0]*Q[0][1]*Q[3][2]*Q[2][3]
303  + Q[1][0]*Q[2][1]*Q[0][2]*Q[3][3]
304  - Q[1][0]*Q[2][1]*Q[3][2]*Q[0][3]
305  - Q[1][0]*Q[3][1]*Q[0][2]*Q[2][3]
306  + Q[1][0]*Q[3][1]*Q[2][2]*Q[0][3]
307  + Q[2][0]*Q[0][1]*Q[1][2]*Q[3][3]
308  - Q[2][0]*Q[0][1]*Q[3][2]*Q[1][3]
309  - Q[2][0]*Q[1][1]*Q[0][2]*Q[3][3]
310  + Q[2][0]*Q[1][1]*Q[3][2]*Q[0][3]
311  + Q[2][0]*Q[3][1]*Q[0][2]*Q[1][3]
312  - Q[2][0]*Q[3][1]*Q[1][2]*Q[0][3]
313  - Q[3][0]*Q[0][1]*Q[1][2]*Q[2][3]
314  + Q[3][0]*Q[0][1]*Q[2][2]*Q[1][3]
315  + Q[3][0]*Q[1][1]*Q[0][2]*Q[2][3]
316  - Q[3][0]*Q[1][1]*Q[2][2]*Q[0][3]
317  - Q[3][0]*Q[2][1]*Q[0][2]*Q[1][3]
318  + Q[3][0]*Q[2][1]*Q[1][2]*Q[0][3];
319 
320  if(fabs(det)<T(DET_FACTOR)*tol)
321  det_zero_ = true;
322  bool gt_0 = det>z;
323 /*
324  quadric type equation r3 r4 gt_0 signu sign
325 coincident_planes x^2=0 1 1
326 imaginary_ellipsoid (x^2)/(a^2)+(y^2)/(b^2)+(z^2)/(c^2)=-1 3 4 true true
327 real_ellipsoid (x^2)/(a^2)+(y^2)/(b^2)+(z^2)/(c^2)=1 3 4 false true
328 imaginary_elliptic_cone (x^2)/(a^2)+(y^2)/(b^2)+(z^2)/(c^2)=0 3 3 true
329 real_elliptic_cone (x^2)/(a^2)+(y^2)/(b^2)-(z^2)/(c^2)=0 3 3 false
330 imaginary_elliptic_cylinder (x^2)/(a^2)+(y^2)/(b^2)=-1 2 3 true true
331 real_elliptic_cylinder (x^2)/(a^2)+(y^2)/(b^2)=1 2 3 true false
332 elliptic_paraboloid z=(x^2)/(a^2)+(y^2)/(b^2) 2 4 false true
333 hyperbolic_cylinder (x^2)/(a^2)-(y^2)/(b^2)=-1 2 3 false
334 hyperbolic_paraboloid z=(y^2)/(b^2)-(x^2)/(a^2) 2 4 true false
335 hyperboloid_of_one_sheet (x^2)/(a^2)+(y^2)/(b^2)-(z^2)/(c^2)=1 3 4 true false
336 hyperboloid_of_two_sheets (x^2)/(a^2)+(y^2)/(b^2)-(z^2)/(c^2)=-1 3 4 false flase
337 imaginary_intersecting_planes (x^2)/(a^2)+(y^2)/(b^2)=0 2 2 true
338 real_intersecting_planes (x^2)/(a^2)-(y^2)/(b^2)=0 2 2 false
339 parabolic_cylinder x^2+2rz=0 1 3
340 imaginary_parallel_planes x^2=-a^2 1 2 true
341 real_parallel planes x^2=a^2 1 2 false
342 */
343 // the table above
344  if(r3==1&&r4==1) type_ = coincident_planes;
345  else if(r3== 3 &&r4== 4 &&gt_0&&signu) type_ = imaginary_ellipsoid;
346  else if(r3== 3 &&r4== 4 &&!gt_0&&signu) type_ = real_ellipsoid;
347  else if(r3== 3 &&r4== 3 &&signu) type_ = imaginary_elliptic_cone;
348  else if(r3== 3 &&r4== 3 &&!signu) type_ = real_elliptic_cone;
349  else if(r3== 2 &&r4== 3 &&signu&&sign) type_ = imaginary_elliptic_cylinder;
350  else if(r3== 2 &&r4== 3 &&signu&&!sign) type_ = real_elliptic_cylinder;
351  else if(r3== 2 &&r4== 4 &&!gt_0&&signu) type_ = elliptic_paraboloid;
352  else if(r3== 2 &&r4== 3 &&!signu) type_ = hyperbolic_cylinder;
353  else if(r3== 2 &&r4== 4 &&gt_0&&!signu) type_ = hyperbolic_paraboloid;
354  else if(r3== 3 &&r4== 4 &&gt_0&&!signu) type_ = hyperboloid_of_one_sheet;
355  else if(r3== 3 &&r4== 4 &&!gt_0&&!signu) type_ = hyperboloid_of_two_sheets;
356  else if(r3== 2 &&r4== 2 &&signu) type_ = imaginary_intersecting_planes;
357  else if(r3== 2 &&r4== 2 &&!signu) type_ = real_intersecting_planes;
358  else if(r3== 1 &&r4== 3 ) type_ = parabolic_cylinder;
359  else if(r3== 1 &&r4== 2 &&sign) type_ = imaginary_parallel_planes;
360  else if(r3== 1 &&r4== 2 &&!sign) type_ = real_parallel_planes;
361 }
362 template <class T>
363 bool vgl_quadric_3d<T>::operator==(vgl_quadric_3d<T> const& that) const
364 {
365  if ( type() != that.type() ) return false;
366  T mag_coefs = (fabs(a_)+fabs(b_)+fabs(c_)+fabs(d_)+fabs(e_)+fabs(f_)+fabs(g_)+fabs(h_)+fabs(i_)+fabs(j_))/T(10);
367  T mag_coefs_that = (fabs(that.a())+fabs(that.b())+fabs(that.c())+fabs(that.d())+fabs(that.e())+fabs(that.f())+fabs(that.g())+fabs(that.h())+fabs(that.i())+fabs(that.j()))/T(10);
368  T tol = vgl_tolerance<T>::position;
369  return fabs(a_*mag_coefs_that - that.a()*mag_coefs)<tol &&
370  fabs(b_*mag_coefs_that - that.b()*mag_coefs)<tol &&
371  fabs(c_*mag_coefs_that - that.c()*mag_coefs)<tol &&
372  fabs(d_*mag_coefs_that - that.d()*mag_coefs)<tol &&
373  fabs(e_*mag_coefs_that - that.e()*mag_coefs)<tol &&
374  fabs(f_*mag_coefs_that - that.f()*mag_coefs)<tol &&
375  fabs(g_*mag_coefs_that - that.g()*mag_coefs)<tol &&
376  fabs(h_*mag_coefs_that - that.h()*mag_coefs)<tol &&
377  fabs(i_*mag_coefs_that - that.i()*mag_coefs)<tol &&
378  fabs(j_*mag_coefs_that - that.j()*mag_coefs)<tol;
379 }
380 template <class T>
381 void vgl_quadric_3d<T>::upper_3x3_eigensystem(std::vector<T>& eigenvalues, std::vector<std::vector<T> >& eigenvectors) const{
382  std::vector<std::vector<T> > Q = this->coef_matrix();
383  T mu[3][3]; T lu[3]; T vcu[3][3];
384  for(size_t r = 0; r<3; ++r)
385  for(size_t c = 0; c<3; ++c)
386  mu[r][c] = Q[r][c];
387 
388  eigen<T, 3>( mu, lu, vcu);
389  for(auto & i : lu){
390  eigenvalues.push_back(i);
391  }
392  eigenvectors.resize(3, std::vector<T>(3, T(0)));
393  for(size_t r = 0; r<3; ++r)
394  for(size_t c = 0; c<3; ++c)
395  eigenvectors[r][c]=vcu[r][c];
396 }
397 template <class T>
398 bool vgl_quadric_3d<T>::canonical_central_quadric(std::vector<T>& diag, std::vector<std::vector<T> >& H) const{
399  diag.resize(4, T(0));
400  H.resize(4, std::vector<T>(4, T(0)));
401  vgl_point_3d<T> cent;
402  bool good = this->center(cent);//the quadric origin
403  if(!good)
404  return false;
405  H[0][3] = cent.x(); H[1][3] = cent.y(); H[2][3] = cent.z();
406  H[3][3] = T(1);
407  // the constant term in the centered coordinate frame
408  T centered_j = j_ + (cent.x()*g_ + cent.y()*h_ + cent.z()*i_)/T(2);
409  // find the upper 3x3 coordinate system
410  std::vector<T> eigenvalues;
411  std::vector<std::vector<T> > eigenvectors;
412  this->upper_3x3_eigensystem(eigenvalues, eigenvectors);
413 
414  for(size_t r = 0; r<3; ++r)
415  for(size_t c = 0; c<3; ++c)
416  H[r][c]=eigenvectors[c][r];
417 
418  for(size_t i = 0; i<3; ++i)
419  diag[i]=eigenvalues[i];
420  diag[3] = centered_j;
421  return true;
422 }
423 template <class T>
424 std::vector<std::vector<T> > vgl_quadric_3d<T>::canonical_quadric(std::vector<std::vector<T> >& H) const{
425  std::vector<std::vector<T> > ret(4, std::vector<T>(4, T(0)));
426  std::vector<T> tr(3, T(0));
427  if(type_ == no_type){
428  std::cout << "Invalid quadric" << std::endl;
429  return ret;
430  }
431  //check first for a central quadric
432  if(type_ == real_ellipsoid || type_==real_elliptic_cone ||
433  type_ == hyperboloid_of_one_sheet || type_ == hyperboloid_of_two_sheets){
434  std::vector<T> diag;
435  if(canonical_central_quadric(diag, H)){
436  for(size_t i = 0; i<4; ++i)
437  ret[i][i] = diag[i];
438  return ret;
439  }else{
440  std::cout << "Shouldn't happen! Inconsistent quadric type assignment " << type_by_number(type_) << std::endl;
441  return ret;
442  }
443  }
444 
445  H.resize(4, std::vector<T>(4, T(0)));
446 
447  // not a central quadric get the eigensystem for the upper 3x3
448  std::vector<T> lambda, sorted_eigenvalues(3,T(0));
449  std::vector<std::vector<T> > E;
450  this->upper_3x3_eigensystem(lambda, E);
451 
452  // to rotate the canonical form back to the original frame
453  for(size_t r = 0; r<3; ++r)
454  for(size_t c = 0; c<3; ++c)
455  H[r][c] = E[c][r];
456  H[3][3] = T(1);
457 
458  for(size_t i = 0; i<3; ++i)
459  sorted_eigenvalues[i]=fabs(lambda[i]);
460  std::sort(sorted_eigenvalues.begin(), sorted_eigenvalues.end(), std::greater<T>());
461  T largest_eigenval = sorted_eigenvalues[0];
462  T rtol = T(RANK_FACTOR)*vgl_tolerance<T>::position;
463  size_t rank = 3;
464  for(size_t i = 0; i<3; ++i)
465  if(fabs(lambda[i]/largest_eigenval) < rtol){
466  lambda[i]=T(0);
467  rank--;
468  }
469  if(rank == 3 || rank == 0){
470  std::cout << "Shouldn't happen! rank == 3 or rank ==0 " << type_by_number(type_) << std::endl;
471  return ret;
472  }
473  // fill in the diagonalized upper 3x3
474  for(size_t i = 0; i<3; ++i)
475  ret[i][i] = lambda[i];
476  //compute transformed quadric matrix g, h, i coefficients gp, hp, ip
477  // where,
478  // gp g
479  // hp = 1/2 E h
480  // ip i
481 
482  T gp = (E[0][0]*g_ + E[0][1]*h_ + E[0][2]*i_)/T(2);
483  T hp = (E[1][0]*g_ + E[1][1]*h_ + E[1][2]*i_)/T(2);
484  T ip = (E[2][0]*g_ + E[2][1]*h_ + E[2][2]*i_)/T(2);
485 
486  std::vector<bool> t_known(3, false);
487  T sum = T(0);
488  if(lambda[0] != T(0)){
489  tr[0] = gp/lambda[0];
490  t_known[0]=true;
491  sum += gp*tr[0];
492  }else{
493  ret[0][3] = gp;
494  ret[3][0] = gp;
495  }
496  if(lambda[1] != T(0)){
497  tr[1] = hp/lambda[1];
498  t_known[1]=true;
499  sum += hp*tr[1];
500  }else{
501  ret[1][3] = hp;
502  ret[3][1] = hp;
503  }
504  if(lambda[2] != T(0)){
505  tr[2] = ip/lambda[2];
506  t_known[2]=true;
507  sum += ip*tr[2];
508  }else{
509  ret[2][3] = ip;
510  ret[3][2] = ip;
511  }
512  // for rank == 2, determine the one remaining
513  // unknown translation component to set j' = 0
514  // consider the possibility that g,h,i, coefficients
515  // are too small to be used (i.e. < rtol)
516  if(rank == 2){
517  if(!t_known[0]){
518  if(fabs(gp) < rtol){
519  ret[3][3] = j_;
520  ret[0][3] = T(0);
521  ret[3][0] = T(0);
522  }else{
523  tr[0] = (j_ - sum)/(T(2)*gp);
524  }
525  }else if(!t_known[1]){
526  if(fabs(hp) < rtol){
527  ret[3][3] = j_;
528  ret[1][3] = T(0);
529  ret[3][1] = T(0);
530  }else{
531  tr[1] = (j_ - sum)/(T(2)*hp);
532  }
533  }else if(!t_known[2]){
534  if(fabs(ip) < rtol){
535  ret[3][3] = j_;
536  ret[2][3] = T(0);
537  ret[3][2] = T(0);
538  }else{
539  tr[2] = (j_ - sum)/(T(2)*ip);
540  }
541  }
542  }
543  // for rank == 1, determine the two translational
544  // degrees of freedom to reduce j to zero
545  // consider the possibility that g,h,i, coefficients
546  // are too small to be used (i.e. < rtol)
547  if(rank == 1){
548  // handle all cases of small coefficients, tedious but necessary
549  if(!t_known[0]&&!t_known[1]){
550  if((fabs(gp) < rtol)&& (fabs(hp) < rtol)){
551  ret[3][3] = j_;
552  }else if((fabs(gp) >= rtol) && (fabs(hp) >= rtol)){
553  T temp = (j_ - sum)/(T(2)*(gp+hp));
554  tr[0] = temp; tr[1] = temp;
555  }else if((fabs(gp) >= rtol)&&(fabs(hp) < rtol)){
556  tr[0] = (j_ - sum)/(T(2)*gp);
557  ret[1][3] = T(0);
558  ret[3][1] = T(0);
559  }else if((fabs(gp) < rtol)&&(fabs(hp) >= rtol)){
560  tr[1] = (j_ - sum)/(T(2)*hp);
561  ret[0][3] = T(0);
562  ret[3][0] = T(0);
563  }
564  }else if(!t_known[0]&&!t_known[2]){
565  if((fabs(gp) < rtol)&& (fabs(ip) < rtol)){
566  ret[3][3] = j_;
567  }else if((fabs(gp) >= rtol) && (fabs(ip) >= rtol)){
568  T temp = (j_ - sum)/(T(2)*(gp+ip));
569  tr[0] = temp; tr[2] = temp;
570  }else if((fabs(gp) >= rtol)&&(fabs(ip) < rtol)){
571  tr[0] = (j_ - sum)/(T(2)*gp);
572  ret[2][3] = T(0);
573  ret[3][2] = T(0);
574  }else if((fabs(gp) < rtol)&&(fabs(ip) >= rtol)){
575  tr[2] = (j_ - sum)/(T(2)*ip);
576  ret[0][3] = T(0);
577  ret[3][0] = T(0);
578  }
579  }else if(!t_known[1]&&!t_known[2]){
580  if((fabs(hp) < rtol) && (fabs(ip) < rtol)){
581  ret[3][3] = j_;
582  }else if((fabs(hp) >= rtol) && (fabs(ip) >= rtol)){
583  T temp = (j_ - sum)/(T(2)*(hp+ip));
584  tr[1] = temp; tr[2] = temp;
585  }else if((fabs(hp) >= rtol) && (fabs(ip) < rtol)){
586  tr[1] = (j_ - sum)/(T(2)*hp);
587  ret[2][3] = T(0);
588  ret[3][2] = T(0);
589  }else if((fabs(hp) < rtol) && (fabs(ip) >= rtol)){
590  tr[2] = (j_ - sum)/(T(2)*ip);
591  ret[1][3] = T(0);
592  ret[3][1] = T(0);
593  }
594  }
595  }
596  H[0][3] = -(E[0][0]*tr[0] + E[1][0]*tr[1] + E[2][0]*tr[2]);
597  H[1][3] = -(E[0][1]*tr[0] + E[1][1]*tr[1] + E[2][1]*tr[2]);
598  H[2][3] = -(E[0][2]*tr[0] + E[1][2]*tr[1] + E[2][2]*tr[2]);
599  return ret;
600 }
601 // The eigensystem computation below is described at
602 //http://www.cap-lore.com/MathPhys/eigen/
603 template <class R, size_t n>
604 void twst(R m[n][n], R c, R s, int i, int j){
605  int k=n;
606  while(k--){
607  R t = m[i][k]*c+ m[j][k]*s;
608  m[j][k] = -s*(m[i][k])+c*(m[j][k]); m[i][k] = t;
609  }
610 }
611 
612 template <class R, size_t n>
613 void eigen(R m[n][n], R l[n], R vc[n][n]){
614 #define db 0
615  R m2[n][n];
616  {int i=n;
617  while(i--) {
618  int j=n;
619  while(j--) {
620  m2[i][j] = m[i][j];
621  vc[i][j] = i==j;
622  }
623  }
624  }
625  while(true){
626  R mod = 0; int i=0, j=0;
627  {int k=n; while(k--){
628  int m=n;
629  while((--m)>k){
630  R q = fabs(m2[k][m]);
631  if(q > mod) {mod=q; i=k; j=m;
632  }
633  }
634  }
635  }
636  if(mod < 0.00000000001) break;
637  // if(db) printf("mii=%e, mjj=%e\n", m2[i][i], m2[j][j]);
638  if(db) std::cout << "mii=" << m2[i][i] << "mjj=" << m2[j][j] << std::endl;
639  R th = 0.5*atan(2*m2[i][j]/(m2[i][i] - m2[j][j]));
640  // if(db) printf("th=%e, i=%d, j=%d\n", th, i, j);
641  if(db) std::cout << "th=" << th << "i" << i << "j=" << j << std::endl;
642  {
643  R c = cos(th), s = sin(th);
644  {int k=n; while(k--){
645  R t = c*m2[k][i] + s*m2[k][j];
646  m2[k][j] = -s*m2[k][i] + c*m2[k][j]; m2[k][i]=t;
647  }
648  }
649  twst<R,n>( m2, c, s, i, j);
650  twst<R,n>( vc, c, s, i, j);
651  }
652  }
653  {
654  int j=n;
655  while(j--) l[j] = m2[j][j];
656  }
657 }
658 
659 template <class T>
660 std::ostream& operator<<(std::ostream& os, const vgl_quadric_3d<T >& q){
661  os << "vgl_quadric_3d: a=" << q.a() << " b=" << q.b() << " c=" << q.c() << " d="
662  << q.d() << " e=" << q.e()<< " f=" << q.f() << " g=" << q.g()
663  << " h=" << q.h() << " i=" << q.i() << " j=" << q.j() << std::endl;
664  return os;
665 }
666 
667 template <class T>
668 std::istream& operator>>(std::istream& ist, vgl_quadric_3d<T >& q){
669  T a,b,c,d,e,f,g,h,i,j;
670  ist >> a >> b >> c >> d >> e >> f >> g >> h >> i >> j;
671  q.set(a, b, c, d, e, f, g, h , i, j);
672  return ist;}
673 
674 #undef VGL_QUADRIC_3D_INSTANTIATE
675 #define VGL_QUADRIC_3D_INSTANTIATE(T) \
676 template class vgl_quadric_3d<T>; \
677 template std::ostream& operator<<(std::ostream&, const vgl_quadric_3d<T>&); \
678 template std::istream& operator>>(std::istream&, vgl_quadric_3d<T>&)
679 #endif //vgl_quadric
twst
void twst(R m[n][n], R c, R s, int i, int j)
Definition: vgl_quadric_3d.hxx:604
vgl_quadric_3d::h
T h() const
Returns the coefficient of .
Definition: vgl_quadric_3d.h:104
vgl_quadric_3d::operator==
bool operator==(vgl_quadric_3d< T > const &c) const
comparison operator.
Definition: vgl_quadric_3d.hxx:363
db
#define db
vgl_quadric_3d::vgl_quadric_type
vgl_quadric_type
Definition: vgl_quadric_3d.h:35
vgl_quadric_3d::b
T b() const
Returns the coefficient of .
Definition: vgl_quadric_3d.h:86
vgl_quadric_3d::center
bool center(vgl_point_3d< T > &center) const
if the upper 3x3 submatrix of Q is full rank then the center of the quadric can be defined.
Definition: vgl_quadric_3d.hxx:166
vgl_quadric_3d::vgl_quadric_3d
vgl_quadric_3d()
Definition: vgl_quadric_3d.h:114
vgl_tolerance
Definition: vgl_tolerance.h:12
vgl_quadric_3d::type
vgl_quadric_type type() const
Definition: vgl_quadric_3d.h:73
RANK_FACTOR
#define RANK_FACTOR
Definition: vgl_quadric_3d.hxx:9
vgl_quadric_3d::on
bool on(vgl_homg_point_3d< T > const &pt, T tol=T(0)) const
Returns true if the point pt belongs to the quadric surface.
Definition: vgl_quadric_3d.hxx:159
vgl_quadric_3d::c
T c() const
Returns the coefficient of .
Definition: vgl_quadric_3d.h:89
vgl_quadric_3d::j
T j() const
Returns the coefficient of .
Definition: vgl_quadric_3d.h:110
vgl_quadric_3d::coef_matrix
std::vector< std::vector< T > > coef_matrix() const
return a matrix of quadric coefficients of the form:.
Definition: vgl_quadric_3d.hxx:139
vgl_quadric_3d::upper_3x3_eigensystem
void upper_3x3_eigensystem(std::vector< T > &eigenvalues, std::vector< std::vector< T > > &eigenvectors) const
: eigenvalues and eigenvectors of the upper 3x3 quadric matrix.
Definition: vgl_quadric_3d.hxx:381
vgl_point_3d< T >
vgl_quadric_3d::canonical_quadric
std::vector< std::vector< T > > canonical_quadric(std::vector< std::vector< T > > &H) const
The quadric coefficient matrix in the canonical frame, whether or not the quadric is central.
Definition: vgl_quadric_3d.hxx:424
vgl_box_3d::operator<<
std::ostream & operator<<(std::ostream &s, vgl_orient_box_3d< Type > const &p)
Write box to stream.
vgl_homg_point_3d
Represents a homogeneous 3D point.
Definition: vgl_fwd.h:9
vgl_point_3d::z
Type & z()
Definition: vgl_point_3d.h:73
vgl_quadric_3d.h
A 2nd order algebraic surface in 3-d.
vgl_quadric_3d::type_by_name
static vgl_quadric_type type_by_name(std::string const &name)
Returns the internal enum value corresponding to the string argument.
Definition: vgl_quadric_3d.hxx:36
vgl_quadric_3d::e
T e() const
Returns the coefficient of .
Definition: vgl_quadric_3d.h:95
eigen
void eigen(R m[n][n], R l[n], R vc[n][n])
Definition: vgl_quadric_3d.hxx:613
vgl_homg_point_3d::z
Type z() const
Definition: vgl_homg_point_3d.h:87
vgl_quadric_3d::set
void set(T a, T b, T c, T d, T e, T f, T g, T h, T i, T j)
set or reset the quadric using polynomial coefficients.
Definition: vgl_quadric_3d.hxx:127
DET_FACTOR
#define DET_FACTOR
Definition: vgl_quadric_3d.hxx:10
vgl_homg_point_3d::w
Type w() const
Definition: vgl_homg_point_3d.h:88
vgl_quadric_3d::sampson_dist
T sampson_dist(vgl_homg_point_3d< T > const &pt) const
Sampson distance d_sampson(p) = ||p^t Q p||^2/||grad(p*t Q p)||^2.
Definition: vgl_quadric_3d.hxx:148
vgl_quadric_3d::f
T f() const
Returns the coefficient of .
Definition: vgl_quadric_3d.h:98
vgl_point_3d::x
Type & x()
Definition: vgl_point_3d.h:71
vgl_quadric_3d::d
T d() const
Returns the coefficient of .
Definition: vgl_quadric_3d.h:92
vgl_tolerance.h
vgl_box_3d::operator>>
std::istream & operator>>(std::istream &is, vgl_orient_box_3d< Type > &p)
Read box from stream.
vgl_quadric_3d::compute_type
void compute_type()
set quadric type from polynomial coefficients and store in member type_.
Definition: vgl_quadric_3d.hxx:205
l
#define l
Definition: vgl_homg_line_2d.h:119
vgl_quadric_3d
Definition: vgl_quadric_3d.h:32
vgl_point_3d::set
void set(Type px, Type py, Type pz)
Set x, y and z.
Definition: vgl_point_3d.h:81
vgl_quadric_3d::a
T a() const
Returns the coefficient of .
Definition: vgl_quadric_3d.h:83
vgl_homg_point_3d::y
Type y() const
Definition: vgl_homg_point_3d.h:86
vgl_quadric_3d::g
T g() const
Returns the coefficient of .
Definition: vgl_quadric_3d.h:101
vgl_homg_point_3d::x
Type x() const
Definition: vgl_homg_point_3d.h:85
vgl_quadric_3d::i
T i() const
Returns the coefficient of .
Definition: vgl_quadric_3d.h:107
vgl_quadric_3d::canonical_central_quadric
bool canonical_central_quadric(std::vector< T > &diag, std::vector< std::vector< T > > &H) const
: The quadric in its canonical frame if the center is defined, i.e. the upper 3x3 quadric matrix is f...
Definition: vgl_quadric_3d.hxx:398
vgl_quadric_3d::type_by_number
static std::string type_by_number(vgl_quadric_type const &type)
Converts the quadric type from enum (internal representation) to string.
Definition: vgl_quadric_3d.hxx:33
vgl_point_3d::y
Type & y()
Definition: vgl_point_3d.h:72
Generated by   doxygen 1.8.15