1 // This is core/vgl/algo/vgl_conic_2d_regression.h
2 #ifndef vgl_conic_2d_regression_h_
3 #define vgl_conic_2d_regression_h_
4 //:
5 // \file
6 // \brief Fits a conic to a set of points using linear regression
7 // \author J.L. Mundy
8 // \date June 17, 2005
9 //
10 //  Since conic fitting is rather ill-conditioned it is necessary to
11 //  normalize the point coordinates so that they have equal standard
12 //  deviations with respect to the center (mean) of the pointset.
13 //  The points are transformed to have zero mean. The resulting conic
14 //  is transformed back to the original frame for computing
15 //  the Sampson approximation to fitting error.
16 //
17 //  The regression uses the Bookstein algorithm which constrains the
18 //  conic norm, $ |ax^2 + bxy + cy^2 + dx + ey +f|^2 $, so that
19 //   $ a^2 + 0.5*b^2 + c^2 = 2 $
20 //  With this normalization, the resulting fit is invariant up to a similarity
21 //  transform of the pointset.  The solution is formulated as an eigenvalue
22 //  problem as follows:
23 //
24 //      The scatter matrix S is decomposed as
25 //
26 //      $ S   =  \begin{array}{cc}     S_{11} & S_{12}
27 //                              \\     S_{21} & S_{22}     \end{array}$
28 //      note $S_{21} = S_{12}^t$
29 //
30 //      The conic coefficients are $v_1 = \{a,b,c\}^t$ and $v_2 = \{d,e,f\}^t$
31 //      The Bookstein constraint is expressed by the diagonal matrix
32 //               D = Diag{1, 0.5, 1, 0, 0, 0}
33 //      The Lagrangian to be minimized is
34 //      $ L =  v_1^t S_{11} v_1  + 2 v_2^t S_{21} v_1  + v_2^t S_{22} v_2
35 //             - \lambda(v_1^t D v_1 -2) $.
36 //
37 //      Minimizing with respect to v2 gives
38 //      $ dL/dv_2 = 2 S_{21} v_1 + 2 S_{22} v_2 = 0 $
39 //
40 //      So,  $ v_2 = -S_{22}^-1 S_{21} v_1 $.
41 //
42 //      Substituting for v2 in L,
43 //
44 // $ L = v_1^t ( S_{11} - S_{12} * S_{22}^-1 * S_{21}) v_1 - \lambda(v_1^t D v_1 -2)
45 //     = v_1^t ( (S_{11} - S_{12} * S_{22}^-1 * S_{21}) - \lambda D ) v_1 - 2 \lambda $,
46 //
47 // $ dL/dv_1 =  ( (S_{11} - S_{12} * S_{22}^-1 * S_{21}) - \lambda D ) v_1 = 0 $.
48 //
49 //   So, $ \lambda v_1 = D^-1 (S_{11} - S_{12} * S_{22}^-1 * S_{21}) v_1 $.
50 //
51 //   This eigenvalue problem is solved using singular value decomposition.
52 //
53 // \verbatim
54 //  Modifications
55 //   none
56 // \endverbatim
57 
58 #include <vector>
59 #include <iostream>
60 #ifdef _MSC_VER
61 #  include <vcl_msvc_warnings.h>
62 #endif
63 #include <vnl/vnl_matrix_fixed.h>
64 #include <vgl/vgl_point_2d.h>
65 #include <vgl/vgl_homg_point_2d.h>
66 #include <vgl/vgl_conic.h>
67 #include <vgl/algo/vgl_norm_trans_2d.h>
68 
69 template <class T>
70 class vgl_conic_2d_regression
71 {
72  public:
73 
74   // Constructors/Initializers/Destructors-------------------------------------
75 
76   vgl_conic_2d_regression();
77 
78   ~vgl_conic_2d_regression()= default;
79 
80   // Operations----------------------------------------------------------------
81 
82 
83   //: Number of regression points
get_n_pts()84   unsigned get_n_pts() const {return npts_;}
85 
86   //: clear the regression data
87   void clear_points();
88 
89   void add_point(vgl_point_2d<T> const& p);
90 
91   void remove_point(vgl_point_2d<T> const& p);
92 
93   //: get the estimated Euclidean error with respect to the fitted conic segment in the original frame
94   T get_rms_error_est(vgl_point_2d<T> const& p) const;
95 
96   //: get the current Euclidean fitting error in the original frame
get_rms_sampson_error()97   T get_rms_sampson_error() const { return sampson_error_; } //temporarily
98 
99   //: get the current algebraic fitting error in the normalized frame
100   T get_rms_algebraic_error() const;
101 
102   //: the fitting method
103   bool fit();
104 
105   // Data Access---------------------------------------------------------------
106 
conic()107   vgl_conic<T> conic() const { return conic_; }
108 
109   // Debug support
110   void print_pointset(std::ostream& str = std::cout );
111  protected:
112   // Internals
113   void init();
114   void compute_partial_sums();
115   void fill_scatter_matrix();
116   void set_sampson_error(T a, T b, T c, T d, T e, T f);
117 
118   // Data Members--------------------------------------------------------------
119 
120   //: The current set of points
121   std::vector<vgl_point_2d<T> > points_;
122 
123   //: Size of point set
124   unsigned npts_;
125 
126   //: The normalizing transformation
127   vgl_norm_trans_2d<T> trans_;
128 
129   //: The partial scatter term sums, updated with each ::add_point
130   std::vector<T> partial_sums_;
131 
132   //: The fitting matrices
133   vnl_matrix_fixed<T,3,3> S11_, S12_, S22_;
134   vnl_matrix_fixed<T,3,3> Dinv_;
135 
136   //: The fitted conic in the un-normalized frame
137   vgl_conic<T> conic_;
138 
139   //: The algebraic fitting cost in the normalized frame
140   T cost_;
141 
142   //: The Sampson approximation to Euclidean distance in the normalized frame
143   T sampson_error_;
144 
145   //: Normalized points
146   std::vector<vgl_homg_point_2d<T> > hnorm_points_;
147 };
148 
149 #define VGL_CONIC_2D_REGRESSION_INSTANTIATE(T) extern "please include vgl/algo/vgl_conic_2d_regression.hxx first"
150 
151 #endif // vgl_conic_2d_regression_h_
152