contrib/oxl/osl/osl_fit_circle.cxx
Go to the documentation of this file.
00001 // This is oxl/osl/osl_fit_circle.cxx
00002 #ifdef VCL_NEEDS_PRAGMA_INTERFACE
00003 #pragma implementation
00004 #endif
00005 
00006 #include "osl_fit_circle.h"
00007 #include <vcl_cmath.h>
00008 #include <vnl/vnl_double_2.h>
00009 #include <vnl/algo/vnl_svd.h>
00010 
00011 // The well-known square function
00012 static inline double square(double x) { return x * x; }
00013 
00014 osl_fit_circle::osl_fit_circle(const vcl_list<vgl_point_2d<double> > &points)
00015 {
00016     calculate(points);
00017 }
00018 
00019 osl_fit_circle::osl_fit_circle(const osl_edgel_chain &chain)
00020 {
00021     // Extract points from edgel chain. I agree that this
00022     // may cause overhead.
00023     vcl_list<vgl_point_2d<double> > points;
00024 
00025     for (unsigned int i = 0; i < chain.size(); i++)
00026     {
00027         points.push_back(vgl_point_2d<double>(
00028             chain.GetX(i), chain.GetY(i)));
00029     }
00030 
00031     calculate(points);
00032 }
00033 
00034 void osl_fit_circle::calculate(const vcl_list<vgl_point_2d<double> > &points)
00035 {
00036     int rows = points.size();
00037 
00038     error_ = false;
00039 
00040     // must have at least 3 points to find circle
00041     if (rows < 3)
00042     {
00043         error_ = true;
00044         return;
00045     }
00046 
00047     vnl_vector<double> col1(rows);
00048     vnl_vector<double> col2(rows);
00049     vnl_vector<double> col3(rows);
00050     vnl_vector<double> col4(rows);
00051 
00052     vcl_list<vgl_point_2d<double> >::const_iterator it = points.begin();
00053     for (int i = 0; it != points.end(); ++it, ++i)
00054     {
00055         col2.put(i, (*it).y());
00056         col3.put(i, (*it).x());
00057     }
00058 
00059     for (int i = 0; i < rows; ++i)
00060     {
00061         col1.put(i, square(col2.get(i)) + square(col3.get(i)));
00062         col4.put(i, 1.0);
00063     }
00064 
00065     vnl_matrix<double> m(rows, 4);
00066 
00067     m.set_column(0, col1);
00068     m.set_column(1, col2);
00069     m.set_column(2, col3);
00070     m.set_column(3, col4);
00071 
00072     vnl_svd<double> svd(m);
00073 
00074     // singular values are stored by vnl_svd in decreasing
00075     // order, so get last column to get optimal solution
00076     vnl_vector<double> u = svd.V().get_column(3);
00077 
00078     double a = u(0);
00079     vnl_double_2 b(u(1),u(2));
00080 
00081     double c = u(3);
00082 
00083     center_.set(-b(1) / 2 / a, -b(0) / 2 / a);
00084     radius_ = vcl_sqrt(square(center_.x()) + square(center_.y()) - c / a);
00085 
00086     max_diff_ = avg_diff_ = 0;
00087 
00088     // calculate maximum and average error. this is different
00089     // to the original routine, which calculates an error sum
00090     for (it = points.begin(); it != points.end(); it++)
00091     {
00092         double cur_diff= vcl_fabs(vcl_sqrt(square((*it).x() - center_.x()) +
00093                                            square((*it).y() - center_.y())) - radius_);
00094 
00095         if (cur_diff > max_diff_)
00096             max_diff_ = cur_diff;
00097 
00098         avg_diff_ += cur_diff;
00099     }
00100 
00101     avg_diff_ /= points.size();
00102 }