contrib/mul/mbl/mbl_clamped_plate_spline_2d.cxx
Go to the documentation of this file.
00001 // This is mul/mbl/mbl_clamped_plate_spline_2d.cxx
00002 #include "mbl_clamped_plate_spline_2d.h"
00003 //:
00004 // \file
00005 // \brief Construct thin plate spline to map 2D to 2D
00006 // \author Tim Cootes
00007 
00008 #include <vcl_cmath.h>
00009 #include <vcl_cassert.h>
00010 #include <vcl_cstdlib.h> // for vcl_abort()
00011 #include <vsl/vsl_indent.h>
00012 #include <vsl/vsl_vector_io.h>
00013 #include <vnl/algo/vnl_svd.h>
00014 #include <mbl/mbl_matxvec.h>
00015 #include <vnl/io/vnl_io_vector.h>
00016 #include <vnl/io/vnl_io_matrix.h>
00017 #include <vgl/io/vgl_io_point_2d.h>
00018 
00019 //=======================================================================
00020 
00021 //=======================================================================
00022 // Dflt ctor
00023 // Default constructor gives identity mapping
00024 //=======================================================================
00025 
00026 mbl_clamped_plate_spline_2d::mbl_clamped_plate_spline_2d()
00027   : Wx_(0),Wy_(0)
00028 {
00029 }
00030 
00031 //=======================================================================
00032 // Destructor
00033 //=======================================================================
00034 
00035 mbl_clamped_plate_spline_2d::~mbl_clamped_plate_spline_2d()
00036 {
00037 }
00038 
00039 //: Check that all points are inside unit circle
00040 bool mbl_clamped_plate_spline_2d::all_in_unit_circle(const vcl_vector<vgl_point_2d<double> >& pts)
00041 {
00042   int n = pts.size();
00043   const vgl_point_2d<double> *p = &pts[0];
00044   for (int i=0;i<n;++i)
00045   {
00046     if (p[i].x()*p[i].x()+p[i].y()*p[i].y()>1.0) return false;
00047   }
00048 
00049   return true;
00050 }
00051 
00052 //: Green's function for the clamped plate spline
00053 //  0.5*|x-y|^2(A2-log(A2)-1)  where A2 = (|x|^2|y|^2-2x.y+1)/|x-y|^2
00054 //  If two points are x and y
00055 inline double cps_green(const vgl_point_2d<double>&  p1, const vgl_point_2d<double>& p2)
00056 {
00057   double dx = p1.x()-p2.x();
00058   double dy = p1.y()-p2.y();
00059   double d2 = (dx*dx+dy*dy);
00060   if (d2<1e-8)   return 0.5;  // Avoid numeric instability near zero
00061 
00062   double L1 = p1.x() * p1.x() + p1.y() * p1.y();
00063   double L2 = p2.x() * p2.x() + p2.y() * p2.y();
00064   double d  = p1.x() * p2.x() + p1.y() * p2.y();  // Dot product
00065   double A2 = (L1*L2-2*d+1)/d2;
00066 
00067   return 0.5*d2*(A2-vcl_log(A2)-1);
00068 }
00069 
00070 //: Green's function for the clamped plate spline
00071 //  0.5*|x-y|^2(A2-log(A2)-1)  where A2 = (|x|^2|y|^2-2x.y+1)/|x-y|^2
00072 //  If two points are x and y
00073 inline double cps_green(double x, double y, const vgl_point_2d<double>& p2)
00074 {
00075   double dx = x-p2.x();
00076   double dy = y-p2.y();
00077   double d2 = (dx*dx+dy*dy);
00078   if (d2<1e-8)   return 0.5;  // Avoid numeric instability near zero
00079 
00080   double L1 = x * x + y * y;
00081   double L2 = p2.x() * p2.x() + p2.y() * p2.y();
00082   double d  = x * p2.x() + y * p2.y();  // Dot product
00083   double A2 = (L1*L2-2*d+1)/d2;
00084 
00085   return 0.5*d2*(A2-vcl_log(A2)-1);
00086 }
00087 
00088 // Sets L to be a symmetric square matrix of size n x n (n = pts.nelems)
00089 // with L(i,j) = cps_green(pts[i],pts[j])
00090 static void build_L(vnl_matrix<double>& L,
00091                     const vcl_vector<vgl_point_2d<double> >& pts)
00092 {
00093   unsigned int n = pts.size();
00094   if ( (L.rows()!=n) || (L.columns()!=n) ) L.set_size(n,n);
00095 
00096   const vgl_point_2d<double> * pts_data = &pts[0];
00097   double** K_data = L.data_array();
00098 
00099     // Set the diagonal
00100   for (unsigned int i=0;i<n;i++)
00101     K_data[i][i] = 0.5;
00102     // Now fill upper & lower triangles
00103   for (unsigned int i=1;i<n;i++)
00104     for (unsigned int j=0;j<i;j++)
00105     {
00106       K_data[i][j] = K_data[j][i] = cps_green(pts_data[i],pts_data[j]);
00107     }
00108 }
00109 
00110 //: Set parameters from vectors
00111 void mbl_clamped_plate_spline_2d::set_params(const vnl_vector<double>& Wx,
00112                                              const vnl_vector<double>& Wy)
00113 {
00114   Wx_ = Wx;
00115   Wy_ = Wy;
00116 }
00117 
00118 void mbl_clamped_plate_spline_2d::set_up_rhs(vnl_vector<double>& Bx,
00119                                              vnl_vector<double>& By,
00120                                              const vcl_vector<vgl_point_2d<double> >& src_pts,
00121                                              const vcl_vector<vgl_point_2d<double> >& dest_pts)
00122 {
00123   int n =dest_pts.size();
00124 
00125   Bx.set_size(n);
00126   By.set_size(n);
00127   double* Bx_data=Bx.data_block();
00128   double* By_data=By.data_block();
00129   const vgl_point_2d<double>  *d_pts_data=&dest_pts[0];
00130   const vgl_point_2d<double>  *s_pts_data=&src_pts[0];
00131 
00132   for (int i=0;i<n;i++)
00133   {
00134     Bx_data[i] = d_pts_data[i].x()-s_pts_data[i].x();
00135     By_data[i] = d_pts_data[i].y()-s_pts_data[i].y();
00136   }
00137 }
00138 
00139 void mbl_clamped_plate_spline_2d::build(const vcl_vector<vgl_point_2d<double> >& source_pts,
00140                                         const vcl_vector<vgl_point_2d<double> >& dest_pts)
00141 {
00142   assert(all_in_unit_circle(source_pts));
00143   assert(all_in_unit_circle(dest_pts));
00144 
00145   unsigned int n=source_pts.size();
00146   if (dest_pts.size() != n)
00147   {
00148     vcl_cerr<<"mbl_clamped_plate_spline_2d::build - incompatible number of points.\n";
00149     vcl_abort();
00150   }
00151 
00152   L_inv_.set_size(0,0);
00153 
00154   src_pts_ = source_pts;
00155 
00156   vnl_matrix<double> L;
00157   vnl_vector<double> Bx(n);  // Used to compute X parameters
00158   vnl_vector<double> By(n);  // Used to compute Y parameters
00159 
00160   Wx_.set_size(n);
00161   Wy_.set_size(n);
00162 
00163   build_L(L,source_pts);
00164 
00165   set_up_rhs(Bx,By,source_pts,dest_pts);
00166 
00167   // Solve LW = B for W1 and W2 :
00168   // Note that both Cholesky and QR decompositions fail, apparently because of the
00169   // zeroes on the diagonal.  Use SVD to be safe.
00170   {
00171     vnl_svd<double> svd(L);
00172     svd.solve(Bx.data_block(),Wx_.data_block());
00173     svd.solve(By.data_block(),Wy_.data_block());
00174   }
00175 }
00176 
00177 //: Define source point positions
00178 //  Performs pre-computations so that build(dest_points) can be
00179 //  called multiple times efficiently
00180 void mbl_clamped_plate_spline_2d::set_source_pts(const vcl_vector<vgl_point_2d<double> >& source_pts)
00181 {
00182   assert(all_in_unit_circle(source_pts));
00183 
00184   src_pts_ = source_pts;
00185 
00186   vnl_matrix<double> L;
00187   build_L(L,source_pts);
00188 
00189   // Compute inverse of L
00190   // Note that both Cholesky and QR decompositions fail, apparently because of the
00191   // zeroes on the diagonal.  Use SVD to be safe.
00192   {
00193     vnl_svd<double> svd(L);
00194     L_inv_ = svd.inverse();
00195   }
00196 }
00197 
00198 //: Sets up internal transformation to map source_pts onto dest_pts
00199 void mbl_clamped_plate_spline_2d::build(const vcl_vector<vgl_point_2d<double> >& dest_pts)
00200 {
00201   assert(all_in_unit_circle(dest_pts));
00202 
00203   unsigned int n=src_pts_.size();
00204   if (dest_pts.size() != n)
00205   {
00206     vcl_cerr<<"mbl_clamped_plate_spline_2d::build - incompatible number of points.\n";
00207     vcl_abort();
00208   }
00209 
00210   vnl_vector<double> Bx(n);  // Used to compute X parameters
00211   vnl_vector<double> By(n);  // Used to compute Y parameters
00212 
00213   Wx_.set_size(n);
00214   Wy_.set_size(n);
00215 
00216   set_up_rhs(Bx,By,src_pts_,dest_pts);
00217 
00218   mbl_matxvec_prod_mv(L_inv_,Bx,Wx_);
00219   mbl_matxvec_prod_mv(L_inv_,By,Wy_);
00220 }
00221 
00222 
00223 vgl_point_2d<double> mbl_clamped_plate_spline_2d::operator()(double x, double y) const
00224 {
00225   unsigned int n = src_pts_.size();
00226 
00227   double x_sum = x;
00228   double y_sum = y;
00229 
00230   const vgl_point_2d<double> * pts_data = &src_pts_[0];
00231   const double* Wx_data = Wx_.data_block();
00232   const double* Wy_data = Wy_.data_block();
00233 
00234   for (unsigned int i=0;i<n;i++)
00235   {
00236     double Ui = cps_green(x,y,pts_data[i]);
00237     x_sum += (Ui * Wx_data[i]);
00238     y_sum += (Ui * Wy_data[i]);
00239   }
00240 
00241   return vgl_point_2d<double>(x_sum,y_sum);
00242 }
00243 
00244 //=======================================================================
00245 // Method: version_no
00246 //=======================================================================
00247 
00248 short mbl_clamped_plate_spline_2d::version_no() const
00249 {
00250   return 1;
00251 }
00252 
00253 
00254 //=======================================================================
00255 // Method: print
00256 //=======================================================================
00257 
00258 // required if data is present in this class
00259 void mbl_clamped_plate_spline_2d::print_summary(vcl_ostream& os) const
00260 {
00261   os<<"\nfx:";
00262   for (unsigned int i=0;i<Wx_.size();++i)
00263     os<<" "<<Wx_[i];
00264   os<<"\nfy:";
00265   for (unsigned int i=0;i<Wy_.size();++i)
00266     os<<" "<<Wy_[i];
00267   os<<'\n';
00268 }
00269 
00270 //=======================================================================
00271 // Method: save
00272 //=======================================================================
00273 
00274   // required if data is present in this class
00275 void mbl_clamped_plate_spline_2d::b_write(vsl_b_ostream& bfs) const
00276 {
00277   vsl_b_write(bfs,version_no());
00278   vsl_b_write(bfs,Wx_); vsl_b_write(bfs,Wy_);
00279   vsl_b_write(bfs,src_pts_);
00280   vsl_b_write(bfs,L_inv_);
00281 }
00282 
00283 //=======================================================================
00284 // Method: load
00285 //=======================================================================
00286 
00287   // required if data is present in this class
00288 void mbl_clamped_plate_spline_2d::b_read(vsl_b_istream& bfs)
00289 {
00290   if (!bfs) return;
00291 
00292   short version;
00293   vsl_b_read(bfs,version);
00294   switch (version)
00295   {
00296     case (1):
00297       vsl_b_read(bfs,Wx_);
00298       vsl_b_read(bfs,Wy_);
00299       vsl_b_read(bfs,src_pts_);
00300       vsl_b_read(bfs,L_inv_);
00301       break;
00302     default:
00303       vcl_cerr << "I/O ERROR: vsl_b_read(vsl_b_istream&, mbl_clamped_plate_spline_2d &)\n"
00304                << "           Unknown version number "<< version << '\n';
00305       bfs.is().clear(vcl_ios::badbit); // Set an unrecoverable IO error on stream
00306   }
00307 }
00308 
00309 //: Comparison operator
00310 bool mbl_clamped_plate_spline_2d::operator==(const mbl_clamped_plate_spline_2d& tps) const
00311 {
00312   if (&tps==this) return true;
00313   if (vnl_vector_ssd(Wx_,tps.Wx_)>1e-6) return false;
00314   if (vnl_vector_ssd(Wy_,tps.Wy_)>1e-6) return false;
00315   return true;
00316 }
00317 
00318 //=======================================================================
00319 // Associated function: operator<<
00320 //=======================================================================
00321 
00322 void vsl_b_write(vsl_b_ostream& bfs, const mbl_clamped_plate_spline_2d& b)
00323 {
00324     b.b_write(bfs);
00325 }
00326 
00327 //=======================================================================
00328 // Associated function: operator>>
00329 //=======================================================================
00330 
00331 void vsl_b_read(vsl_b_istream& bfs, mbl_clamped_plate_spline_2d& b)
00332 {
00333     b.b_read(bfs);
00334 }
00335 
00336 //=======================================================================
00337 // Associated function: operator<<
00338 //=======================================================================
00339 
00340 vcl_ostream& operator<<(vcl_ostream& os,const mbl_clamped_plate_spline_2d& b)
00341 {
00342   os << "mbl_clamped_plate_spline_2d: ";
00343   vsl_indent_inc(os);
00344   b.print_summary(os);
00345   vsl_indent_dec(os);
00346   return os;
00347 }