vgl_fit_quadric_3d.hxx
Go to the documentation of this file.
1 // This is core/vgl/algo/vgl_fit_quadric_3d.hxx
2 #ifndef vgl_fit_quadric_3d_hxx_
3 #define vgl_fit_quadric_3d_hxx_
4 //:
5 // \file
6 #include <limits>
7 #include <iostream>
8 #include <vnl/vnl_math.h>
9 #include "vgl_fit_quadric_3d.h"
10 #include <vgl/algo/vgl_norm_trans_3d.h>
11 #include <vnl/algo/vnl_svd.h>
12 #include <vnl/vnl_matrix.h>
13 #ifdef _MSC_VER
14 # include <vcl_msvc_warnings.h>
15 #endif
16 #include <vgl/vgl_distance.h>
17 #include <vgl/vgl_tolerance.h>
18 #include <vnl/algo/vnl_levenberg_marquardt.h>
19 #include <vnl/algo/vnl_generalized_eigensystem.h>
20 #include <vnl/algo/vnl_real_eigensystem.h>
21 #include <vnl/algo/vnl_symmetric_eigensystem.h>
22 #include <vnl/vnl_least_squares_function.h>
23 #define debug 0
24 
25 template <class T>
26 vgl_fit_quadric_3d<T>::vgl_fit_quadric_3d(std::vector<vgl_point_3d<T> > points)
27 
28 {
29  for(typename std::vector<vgl_point_3d<T> >::iterator pit = points.begin();
30  pit != points.end(); ++pit)
31  points_.push_back(vgl_homg_point_3d<T>(*pit));
32 }
33 
34 template <class T>
35 void vgl_fit_quadric_3d<T>::add_point(vgl_point_3d<T> const &p)
36 {
37  points_.push_back(vgl_homg_point_3d<T>(p));
38 }
39 
40 template <class T>
41 void vgl_fit_quadric_3d<T>::add_point(const T x, const T y, const T z)
42 {
43  points_.push_back(vgl_homg_point_3d<T>(x, y, z));
44 }
45 
46  template <class T>
47  void vgl_fit_quadric_3d<T>::clear()
48 {
49  points_.clear();
50 }
51 
52 template <class T>
53 std::vector<vgl_point_3d<T> > vgl_fit_quadric_3d<T>::get_points() const{
54  std::vector<vgl_point_3d<T> > ret;
55  const unsigned n = static_cast<unsigned>(points_.size());
56  for (unsigned i=0; i<n; i++){
57  vgl_point_3d<T> p(points_[i]);
58  ret.push_back(p);
59  }
60  return ret;
61 }
62 
63 template <class T>
64 T vgl_fit_quadric_3d<T>::fit_linear_Taubin(std::ostream* errstream)
65 {
66  size_t n = points_.size();
67  // actually the minimum number is 9 but require at least 10 to get some amount of regularization
68  if(n<10){
69  if (errstream)
70  *errstream << "Insufficient number of points to fit quadric " << n << std::endl;
71  return T(-1);
72  }
73  // normalize the points
74  vgl_norm_trans_3d<T> norm;
75  if (!norm.compute_from_points(points_) && errstream) {
76  *errstream << "there is a problem with norm transform\n";
77  return T(-1);
78  }
79 
80  vnl_matrix<double> M(10,10, 0.0), N(10,10, 0.0);
81  for (size_t i=0; i<n; i++) {
82  vgl_homg_point_3d<T> hp = norm(points_[i]);//normalize
83  // ax^2 + by^2 + cz^2 + dxy + exz + fyz + gxw +hyw +izw +jw^2 = 0
84  // let q = {a , b , c , d , e , f , g ,h ,i ,j}
85  // let p = {x^2 , y^2 , z^2 , xy , xz , yz , xw , yw , zw, w^2}
86  // then q^t p = algebraic distance
87  vnl_matrix<double> p(10, 1), dx(10,1,0.0), dy(10,1,0.0), dz(10,1,0.0);
88  double x = static_cast<double>(hp.x()), y = static_cast<double>(hp.y()),
89  z = static_cast<double>(hp.z()), w = static_cast<double>(hp.w());
90  p[0][0]= x*x; p[1][0]=y*y; p[2][0]=z*z; p[3][0]=x*y; p[4][0]=x*z;
91  p[5][0]= y*z; p[6][0]=x*w; p[7][0]=y*w; p[8][0]=z*w; p[9][0]=w*w;
92  dx[0][0] = 2.0*x; dx[3][0] = y; dx[4][0] = z; dx[6][0]=w;
93  dy[1][0] = 2.0*y; dy[3][0] = x; dy[5][0] = z; dy[7][0]=w;
94  dz[2][0] = 2.0*z; dz[4][0] = x; dz[5][0] = y; dz[8][0]=w;
95  M += p*(p.transpose());
96  N += (dx*(dx.transpose()) + dy*(dy.transpose()) + dz*(dz.transpose()));
97  }
98  M/=n; N/=n;
99 
100  // add a small offset to make N positive definite
101  double tol = 100.0*vgl_tolerance<double>::position;
102  N[9][9] = tol;
103 
104  // solve the generalized eigenvalue problem (M-lambda*N)q=0
105  vnl_generalized_eigensystem gev(M, N);
106 
107  // extract the quadric coefficients
108  vnl_vector<T> q(10);
109  // The solution, q, is the eigenvector corresponding to the
110  // minimum eigenvalue (index = 0);
111  for(size_t r = 0; r<10; ++r)
112  q[r] = static_cast<T>((gev.V)[r][0]);
113 
114 
115  if(debug) std::cout << "q\n" << q << std::endl;
116 
117  // Transform the quadric back to the original coordinate frame
118  quadric_Taubin_.set(q[0],q[1],q[2],q[3],q[4],q[5],q[6],q[7],q[8],q[9]);
119  std::vector<std::vector<T> > qq = quadric_Taubin_.coef_matrix();
120  vnl_matrix<T> Q(4,4);
121  for(size_t r = 0; r<4; ++r)
122  for(size_t c = 0; c<4; ++c)
123  Q[r][c] = qq[r][c];
124  vnl_matrix<T> Tr = norm.get_matrix();
125  vnl_matrix<T> Qorig(4, 4);
126  Qorig = Tr.transpose() * Q * Tr;
127  for(size_t r = 0; r<4; ++r)
128  for(size_t c = 0; c<4; ++c)
129  qq[r][c] = Qorig[r][c];
130 
131  quadric_Taubin_.set(qq);
132  if(debug) std::cout << quadric_Taubin_.type_by_number(quadric_Taubin_.type()) <<std::endl;
133 
134  // compute average sampson distance error for the original point set
135  T dsum = 0.0;
136  for (unsigned i=0; i<n; i++) {
137  T d = quadric_Taubin_.sampson_dist(points_[i]);
138  dsum += d;
139  }
140  return static_cast<T>(dsum/n);
141 }
142 
143 template <class T>
144 T vgl_fit_quadric_3d<T>::fit_ellipsoid_linear_Allaire(std::ostream* errstream){
145  size_t n = points_.size();
146  // actually the minimum number is 9 but require at least 10 to get some amount of regularization
147  if(n<10){
148  if (errstream)
149  *errstream << "Insufficient number of points to fit quadric " << n << std::endl;
150  return T(-1);
151  }
152  // normalize the points
153  vgl_norm_trans_3d<T> norm;
154  if (!norm.compute_from_points(points_) && errstream) {
155  *errstream << "there is a problem with norm transform\n";
156  return T(-1);
157  }
158 
159  vnl_matrix<double> M(10,10, 0.0), N(10,10, 0.0);
160  for (size_t i=0; i<n; i++) {
161  vgl_homg_point_3d<T> hp = norm(points_[i]);//normalize
162  // ax^2 + by^2 + cz^2 + dxy + exz + fyz + gxw +hyw +izw +jw^2 = 0
163  // let q = {a , b , c , d , e , f , g ,h ,i ,j}
164  // let p = {x^2 , y^2 , z^2 , xy , xz , yz , xw , yw , zw, w^2}
165  // then q^t p = algebraic distance
166  vnl_matrix<double> p(10, 1);
167  double x = static_cast<double>(hp.x()), y = static_cast<double>(hp.y()),
168  z = static_cast<double>(hp.z()), w = static_cast<double>(hp.w());
169  p[0][0]= x*x; p[1][0]=y*y; p[2][0]=z*z; p[3][0]=x*y; p[4][0]=x*z;
170  p[5][0]= y*z; p[6][0]=x*w; p[7][0]=y*w; p[8][0]=z*w; p[9][0]=w*w;
171  M += p*(p.transpose());
172  }
173  M/=n;
174  // condition the M matrix to be positive definite
175  double tol = 100.0*vgl_tolerance<double>::position;
176  for(size_t i = 0; i<10; ++i)
177  M[i][i]+=tol;
178  // for ellipsoid the constraint is as follows:
179  // let the coeficient matrix
180  // _ _
181  // Q = |U b|
182  // |b^t j|
183  // - -
184  // so that x^t Q x + bx + j = 0 is the general equation of a quadric
185  // The sum of eigenvalues of U is given by Tr(U)
186  // the product of eigenvalues by |U| (determinant)
187  // and the sum of pairwise eigenvalue products by SU2x2 = U2x2_0 + U2x2_1 + U2x2_2
188  // where U2x2_i are the principal minors of U.
189  //
190  // Allaire et al show that the constraint 4*SU2x2 - Tr(U) = 1
191  // guarantees that one of the solutions below corresponds to an ellipsoid.
192 
193  // this constraint is equivalent to q^t N q = 1 where,
194  N[0][0]= -1.0; N[0][1]= 1.0; N[0][2]= 1.0;
195  N[1][0]= 1.0; N[1][1]= -1.0; N[1][2]= 1.0;
196  N[2][0]= 1.0; N[2][1]= 1.0; N[2][2]= -1.0;
197  N[3][3]= -1.0; N[4][4]= -1.0; N[5][5]= -1.0;
198 
199  // N has eigenvalues [-1 -1 -1 -1 -1 0 0 0 0 0 1], i.e singular
200  // The eigenvalues of the solution eigenvectors will have the same signs
201 
202  // since N is singular it is necessary to solve the
203  // generalized eigensystem (N - lambda M)q = 0
204  // in this case the solution is the eigenvector which corresponds to an ellipsoid
205  // and has the least normalized error, q^t M q / q^t N q .
206  vnl_generalized_eigensystem geign(N, M);
207 
208  // extract the quadric coefficients
209  size_t min_index = 10;
210  vnl_matrix<double> q(10,1);
211  vnl_diag_matrix<double> D = geign.D;
212  double min_error = std::numeric_limits<double>::max();
213  for(size_t i = 0; i<5; ++i){
214  for(size_t r = 0; r<10; ++r)
215  q[r][0] = static_cast<T>((geign.V)[r][i])/sqrt(fabs(D[i]));
216 
217  quadric_Allaire_.set(q[0][0],q[1][0],q[2][0],q[3][0],q[4][0],q[5][0],q[6][0],q[7][0],q[8][0],q[9][0]);
218  if(quadric_Allaire_.type() != vgl_quadric_3d<T>::real_ellipsoid)
219  continue;
220  vnl_matrix<double> neu = q.transpose() * M * q;
221  vnl_matrix<double> den = q.transpose() * N * q;
222  double e = fabs(neu[0][0])/fabs(den[0][0]);
223  if(debug) std::cout << "Error =" << e << "for q[" << i << "]\n" << q << std::endl;
224  if(e<min_error){
225  min_index = i;
226  min_error = e;
227  }
228  }
229  for(size_t r = 0; r<10; ++r)
230  q[r][0] = static_cast<T>((geign.V)[r][9])/sqrt(fabs(D[9]));
231 
232  quadric_Allaire_.set(q[0][0],q[1][0],q[2][0],q[3][0],q[4][0],q[5][0],q[6][0],q[7][0],q[8][0],q[9][0]);
233  if(quadric_Allaire_.type() == vgl_quadric_3d<T>::real_ellipsoid){
234  vnl_matrix<double> neu = q.transpose() * M * q;
235  vnl_matrix<double> den = q.transpose() * N * q;
236  double e = fabs(neu[0][0])/fabs(den[0][0]);
237  if(debug) std::cout << "Error =" << e << " for q[" << 9 << "]\n" << q << std::endl;
238  if(e<min_error){
239  min_error = e;
240  min_index = 9;
241  }
242  }
243  if(min_index == 10){
244  std::cout << "failed to find ellipsoid" << std::endl;
245  return T(-1);
246  }
247  if(debug) std::cout << "Best ellipsoid is " << min_index << std::endl;
248  for(size_t r = 0; r<10; ++r)
249  q[r][0] = static_cast<T>((geign.V)[r][min_index])/sqrt(fabs(D[min_index]));
250 
251  // Transform the quadric back to the original coordinate frame
252  quadric_Allaire_.set(q[0][0],q[1][0],q[2][0],q[3][0],q[4][0],q[5][0],q[6][0],q[7][0],q[8][0],q[9][0]);
253  std::vector<std::vector<T> > qq = quadric_Allaire_.coef_matrix();
254  vnl_matrix<T> Q(4,4);
255  for(size_t r = 0; r<4; ++r)
256  for(size_t c = 0; c<4; ++c)
257  Q[r][c] = qq[r][c];
258  vnl_matrix<T> Tr = norm.get_matrix();
259  vnl_matrix<T> Qorig(4, 4);
260  Qorig = Tr.transpose() * Q * Tr;
261  for(size_t r = 0; r<4; ++r)
262  for(size_t c = 0; c<4; ++c)
263  qq[r][c] = Qorig[r][c];
264 
265  quadric_Allaire_.set(qq);
266  std::cout << quadric_Allaire_.type_by_number(quadric_Allaire_.type()) <<std::endl;
267 
268  // compute average sampson distance error for the original point set
269  T dsum = 0.0;
270  for (unsigned i=0; i<n; i++) {
271  T d = quadric_Allaire_.sampson_dist(points_[i]);
272  dsum += d;
273  }
274  return static_cast<T>(dsum/n);
275 }
276 template <class T>
277 T vgl_fit_quadric_3d<T>::fit_saddle_shaped_quadric_linear_Allaire(std::ostream* errstream){
278  size_t n = points_.size();
279  // actually the minimum number is 9 but require at least 10 to get some amount of regularization
280  if(n<10){
281  if (errstream)
282  *errstream << "Insufficient number of points to fit quadric " << n << std::endl;
283  return T(-1);
284  }
285  // normalize the points
286  vgl_norm_trans_3d<T> norm;
287  if (!norm.compute_from_points(points_) && errstream) {
288  *errstream << "there is a problem with norm transform\n";
289  return T(-1);
290  }
291 
292  vnl_matrix<double> M(10,10, 0.0), N(10,10, 0.0);
293  for (size_t i=0; i<n; i++) {
294  vgl_homg_point_3d<T> hp = norm(points_[i]);//normalize
295  // ax^2 + by^2 + cz^2 + dxy + exz + fyz + gxw +hyw +izw +jw^2 = 0
296  // let q = {a , b , c , d , e , f , g ,h ,i ,j}
297  // let p = {x^2 , y^2 , z^2 , xy , xz , yz , xw , yw , zw, w^2}
298  // then q^t p = algebraic distance
299  vnl_matrix<double> p(10, 1);
300  double x = static_cast<double>(hp.x()), y = static_cast<double>(hp.y()),
301  z = static_cast<double>(hp.z()), w = static_cast<double>(hp.w());
302  p[0][0]= x*x; p[1][0]=y*y; p[2][0]=z*z; p[3][0]=x*y; p[4][0]=x*z;
303  p[5][0]= y*z; p[6][0]=x*w; p[7][0]=y*w; p[8][0]=z*w; p[9][0]=w*w;
304  M += p*(p.transpose());
305  }
306  M/=n;
307  // condition the M matrix to be positive definite
308  double tol = 100.0*vgl_tolerance<double>::position;
309  for(size_t i = 0; i<10; ++i)
310  M[i][i]+=tol;
311 
312  // for saddle shaped quadrics the constraint is as follows:
313  // let the coeficient matrix
314  // _ _
315  // Q = |U b|
316  // |b^t j|
317  // - -
318  // so that x^t Q x + bx + j = 0 is the general equation of a quadric
319  // The sum of eigenvalues of U is given by Tr(U)
320  // the product of eigenvalues by |U| (determinant)
321  // and the sum of pairwise eigenvalue products by SU2x2 = U2x2_0 + U2x2_1 + U2x2_2
322  // where U2x2_i are the principal minors of U.
323  //
324  // Allaire et al show that the constraint SU2x2 = -1 and q^t N q = -1
325  // guarantees that the solution with a negative eigenvalue will fall in the
326  // class of saddle shaped surfaces
327 
328  // the quadradic normalizing constraint is equivalent to q^t N q = -1 where,
329  N[0][0]= 0.0; N[0][1]= 0.5; N[0][2]= 0.5;
330  N[1][0]= 0.5; N[1][1]= 0.0; N[1][2]= 0.5;
331  N[2][0]= 0.5; N[2][1]= 0.5; N[2][2]= 0.0;
332  N[3][3]= -0.25; N[4][4]= -0.25; N[5][5]= -0.25;
333 
334  // N has eigenvalues [-0.5 -0.5 -0.25 -0.25 -0.25 0 0 0 0 1], i.e singular
335  // The eigenvalues of the generalized eigensystem will have the same signs
336 
337  // since N is singular it is necessary to solve the
338  // generalized eigensystem (N - lambda M)q = 0
339  // in this case the solution is the eigenvector with a
340  // negative eigenvalue and with the least error q^t M q / q^t N q .
341  vnl_generalized_eigensystem geign(N, M);
342  if(debug) std::cout << geign.D << std::endl;
343  if(debug) std::cout << geign.V << std::endl;
344 
345  // extract the quadric coefficients
346  size_t min_index = 10;
347  vnl_matrix<double> q(10,1);
348  vnl_diag_matrix<double> D = geign.D;
349  double min_error = std::numeric_limits<double>::max();
350  for(size_t i = 0; i<5; ++i){
351  for(size_t r = 0; r<10; ++r)
352  q[r][0] = (geign.V)[r][i]/sqrt(fabs(D[i]));
353 
354  quadric_Allaire_.set(q[0][0],q[1][0],q[2][0],q[3][0],q[4][0],q[5][0],q[6][0],q[7][0],q[8][0],q[9][0]);
355  vnl_matrix<double> neu = q.transpose() * M * q;
356  vnl_matrix<double> den = q.transpose() * N * q;
357  double e = fabs(neu[0][0])/fabs(den[0][0]);
358  if(debug) std::cout << "Error =" << e << "for q[" << i << "]\n" << q << std::endl;
359  if(debug) std::cout << "Type "<< quadric_Allaire_.type_by_number(quadric_Allaire_.type()) <<std::endl;
360  if(e<min_error){
361  min_index = i;
362  min_error = e;
363  }
364  }
365  if(min_index == 10){
366  std::cout << "failed to find saddle shape" << std::endl;
367  return T(-1);
368  }
369  if(debug) std::cout << "Best saddle shape is " << min_index << std::endl;
370  for(size_t r = 0; r<10; ++r)
371  q[r][0] =(geign.V)[r][min_index]/sqrt(fabs(D[min_index]));
372 
373  // Transform the quadric back to the original coordinate frame
374  quadric_Allaire_.set(q[0][0],q[1][0],q[2][0],q[3][0],q[4][0],q[5][0],q[6][0],q[7][0],q[8][0],q[9][0]);
375  std::vector<std::vector<T> > qq = quadric_Allaire_.coef_matrix();
376  vnl_matrix<T> Q(4,4);
377  for(size_t r = 0; r<4; ++r)
378  for(size_t c = 0; c<4; ++c)
379  Q[r][c] = qq[r][c];
380  vnl_matrix<T> Tr = norm.get_matrix();
381  vnl_matrix<T> Qorig(4, 4);
382  Qorig = Tr.transpose() * Q * Tr;
383  for(size_t r = 0; r<4; ++r)
384  for(size_t c = 0; c<4; ++c)
385  qq[r][c] = Qorig[r][c];
386 
387  quadric_Allaire_.set(qq);
388  std::cout << quadric_Allaire_.type_by_number(quadric_Allaire_.type()) <<std::endl;
389 
390  // compute average sampson distance error for the original point set
391  T dsum = 0.0;
392  for (unsigned i=0; i<n; i++) {
393  T d = quadric_Allaire_.sampson_dist(points_[i]);
394  dsum += d;
395  }
396  return static_cast<T>(dsum/n);
397 }
398 
399 //--------------------------------------------------------------------------
400 #undef VGL_FIT_QUADRIC_3D_INSTANTIATE
401 #define VGL_FIT_QUADRIC_3D_INSTANTIATE(T) \
402 template class vgl_fit_quadric_3d<T >
403 
404 #endif // vgl_fit_quadric_3d_hxx_
vgl_tolerance
Definition: vgl_tolerance.h:12
vgl_fit_quadric_3d::clear
void clear()
clear internal data.
Definition: vgl_fit_quadric_3d.hxx:47
vgl_fit_quadric_3d.h
Fits a quadric surface to a set of 3D points.
vgl_fit_quadric_3d::get_points
std::vector< vgl_point_3d< T > > get_points() const
Definition: vgl_fit_quadric_3d.hxx:53
vgl_point_3d< T >
vgl_homg_point_3d
Represents a homogeneous 3D point.
Definition: vgl_fwd.h:9
vgl_fit_quadric_3d::vgl_fit_quadric_3d
vgl_fit_quadric_3d()=default
vgl_homg_point_3d::z
Type z() const
Definition: vgl_homg_point_3d.h:87
vgl_norm_trans_3d::compute_from_points
bool compute_from_points(std::vector< vgl_homg_point_3d< T > > const &points)
compute the normalizing transform.
Definition: vgl_norm_trans_3d.hxx:76
vgl_norm_trans_3d
Definition: vgl_algo_fwd.h:15
vgl_fit_quadric_3d::fit_ellipsoid_linear_Allaire
T fit_ellipsoid_linear_Allaire(std::ostream *outstream=nullptr)
fit an ellipsoid using the linear Allaire method.
Definition: vgl_fit_quadric_3d.hxx:144
vgl_fit_quadric_3d::fit_saddle_shaped_quadric_linear_Allaire
T fit_saddle_shaped_quadric_linear_Allaire(std::ostream *outstream=nullptr)
fit a quadric class from the set{ hyperboloid_of_one_sheets,.
Definition: vgl_fit_quadric_3d.hxx:277
vgl_homg_point_3d::w
Type w() const
Definition: vgl_homg_point_3d.h:88
debug
#define debug
Definition: vgl_fit_quadric_3d.hxx:23
vgl_fit_quadric_3d::add_point
void add_point(vgl_point_3d< T > const &p)
add a point to point set.
Definition: vgl_fit_quadric_3d.hxx:35
vgl_h_matrix_3d::get_matrix
vnl_matrix_fixed< T, 4, 4 > const & get_matrix() const
Return the 4x4 homography matrix.
Definition: vgl_h_matrix_3d.h:106
vgl_tolerance.h
vgl_quadric_3d
Definition: vgl_quadric_3d.h:32
vgl_fit_quadric_3d::fit_linear_Taubin
T fit_linear_Taubin(std::ostream *outstream=nullptr)
fit a general quadric to the stored points using the linear Taubin method.
Definition: vgl_fit_quadric_3d.hxx:64
vgl_distance.h
Set of distance functions.
vgl_homg_point_3d::y
Type y() const
Definition: vgl_homg_point_3d.h:86
vgl_norm_trans_3d.h
The similarity transform that normalizes a 3-d point set.
vgl_homg_point_3d::x
Type x() const
Definition: vgl_homg_point_3d.h:85
Generated by   doxygen 1.8.15