contrib/mul/mfpf/mfpf_norm_corr1d.cxx
Go to the documentation of this file.
00001 #include "mfpf_norm_corr1d.h"
00002 //:
00003 // \file
00004 // \brief Searches along a profile using normalised correlation
00005 // \author Tim Cootes
00006 
00007 #include <vsl/vsl_binary_loader.h>
00008 #include <vcl_cmath.h> // for std::abs()
00009 #include <vcl_cassert.h>
00010 #include <vcl_algorithm.h>
00011 
00012 #include <vimt/vimt_sample_profile_bilin.h>
00013 #include <vnl/io/vnl_io_vector.h>
00014 #include <vgl/vgl_point_2d.h>
00015 #include <vgl/vgl_vector_2d.h>
00016 
00017 //=======================================================================
00018 // Dflt ctor
00019 //=======================================================================
00020 
00021 mfpf_norm_corr1d::mfpf_norm_corr1d()
00022 {
00023   set_defaults();
00024 }
00025 
00026 //: Define default values
00027 void mfpf_norm_corr1d::set_defaults()
00028 {
00029   step_size_=1.0;
00030   ilo_=1; ihi_=0;
00031   search_ni_=5;
00032 }
00033 
00034 //=======================================================================
00035 // Destructor
00036 //=======================================================================
00037 
00038 mfpf_norm_corr1d::~mfpf_norm_corr1d()
00039 {
00040 }
00041 
00042 //: Generate points in ref frame that represent boundary
00043 //  Points of a contour around the shape.
00044 //  Used for display purposes.
00045 void mfpf_norm_corr1d::get_outline(vcl_vector<vgl_point_2d<double> >& pts) const
00046 {
00047   pts.resize(2);
00048   pts[0]=vgl_point_2d<double>(ilo_-0.5,0);
00049   pts[1]=vgl_point_2d<double>(ihi_+0.5,0);
00050 }
00051 
00052 
00053 //: Define filter kernel to search with
00054 void mfpf_norm_corr1d::set(int ilo, int ihi, const vnl_vector<double>& k)
00055 {
00056   assert(1+ihi-ilo==(int)k.size());
00057   ilo_=ilo;
00058   ihi_=ihi;
00059   kernel_=k;
00060   // Normalise the vector
00061   kernel_-=kernel_.mean();
00062   kernel_.normalize();
00063 }
00064 
00065 //: Number of dimensions in the model
00066 unsigned mfpf_norm_corr1d::model_dim()
00067 {
00068   return kernel_.size();
00069 }
00070 
00071 //: Radius of circle containing modelled region
00072 double mfpf_norm_corr1d::radius() const
00073 {
00074   return vcl_max(vcl_abs(ilo_),vcl_abs(ihi_));
00075 }
00076 
00077 // Assumes v2[i] has zero mean and unit length as a vector
00078 inline double norm_corr(const double* v1, const double* v2, unsigned n)
00079 {
00080   double sum1=0.0,sum2=0.0,sum_sq=0.0;
00081   for (unsigned i=0;i<n;++i)
00082   {
00083     sum1+=v1[i]*v2[i];
00084     sum2+=v1[i];
00085     sum_sq+=v1[i]*v1[i];
00086   }
00087   double mean = sum2/n;
00088   double ss = vcl_max(1e-6,sum_sq-n*mean*mean);
00089   double s = vcl_sqrt(ss);
00090   return sum1/s;
00091 }
00092 
00093 //: Evaluate match at p, using u to define scale and orientation
00094 // Returns -1*edge strength at p along direction u
00095 double mfpf_norm_corr1d::evaluate(const vimt_image_2d_of<float>& image,
00096                                   const vgl_point_2d<double>& p,
00097                                   const vgl_vector_2d<double>& u)
00098 {
00099   int n=1+ihi_-ilo_;
00100   vnl_vector<double> v(n);
00101   vgl_vector_2d<double> u1=step_size_*u;
00102 
00103   const vgl_point_2d<double> p0 = p+ilo_*u1;
00104   vimt_sample_profile_bilin(v,image,p0,u1,n);
00105 
00106   return 1.0-norm_corr(v.data_block(),kernel_.data_block(),n);
00107 }
00108 
00109 //: Evaluate match at in a region around p
00110 // Returns a quality of fit at a set of positions.
00111 // response image (whose size and transform is set inside the
00112 // function), indicates the points at which the function was
00113 // evaluated.  response(i,j) is the fit at the point
00114 // response.world2im().inverse()(i,j).  The world2im() transformation
00115 // may be affine.
00116 void mfpf_norm_corr1d::evaluate_region(
00117                         const vimt_image_2d_of<float>& image,
00118                         const vgl_point_2d<double>& p,
00119                         const vgl_vector_2d<double>& u,
00120                         vimt_image_2d_of<double>& response)
00121 {
00122   int n=1+2*search_ni_;
00123   int ns = 2*search_ni_ + 1+ ihi_-ilo_;
00124   vnl_vector<double> v(ns);
00125   vgl_vector_2d<double> u1=step_size_*u;
00126   const vgl_point_2d<double> p0 = p+(ilo_-search_ni_)*u1;
00127   vimt_sample_profile_bilin(v,image,p0,u1,ns);
00128   response.image().set_size(n,1);
00129   double* r = response.image().top_left_ptr();
00130   for (int i=0;i<n;++i,++r)
00131   {
00132     *r = 1.0-norm_corr(&v[i],kernel_.data_block(),kernel_.size());
00133   }
00134 
00135   // Set up transformation parameters
00136 
00137   // Point (i,j) in dest corresponds to p1+i.u+j.v,
00138   // an affine transformation for image to world
00139   const vgl_point_2d<double> p1 = p-search_ni_*u1;
00140 
00141   vimt_transform_2d i2w;
00142   i2w.set_similarity(vgl_point_2d<double>(u1.x(),u1.y()),p1);
00143   response.set_world2im(i2w.inverse());
00144 }
00145 
00146 //: Search given image around p, using u to define scale and orientation
00147 //  On exit, new_p and new_u define position, scale and orientation of
00148 //  the best nearby match.  Returns a quality of fit measure at that
00149 //  point (the smaller the better).
00150 double mfpf_norm_corr1d::search_one_pose(
00151                                 const vimt_image_2d_of<float>& image,
00152                                 const vgl_point_2d<double>& p,
00153                                 const vgl_vector_2d<double>& u,
00154                                 vgl_point_2d<double>& new_p)
00155 {
00156   int n=1+2*search_ni_;
00157   int ns = 2*search_ni_ + 1+ ihi_-ilo_;
00158   vnl_vector<double> v(ns);
00159   vgl_vector_2d<double> u1=step_size_*u;
00160   const vgl_point_2d<double> p0 = p+(ilo_-search_ni_)*u1;
00161   vimt_sample_profile_bilin(v,image,p0,u1,ns);
00162   int best_i=0;
00163   double best_r = norm_corr(&v[0],kernel_.data_block(),kernel_.size());
00164   for (int i=1;i<n;++i)
00165   {
00166     double r = norm_corr(&v[i],kernel_.data_block(),kernel_.size());
00167     if (r>best_r) { best_r=r; best_i=i; }
00168   }
00169   new_p = p+(best_i-search_ni_)*u1;
00170   return 1.0 - best_r;
00171 }
00172 
00173 
00174 //=======================================================================
00175 // Method: is_a
00176 //=======================================================================
00177 
00178 vcl_string mfpf_norm_corr1d::is_a() const
00179 {
00180   return vcl_string("mfpf_norm_corr1d");
00181 }
00182 
00183 //: Create a copy on the heap and return base class pointer
00184 mfpf_point_finder* mfpf_norm_corr1d::clone() const
00185 {
00186   return new mfpf_norm_corr1d(*this);
00187 }
00188 
00189 //=======================================================================
00190 // Method: print
00191 //=======================================================================
00192 
00193 void mfpf_norm_corr1d::print_summary(vcl_ostream& os) const
00194 {
00195   os << "{ size: [" << ilo_ << ',' << ihi_ << ']'
00196      << " Kernel: " << kernel_ << '\n';
00197   mfpf_point_finder::print_summary(os);
00198   os << '}';
00199 }
00200 
00201 short mfpf_norm_corr1d::version_no() const
00202 {
00203   return 1;
00204 }
00205 
00206 void mfpf_norm_corr1d::b_write(vsl_b_ostream& bfs) const
00207 {
00208   vsl_b_write(bfs,version_no());
00209   mfpf_point_finder::b_write(bfs);  // Save base class
00210   vsl_b_write(bfs,ilo_);
00211   vsl_b_write(bfs,ihi_);
00212   vsl_b_write(bfs,kernel_);
00213 }
00214 
00215 //=======================================================================
00216 // Method: load
00217 //=======================================================================
00218 
00219 void mfpf_norm_corr1d::b_read(vsl_b_istream& bfs)
00220 {
00221   if (!bfs) return;
00222   short version;
00223   vsl_b_read(bfs,version);
00224   switch (version)
00225   {
00226     case 1:
00227       mfpf_point_finder::b_read(bfs);  // Load in base class
00228       vsl_b_read(bfs,ilo_);
00229       vsl_b_read(bfs,ihi_);
00230       vsl_b_read(bfs,kernel_);
00231       break;
00232     default:
00233       vcl_cerr << "I/O ERROR: vsl_b_read(vsl_b_istream&)\n"
00234                << "           Unknown version number "<< version << vcl_endl;
00235       bfs.is().clear(vcl_ios::badbit); // Set an unrecoverable IO error on stream
00236       return;
00237   }
00238 }
00239 
00240 //: Test equality
00241 bool mfpf_norm_corr1d::operator==(const mfpf_norm_corr1d& nc) const
00242 {
00243   if (!base_equality(nc)) return false;
00244   if (ilo_!=nc.ilo_) return false;
00245   if (ihi_!=nc.ihi_) return false;
00246   if (kernel_.size()!=nc.kernel_.size()) return false;
00247   return vnl_vector_ssd(kernel_,nc.kernel_)<1e-4;
00248 }
00249