00001
00002 #include "mbl_thin_plate_spline_weights_3d.h"
00003
00004
00005
00006
00007
00008 #include <vcl_cmath.h>
00009 #include <vcl_cstdlib.h>
00010 #include <vsl/vsl_indent.h>
00011 #include <vsl/vsl_vector_io.h>
00012 #include <vnl/vnl_math.h>
00013 #include <vnl/algo/vnl_svd.h>
00014 #include <vnl/io/vnl_io_vector.h>
00015 #include <vnl/io/vnl_io_matrix.h>
00016 #include <vgl/vgl_vector_3d.h>
00017 #include <vgl/io/vgl_io_point_3d.h>
00018 #include <mbl/mbl_matxvec.h>
00019
00020
00021
00022
00023
00024
00025
00026
00027 mbl_thin_plate_spline_weights_3d::mbl_thin_plate_spline_weights_3d()
00028 : Wx_(0),Wy_(0),Wz_(0),
00029 Ax0_(0),AxX_(1),AxY_(0),AxZ_(0),
00030 Ay0_(0),AyX_(0),AyY_(1),AyZ_(0),
00031 Az0_(0),AzX_(0),AzY_(0),AzZ_(1),
00032 energy_x_(0),energy_y_(0),energy_z_(0),
00033 src_pts_(0), pt_wts_(0)
00034 {
00035 }
00036
00037
00038
00039
00040
00041 mbl_thin_plate_spline_weights_3d::~mbl_thin_plate_spline_weights_3d()
00042 {
00043 }
00044
00045
00046
00047 inline double r2lnr( const vgl_vector_3d<double>& pt, const vgl_point_3d<double> & wt )
00048 {
00049 double r2 = ( pt.x() * pt.x() * wt.x() ) +
00050 ( pt.y() * pt.y() * wt.y() ) +
00051 ( pt.z() * pt.z() * wt.z() );
00052 if (r2>1e-8)
00053 return 0.5 * r2 * vcl_log(r2);
00054 else
00055 return 0;
00056 }
00057
00058 inline double r2lnr(double x, double y, double z, double wx, double wy, double wz)
00059 {
00060 double r2 = (x * x * wx) + (y * y * wy) + (z * z * wz);
00061 if (r2>1e-8)
00062 return 0.5 * r2 * vcl_log(r2);
00063 else
00064 return 0;
00065 }
00066
00067
00068
00069
00070 static void build_K_part( vnl_matrix<double>& L,
00071 const vcl_vector<vgl_point_3d<double> >& pts,
00072 const vcl_vector<vgl_point_3d<double> >& wts)
00073 {
00074 unsigned int n = pts.size();
00075 if ( (L.rows()!=n+4) | (L.columns()!=n+4) ) L.set_size(n+4,n+4);
00076
00077 const vgl_point_3d<double> * pts_data = &pts[0];
00078 double** K_data = L.data_array();
00079
00080
00081 for (unsigned int i=0;i<n;i++)
00082 K_data[i][i] = 0;
00083
00084
00085 for (unsigned int i=0;i<n;i++)
00086 for (unsigned int j=0;j<n;j++)
00087 {
00088 vgl_point_3d<double> temp_wt;
00089 if ( j < wts.size() )
00090 {
00091 temp_wt.set( wts[j].x(), wts[j].y(), wts[j].z() );
00092 }
00093 else
00094 {
00095 temp_wt.set( 1., 1., 1. );
00096 }
00097
00098 K_data[i][j] = r2lnr( pts_data[i]-pts_data[j], temp_wt );
00099 }
00100 }
00101
00102
00103
00104
00105
00106
00107
00108
00109 static void build_L( vnl_matrix<double>& L,
00110 const vcl_vector<vgl_point_3d<double> >& pts,
00111 const vcl_vector<vgl_point_3d<double> >& wts)
00112 {
00113 int i,j;
00114
00115 build_K_part( L, pts, wts );
00116
00117 int n = pts.size();
00118
00119 const vgl_point_3d<double> * pts_data = &pts[0];
00120 double** L_data = L.data_array();
00121
00122
00123
00124 for (i=0;i<n;i++)
00125 {
00126 L_data[i][n] = 1;
00127 L_data[i][n+1] = pts_data[i].x();
00128 L_data[i][n+2] = pts_data[i].y();
00129 L_data[i][n+3] = pts_data[i].z();
00130
00131 L_data[n][i] = 1;
00132 L_data[n+1][i] = pts_data[i].x();
00133 L_data[n+2][i] = pts_data[i].y();
00134 L_data[n+3][i] = pts_data[i].z();
00135 }
00136
00137
00138 for (i=n;i<n+4;i++)
00139 for (j=n;j<n+4;j++)
00140 L_data[i][j] = 0;
00141 }
00142
00143
00144 void mbl_thin_plate_spline_weights_3d::build_pure_affine(
00145 const vcl_vector<vgl_point_3d<double> >& source_pts,
00146 const vcl_vector<vgl_point_3d<double> >& dest_pts)
00147 {
00148 int n=source_pts.size();
00149 L_inv_.set_size(0,0);
00150 if (n==0)
00151 {
00152
00153 Ax0_ = 0;
00154 Ay0_ = 0;
00155 Az0_ = 0;
00156 AxX_ = 1; AxY_ = 0; AxZ_ = 0;
00157 AyX_ = 0; AyY_ = 1; AyZ_ = 0;
00158 AzX_ = 0; AzY_ = 0; AzZ_ = 1;
00159
00160 Wx_.set_size(0);
00161 Wy_.set_size(0);
00162 Wz_.set_size(0);
00163
00164 src_pts_.resize(0);
00165
00166 return;
00167 }
00168
00169 if (n==1)
00170 {
00171
00172 Ax0_ = dest_pts[0].x() - source_pts[0].x();
00173 Ay0_ = dest_pts[0].y() - source_pts[0].y();
00174 Az0_ = dest_pts[0].z() - source_pts[0].z();
00175 Wx_.set_size(0);
00176 Wy_.set_size(0);
00177 Wz_.set_size(0);
00178 AxX_ = 1; AxY_ = 0; AxZ_ = 0;
00179 AyX_ = 0; AyY_ = 1; AyZ_ = 0;
00180 AzX_ = 0; AzY_ = 0; AzZ_ = 1;
00181 src_pts_.resize(0);
00182
00183 return;
00184 }
00185 if (n>=2)
00186 {
00187 vcl_cerr<<"mbl_thin_plate_spline_weights_3d::build_pure_affine() Incomplete. sorry.\n";
00188 vcl_abort();
00189 }
00190 }
00191
00192
00193 void mbl_thin_plate_spline_weights_3d::set_params(const vnl_vector<double>& W1,
00194 const vnl_vector<double>& W2,
00195 const vnl_vector<double>& W3)
00196 {
00197 int n = W1.size()-4;
00198
00199 if (int(Wx_.size()) < n) Wx_.set_size(n);
00200 if (int(Wy_.size()) < n) Wy_.set_size(n);
00201 if (int(Wz_.size()) < n) Wz_.set_size(n);
00202
00203 double *Wx_data=Wx_.data_block();
00204 double *Wy_data=Wy_.data_block();
00205 double *Wz_data=Wz_.data_block();
00206 const double *W1_data=W1.data_block();
00207 const double *W2_data=W2.data_block();
00208 const double *W3_data=W3.data_block();
00209
00210 for (int i=0;i<n;i++)
00211 {
00212 Wx_data[i] = W1_data[i];
00213 Wy_data[i] = W2_data[i];
00214 Wz_data[i] = W3_data[i];
00215 }
00216
00217 Ax0_ = W1_data[n];
00218 AxX_ = W1_data[n+1];
00219 AxY_ = W1_data[n+2];
00220 AxZ_ = W1_data[n+3];
00221
00222 Ay0_ = W2_data[n];
00223 AyX_ = W2_data[n+1];
00224 AyY_ = W2_data[n+2];
00225 AyZ_ = W2_data[n+3];
00226
00227 Az0_ = W3_data[n];
00228 AzX_ = W3_data[n+1];
00229 AzY_ = W3_data[n+2];
00230 AzZ_ = W3_data[n+3];
00231 }
00232
00233 void mbl_thin_plate_spline_weights_3d::compute_energy(vnl_vector<double>& W1,
00234 vnl_vector<double>& W2,
00235 vnl_vector<double>& W3,
00236 const vnl_matrix<double>& L)
00237 {
00238 int n = W1.size()-3;
00239 double *W1_data=W1.data_block();
00240 double *W2_data=W2.data_block();
00241 double *W3_data=W3.data_block();
00242
00243
00244 for (int i = n;i<=n+3;++i)
00245 {
00246 W1_data[i]=0.0;
00247 W2_data[i]=0.0;
00248 W3_data[i]=0.0;
00249 }
00250
00251
00252 vnl_vector<double> LW;
00253 mbl_matxvec_prod_mv(L,W1,LW);
00254 energy_x_ = dot_product(W1,LW) / (8*vnl_math::pi);
00255
00256 mbl_matxvec_prod_mv(L,W2,LW);
00257 energy_y_ = dot_product(W2,LW) / (8*vnl_math::pi);
00258
00259 mbl_matxvec_prod_mv(L,W3,LW);
00260 energy_z_ = dot_product(W3,LW) / (8*vnl_math::pi);
00261 }
00262
00263 void mbl_thin_plate_spline_weights_3d::set_up_rhs(vnl_vector<double>& Bx,
00264 vnl_vector<double>& By,
00265 vnl_vector<double>& Bz,
00266 const vcl_vector<vgl_point_3d<double> >& dest_pts)
00267 {
00268 int n =dest_pts.size();
00269
00270 Bx.set_size(n+4);
00271 By.set_size(n+4);
00272 Bz.set_size(n+4);
00273 double* Bx_data=Bx.data_block();
00274 double* By_data=By.data_block();
00275 double* Bz_data=Bz.data_block();
00276 const vgl_point_3d<double> *d_pts_data=&dest_pts[0];
00277
00278 for (int i=0;i<n;i++)
00279 {
00280 Bx_data[i] = d_pts_data[i].x();
00281 By_data[i] = d_pts_data[i].y();
00282 Bz_data[i] = d_pts_data[i].z();
00283 }
00284 for (int i=n;i<n+4;i++)
00285 {
00286 Bx_data[i] = 0;
00287 By_data[i] = 0;
00288 Bz_data[i] = 0;
00289 }
00290 }
00291
00292 void mbl_thin_plate_spline_weights_3d::build(const vcl_vector<vgl_point_3d<double> >& source_pts,
00293 const vcl_vector<vgl_point_3d<double> >& dest_pts,
00294 bool compute_the_energy)
00295 {
00296
00297
00298 unsigned int n=source_pts.size();
00299 if (dest_pts.size() != n)
00300 {
00301 vcl_cerr<<"mbl_thin_plate_spline_weights_3d::build - incompatible number of points.\n";
00302 vcl_abort();
00303 }
00304
00305 L_inv_.set_size(0,0);
00306
00307 if (n<2)
00308 {
00309 build_pure_affine(source_pts,dest_pts);
00310 return;
00311 }
00312
00313 src_pts_ = source_pts;
00314
00315 vnl_matrix<double> L;
00316 vnl_vector<double> Bx(n+4), W1(n+4);
00317 vnl_vector<double> By(n+4), W2(n+4);
00318 vnl_vector<double> Bz(n+4), W3(n+4);
00319
00320 build_L( L, source_pts, pt_wts_ );
00321
00322 set_up_rhs(Bx,By,Bz,dest_pts);
00323
00324
00325
00326
00327 {
00328 vnl_svd<double> svd(L);
00329 svd.solve(Bx.data_block(),W1.data_block());
00330 svd.solve(By.data_block(),W2.data_block());
00331 svd.solve(Bz.data_block(),W3.data_block());
00332 }
00333
00334 set_params(W1,W2,W3);
00335 if (compute_the_energy)
00336 compute_energy(W1,W2,W3,L);
00337 }
00338
00339
00340
00341
00342 void mbl_thin_plate_spline_weights_3d::set_source_pts(const vcl_vector<vgl_point_3d<double> >& source_pts)
00343 {
00344 unsigned int n=source_pts.size();
00345 src_pts_ = source_pts;
00346
00347 if (n<2)
00348 {
00349 L_inv_.set_size(0,0);
00350 return;
00351 }
00352
00353 vnl_matrix<double> L;
00354 build_L( L, source_pts, pt_wts_ );
00355
00356
00357
00358
00359 {
00360 vnl_svd<double> svd(L);
00361 L_inv_ = svd.inverse();
00362 }
00363 }
00364
00365
00366
00367 void mbl_thin_plate_spline_weights_3d::set_pt_wts( const vcl_vector<vgl_point_3d<double> >& pt_wts )
00368 {
00369 pt_wts_ = pt_wts;
00370 }
00371
00372
00373 void mbl_thin_plate_spline_weights_3d::build(const vcl_vector<vgl_point_3d<double> >& dest_pts)
00374 {
00375 unsigned int n=src_pts_.size();
00376 if (dest_pts.size() != n)
00377 {
00378 vcl_cerr<<"mbl_thin_plate_spline_weights_3d::build - incompatible number of points.\n";
00379 vcl_abort();
00380 }
00381
00382 if (n<2)
00383 {
00384 build_pure_affine(src_pts_,dest_pts);
00385 return;
00386 }
00387
00388 vnl_vector<double> Bx(n+4), W1(n+4);
00389 vnl_vector<double> By(n+4), W2(n+4);
00390 vnl_vector<double> Bz(n+4), W3(n+4);
00391
00392 set_up_rhs(Bx,By,Bz,dest_pts);
00393
00394 mbl_matxvec_prod_mv(L_inv_,Bx,W1);
00395 mbl_matxvec_prod_mv(L_inv_,By,W2);
00396 mbl_matxvec_prod_mv(L_inv_,Bz,W3);
00397
00398 set_params(W1,W2,W3);
00399 energy_x_ = -1;
00400 energy_y_ = -1;
00401 energy_z_ = -1;
00402 }
00403
00404
00405 vgl_point_3d<double> mbl_thin_plate_spline_weights_3d::operator()(double x, double y, double z) const
00406 {
00407 unsigned int n = src_pts_.size();
00408
00409 double x_sum = Ax0_ + AxX_ * x + AxY_ * y + AxZ_ * z;
00410 double y_sum = Ay0_ + AyX_ * x + AyY_ * y + AyZ_ * z;
00411 double z_sum = Az0_ + AzX_ * x + AzY_ * y + AzZ_ * z;
00412
00413 if (n<=4)
00414 return vgl_point_3d<double>(x_sum,y_sum,z_sum);
00415
00416 const vgl_point_3d<double> * pts_data = &src_pts_[0];
00417 const double* Wx_data = Wx_.data_block();
00418 const double* Wy_data = Wy_.data_block();
00419 const double* Wz_data = Wz_.data_block();
00420
00421 vgl_point_3d<double> temp_wt;
00422
00423 for (unsigned int i=0;i<n;i++)
00424 {
00425 if ( i < pt_wts_.size() )
00426 {
00427 temp_wt.set( pt_wts_[i].x(), pt_wts_[i].y(), pt_wts_[i].z() );
00428 }
00429 else
00430 {
00431 temp_wt.set( 1., 1., 1. );
00432 }
00433
00434 double Ui = r2lnr(x - pts_data[i].x(), y - pts_data[i].y(), z - pts_data[i].z(), temp_wt.x(), temp_wt.y(), temp_wt.z() );
00435
00436 x_sum += (Ui * Wx_data[i]);
00437 y_sum += (Ui * Wy_data[i]);
00438 z_sum += (Ui * Wz_data[i]);
00439 }
00440
00441 return vgl_point_3d<double>(x_sum,y_sum,z_sum);
00442 }
00443
00444
00445
00446
00447
00448 short mbl_thin_plate_spline_weights_3d::version_no() const
00449 {
00450 return 1;
00451 }
00452
00453
00454
00455
00456
00457
00458
00459 void mbl_thin_plate_spline_weights_3d::print_summary(vcl_ostream& os) const
00460 {
00461 os<<"\nfx: "<<Ax0_<<" + "<<AxX_<<"*x + "<<AxY_
00462 <<"*y + "<<AxZ_<<"*z Nonlinear terms:";
00463 for (unsigned int i=0;i<Wx_.size();++i)
00464 os<<' '<<Wx_[i];
00465 os<<"\nfy: "<<Ay0_<<" + "<<AyX_<<"*x + "<<AyY_
00466 <<"*y + "<<AyZ_<<"*z Nonlinear terms:";
00467 for (unsigned int i=0;i<Wy_.size();++i)
00468 os<<' '<<Wy_[i];
00469 os<<"\nfy: "<<Az0_<<" + "<<AzX_<<"*x + "<<AzY_
00470 <<"*y + "<<AzZ_<<"*z Nonlinear terms:";
00471 for (unsigned int i=0;i<Wz_.size();++i)
00472 os<<' '<<Wz_[i];
00473 os<<'\n';
00474 }
00475
00476
00477
00478
00479
00480
00481 void mbl_thin_plate_spline_weights_3d::b_write(vsl_b_ostream& bfs) const
00482 {
00483 vsl_b_write(bfs,version_no());
00484 vsl_b_write(bfs,Wx_); vsl_b_write(bfs,Wy_);vsl_b_write(bfs,Wz_);
00485 vsl_b_write(bfs,Ax0_); vsl_b_write(bfs,AxX_);
00486 vsl_b_write(bfs,AxY_); vsl_b_write(bfs,AxZ_);
00487 vsl_b_write(bfs,Ay0_); vsl_b_write(bfs,AyX_);
00488 vsl_b_write(bfs,AyY_); vsl_b_write(bfs,AyZ_);
00489 vsl_b_write(bfs,Az0_); vsl_b_write(bfs,AzX_);
00490 vsl_b_write(bfs,AzY_); vsl_b_write(bfs,AzZ_);
00491 vsl_b_write(bfs,energy_x_);
00492 vsl_b_write(bfs,energy_y_);
00493 vsl_b_write(bfs,energy_z_);
00494 vsl_b_write(bfs,src_pts_);
00495 vsl_b_write(bfs,L_inv_);
00496 }
00497
00498
00499
00500
00501
00502
00503 void mbl_thin_plate_spline_weights_3d::b_read(vsl_b_istream& bfs)
00504 {
00505 if (!bfs) return;
00506
00507 short version;
00508 vsl_b_read(bfs,version);
00509 switch (version)
00510 {
00511 case (1):
00512 vsl_b_read(bfs,Wx_); vsl_b_read(bfs,Wy_);vsl_b_read(bfs,Wz_);
00513 vsl_b_read(bfs,Ax0_); vsl_b_read(bfs,AxX_);
00514 vsl_b_read(bfs,AxY_); vsl_b_read(bfs,AxZ_);
00515 vsl_b_read(bfs,Ay0_); vsl_b_read(bfs,AyX_);
00516 vsl_b_read(bfs,AyY_); vsl_b_read(bfs,AyZ_);
00517 vsl_b_read(bfs,Az0_); vsl_b_read(bfs,AzX_);
00518 vsl_b_read(bfs,AzY_); vsl_b_read(bfs,AzZ_);
00519 vsl_b_read(bfs,energy_x_);
00520 vsl_b_read(bfs,energy_y_);
00521 vsl_b_read(bfs,energy_z_);
00522 vsl_b_read(bfs,src_pts_);
00523 vsl_b_read(bfs,L_inv_);
00524 break;
00525 default:
00526 vcl_cerr << "I/O ERROR: vsl_b_read(vsl_b_istream&, mbl_thin_plate_spline_weights_3d &)\n"
00527 << " Unknown version number "<< version << '\n';
00528 bfs.is().clear(vcl_ios::badbit);
00529 return;
00530 }
00531 }
00532
00533
00534 bool mbl_thin_plate_spline_weights_3d::operator==(const mbl_thin_plate_spline_weights_3d& tps) const
00535 {
00536 if (&tps==this) return true;
00537 if (vcl_fabs(Ax0_-tps.Ax0_)>1e-8) return false;
00538 if (vcl_fabs(AxX_-tps.AxX_)>1e-8) return false;
00539 if (vcl_fabs(AxY_-tps.AxY_)>1e-8) return false;
00540 if (vcl_fabs(AxZ_-tps.AxZ_)>1e-8) return false;
00541 if (vcl_fabs(Ay0_-tps.Ay0_)>1e-8) return false;
00542 if (vcl_fabs(AyX_-tps.AyX_)>1e-8) return false;
00543 if (vcl_fabs(AyY_-tps.AyY_)>1e-8) return false;
00544 if (vcl_fabs(AyZ_-tps.AyZ_)>1e-8) return false;
00545 if (vcl_fabs(Az0_-tps.Az0_)>1e-8) return false;
00546 if (vcl_fabs(AzX_-tps.AzX_)>1e-8) return false;
00547 if (vcl_fabs(AzY_-tps.AzY_)>1e-8) return false;
00548 if (vcl_fabs(AzZ_-tps.AzZ_)>1e-8) return false;
00549 if (vnl_vector_ssd(Wx_,tps.Wx_)>1e-6) return false;
00550 if (vnl_vector_ssd(Wy_,tps.Wy_)>1e-6) return false;
00551 if (vnl_vector_ssd(Wz_,tps.Wz_)>1e-6) return false;
00552 return true;
00553 }
00554
00555
00556
00557
00558
00559 void vsl_b_write(vsl_b_ostream& bfs, const mbl_thin_plate_spline_weights_3d& b)
00560 {
00561 b.b_write(bfs);
00562 }
00563
00564
00565
00566
00567
00568 void vsl_b_read(vsl_b_istream& bfs, mbl_thin_plate_spline_weights_3d& b)
00569 {
00570 b.b_read(bfs);
00571 }
00572
00573
00574
00575
00576
00577 vcl_ostream& operator<<(vcl_ostream& os,const mbl_thin_plate_spline_weights_3d& b)
00578 {
00579 os << "mbl_thin_plate_spline_weights_3d: ";
00580 vsl_indent_inc(os);
00581 b.print_summary(os);
00582 vsl_indent_dec(os);
00583 return os;
00584 }