00001
00002 #include "mbl_clamped_plate_spline_2d.h"
00003
00004
00005
00006
00007
00008 #include <vcl_cmath.h>
00009 #include <vcl_cassert.h>
00010 #include <vcl_cstdlib.h>
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
00023
00024
00025
00026 mbl_clamped_plate_spline_2d::mbl_clamped_plate_spline_2d()
00027 : Wx_(0),Wy_(0)
00028 {
00029 }
00030
00031
00032
00033
00034
00035 mbl_clamped_plate_spline_2d::~mbl_clamped_plate_spline_2d()
00036 {
00037 }
00038
00039
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
00053
00054
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;
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();
00065 double A2 = (L1*L2-2*d+1)/d2;
00066
00067 return 0.5*d2*(A2-vcl_log(A2)-1);
00068 }
00069
00070
00071
00072
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;
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();
00083 double A2 = (L1*L2-2*d+1)/d2;
00084
00085 return 0.5*d2*(A2-vcl_log(A2)-1);
00086 }
00087
00088
00089
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
00100 for (unsigned int i=0;i<n;i++)
00101 K_data[i][i] = 0.5;
00102
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
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);
00158 vnl_vector<double> By(n);
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
00168
00169
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
00178
00179
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
00190
00191
00192 {
00193 vnl_svd<double> svd(L);
00194 L_inv_ = svd.inverse();
00195 }
00196 }
00197
00198
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);
00211 vnl_vector<double> By(n);
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
00246
00247
00248 short mbl_clamped_plate_spline_2d::version_no() const
00249 {
00250 return 1;
00251 }
00252
00253
00254
00255
00256
00257
00258
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
00272
00273
00274
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
00285
00286
00287
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);
00306 }
00307 }
00308
00309
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
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
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
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 }