00001
00002 #ifdef VCL_NEEDS_PRAGMA_INTERFACE
00003 #pragma implementation
00004 #endif
00005
00006
00007
00008
00009
00010
00011 #include "vnl_sparse_lst_sqr_function.h"
00012 #include <vcl_iostream.h>
00013 #include <vcl_cassert.h>
00014 #include <vnl/vnl_vector_ref.h>
00015
00016 void vnl_sparse_lst_sqr_function::dim_warning(unsigned int nr_of_unknowns,
00017 unsigned int nr_of_residuals)
00018 {
00019 if (nr_of_unknowns > nr_of_residuals)
00020 vcl_cerr << "vnl_sparse_lst_sqr_function: WARNING: "
00021 << "unknowns(" << nr_of_unknowns << ") > "
00022 << "residuals("<< nr_of_residuals << ")\n";
00023 }
00024
00025
00026
00027
00028
00029
00030
00031
00032 vnl_sparse_lst_sqr_function::vnl_sparse_lst_sqr_function(
00033 unsigned int num_a,
00034 unsigned int num_params_per_a,
00035 unsigned int num_b,
00036 unsigned int num_params_per_b,
00037 unsigned int num_params_c,
00038 unsigned int num_residuals_per_e,
00039 UseGradient g,
00040 UseWeights w)
00041 : failure(false),
00042 residual_indices_(),
00043 indices_a_(num_a+1,0),
00044 indices_b_(num_b+1,0),
00045 num_params_c_(num_params_c),
00046 indices_e_(num_a*num_b+1,0),
00047 use_gradient_(g == use_gradient),
00048 use_weights_(w == use_weights)
00049 {
00050 unsigned int k = num_params_per_a;
00051 for (unsigned int i=1; i<indices_a_.size(); ++i, k+=num_params_per_a)
00052 indices_a_[i] = k;
00053
00054 k = num_params_per_b;
00055 for (unsigned int i=1; i<indices_b_.size(); ++i, k+=num_params_per_b)
00056 indices_b_[i] = k;
00057
00058 k = num_residuals_per_e;
00059 for (unsigned int i=1; i<indices_e_.size(); ++i, k+=num_residuals_per_e)
00060 indices_e_[i] = k;
00061 }
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071 vnl_sparse_lst_sqr_function::vnl_sparse_lst_sqr_function(
00072 unsigned int num_a,
00073 unsigned int num_params_per_a,
00074 unsigned int num_b,
00075 unsigned int num_params_per_b,
00076 unsigned int num_params_c,
00077 const vcl_vector<vcl_vector<bool> >& xmask,
00078 unsigned int num_residuals_per_e,
00079 UseGradient g,
00080 UseWeights w)
00081 : failure(false),
00082 residual_indices_(xmask),
00083 indices_a_(num_a+1,0),
00084 indices_b_(num_b+1,0),
00085 num_params_c_(num_params_c),
00086 indices_e_(residual_indices_.num_non_zero()+1,0),
00087 use_gradient_(g == use_gradient),
00088 use_weights_(w == use_weights)
00089 {
00090 unsigned int k = num_params_per_a;
00091 for (unsigned int i=1; i<indices_a_.size(); ++i, k+=num_params_per_a)
00092 indices_a_[i] = k;
00093
00094 k = num_params_per_b;
00095 for (unsigned int i=1; i<indices_b_.size(); ++i, k+=num_params_per_b)
00096 indices_b_[i] = k;
00097
00098 k = num_residuals_per_e;
00099 for (unsigned int i=1; i<indices_e_.size(); ++i, k+=num_residuals_per_e)
00100 indices_e_[i] = k;
00101
00102 dim_warning(num_a*num_params_per_a + num_b*num_params_per_b + num_params_c, k);
00103 }
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116 vnl_sparse_lst_sqr_function::vnl_sparse_lst_sqr_function(
00117 const vcl_vector<unsigned int>& a_sizes,
00118 const vcl_vector<unsigned int>& b_sizes,
00119 unsigned int num_params_c,
00120 const vcl_vector<unsigned int>& e_sizes,
00121 const vcl_vector<vcl_vector<bool> >& xmask,
00122 UseGradient g,
00123 UseWeights w)
00124 : failure(false),
00125 residual_indices_(xmask),
00126 indices_a_(a_sizes.size()+1,0),
00127 indices_b_(b_sizes.size()+1,0),
00128 num_params_c_(num_params_c),
00129 indices_e_(e_sizes.size()+1,0),
00130 use_gradient_(g == use_gradient),
00131 use_weights_(w == use_weights)
00132 {
00133 assert(residual_indices_.num_non_zero() == (int)e_sizes.size());
00134 assert(residual_indices_.num_rows() == (int)a_sizes.size());
00135 assert(residual_indices_.num_cols() == (int)b_sizes.size());
00136
00137 for (unsigned int i=0; i<a_sizes.size(); ++i)
00138 indices_a_[i+1] = indices_a_[i]+a_sizes[i];
00139
00140 for (unsigned int i=0; i<b_sizes.size(); ++i)
00141 indices_b_[i+1] = indices_b_[i]+b_sizes[i];
00142
00143 for (unsigned int i=0; i<e_sizes.size(); ++i)
00144 indices_e_[i+1] = indices_e_[i]+e_sizes[i];
00145
00146 dim_warning(indices_a_.back() + indices_b_.back() + num_params_c,
00147 indices_e_.back());
00148 }
00149
00150
00151
00152
00153
00154
00155
00156
00157 void
00158 vnl_sparse_lst_sqr_function::f(vnl_vector<double> const& a,
00159 vnl_vector<double> const& b,
00160 vnl_vector<double> const& c,
00161 vnl_vector<double>& e)
00162 {
00163 typedef vnl_crs_index::sparse_vector::iterator sv_itr;
00164 for (unsigned int i=0; i<number_of_a(); ++i)
00165 {
00166
00167 const vnl_vector_ref<double> ai(number_of_params_a(i),
00168 const_cast<double*>(a.data_block())+index_a(i));
00169
00170 vnl_crs_index::sparse_vector row = residual_indices_.sparse_row(i);
00171 for (sv_itr r_itr=row.begin(); r_itr!=row.end(); ++r_itr)
00172 {
00173 unsigned int j = r_itr->second;
00174 unsigned int k = r_itr->first;
00175
00176 const vnl_vector_ref<double> bj(number_of_params_b(j),
00177 const_cast<double*>(b.data_block())+index_b(j));
00178 vnl_vector_ref<double> eij(number_of_residuals(k), e.data_block()+index_e(k));
00179 fij(i,j,ai,bj,c,eij);
00180 }
00181 }
00182 }
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193 void
00194 vnl_sparse_lst_sqr_function::jac_blocks(vnl_vector<double> const& a,
00195 vnl_vector<double> const& b,
00196 vnl_vector<double> const& c,
00197 vcl_vector<vnl_matrix<double> >& A,
00198 vcl_vector<vnl_matrix<double> >& B,
00199 vcl_vector<vnl_matrix<double> >& C)
00200 {
00201 typedef vnl_crs_index::sparse_vector::iterator sv_itr;
00202 for (unsigned int i=0; i<number_of_a(); ++i)
00203 {
00204
00205 const vnl_vector_ref<double> ai(number_of_params_a(i),
00206 const_cast<double*>(a.data_block())+index_a(i));
00207
00208 vnl_crs_index::sparse_vector row = residual_indices_.sparse_row(i);
00209 for (sv_itr r_itr=row.begin(); r_itr!=row.end(); ++r_itr)
00210 {
00211 unsigned int j = r_itr->second;
00212 unsigned int k = r_itr->first;
00213
00214 const vnl_vector_ref<double> bj(number_of_params_b(j),
00215 const_cast<double*>(b.data_block())+index_b(j));
00216
00217 jac_Aij(i,j,ai,bj,c,A[k]);
00218 jac_Bij(i,j,ai,bj,c,B[k]);
00219 jac_Cij(i,j,ai,bj,c,C[k]);
00220 }
00221 }
00222 }
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233 void
00234 vnl_sparse_lst_sqr_function::fd_jac_blocks(vnl_vector<double> const& a,
00235 vnl_vector<double> const& b,
00236 vnl_vector<double> const& c,
00237 vcl_vector<vnl_matrix<double> >& A,
00238 vcl_vector<vnl_matrix<double> >& B,
00239 vcl_vector<vnl_matrix<double> >& C,
00240 double stepsize)
00241 {
00242 typedef vnl_crs_index::sparse_vector::iterator sv_itr;
00243 for (unsigned int i=0; i<number_of_a(); ++i)
00244 {
00245
00246 const vnl_vector_ref<double> ai(number_of_params_a(i),
00247 const_cast<double*>(a.data_block())+index_a(i));
00248
00249 vnl_crs_index::sparse_vector row = residual_indices_.sparse_row(i);
00250 for (sv_itr r_itr=row.begin(); r_itr!=row.end(); ++r_itr)
00251 {
00252 unsigned int j = r_itr->second;
00253 unsigned int k = r_itr->first;
00254
00255 const vnl_vector_ref<double> bj(number_of_params_b(j),
00256 const_cast<double*>(b.data_block())+index_b(j));
00257
00258 fd_jac_Aij(i,j,ai,bj,c,A[k],stepsize);
00259 fd_jac_Bij(i,j,ai,bj,c,B[k],stepsize);
00260 fd_jac_Cij(i,j,ai,bj,c,C[k],stepsize);
00261 }
00262 }
00263 }
00264
00265
00266
00267
00268
00269
00270
00271
00272 void
00273 vnl_sparse_lst_sqr_function::compute_weights(vnl_vector<double> const& a,
00274 vnl_vector<double> const& b,
00275 vnl_vector<double> const& c,
00276 vnl_vector<double> const& e,
00277 vnl_vector<double>& weights)
00278 {
00279 typedef vnl_crs_index::sparse_vector::iterator sv_itr;
00280 for (unsigned int i=0; i<number_of_a(); ++i)
00281 {
00282
00283 const vnl_vector_ref<double> ai(number_of_params_a(i),
00284 const_cast<double*>(a.data_block())+index_a(i));
00285
00286 vnl_crs_index::sparse_vector row = residual_indices_.sparse_row(i);
00287 for (sv_itr r_itr=row.begin(); r_itr!=row.end(); ++r_itr)
00288 {
00289 unsigned int j = r_itr->second;
00290 unsigned int k = r_itr->first;
00291
00292 const vnl_vector_ref<double> bj(number_of_params_b(j),
00293 const_cast<double*>(b.data_block())+index_b(j));
00294 const vnl_vector_ref<double> eij(number_of_residuals(k),
00295 const_cast<double*>(e.data_block()+index_e(k)));
00296 compute_weight_ij(i,j,ai,bj,c,eij,weights[k]);
00297 }
00298 }
00299 }
00300
00301
00302
00303
00304
00305
00306
00307 void
00308 vnl_sparse_lst_sqr_function::apply_weights(vnl_vector<double> const& weights,
00309 vnl_vector<double>& e)
00310 {
00311 typedef vnl_crs_index::sparse_vector::iterator sv_itr;
00312 for (unsigned int i=0; i<number_of_a(); ++i)
00313 {
00314 vnl_crs_index::sparse_vector row = residual_indices_.sparse_row(i);
00315 for (sv_itr r_itr=row.begin(); r_itr!=row.end(); ++r_itr)
00316 {
00317 unsigned int j = r_itr->second;
00318 unsigned int k = r_itr->first;
00319 vnl_vector_ref<double> eij(number_of_residuals(k), e.data_block()+index_e(k));
00320 apply_weight_ij(i,j,weights[k],eij);
00321 }
00322 }
00323 }
00324
00325
00326
00327
00328
00329
00330
00331 void
00332 vnl_sparse_lst_sqr_function::apply_weights(vnl_vector<double> const& weights,
00333 vcl_vector<vnl_matrix<double> >& A,
00334 vcl_vector<vnl_matrix<double> >& B,
00335 vcl_vector<vnl_matrix<double> >& C)
00336 {
00337 typedef vnl_crs_index::sparse_vector::iterator sv_itr;
00338 for (unsigned int i=0; i<number_of_a(); ++i)
00339 {
00340 vnl_crs_index::sparse_vector row = residual_indices_.sparse_row(i);
00341 for (sv_itr r_itr=row.begin(); r_itr!=row.end(); ++r_itr)
00342 {
00343 unsigned int j = r_itr->second;
00344 unsigned int k = r_itr->first;
00345 apply_weight_ij(i,j,weights[k],A[k],B[k],C[k]);
00346 }
00347 }
00348 }
00349
00350
00351
00352
00353
00354 void
00355 vnl_sparse_lst_sqr_function::fij(int i, int j,
00356 vnl_vector<double> const& ai,
00357 vnl_vector<double> const& bj,
00358 vnl_vector<double> const& c,
00359 vnl_vector<double>& f_i_j)
00360 {
00361 vcl_cerr << "Warning: fij() called but not implemented in derived class\n";
00362 }
00363
00364
00365 void
00366 vnl_sparse_lst_sqr_function::jac_Aij(int i, int j,
00367 vnl_vector<double> const& ai,
00368 vnl_vector<double> const& bj,
00369 vnl_vector<double> const& c,
00370 vnl_matrix<double>& Aij)
00371 {
00372 vcl_cerr << "Warning: jac_Aij() called but not implemented in derived class\n";
00373 }
00374
00375
00376 void
00377 vnl_sparse_lst_sqr_function::jac_Bij(int i, int j,
00378 vnl_vector<double> const& ai,
00379 vnl_vector<double> const& bj,
00380 vnl_vector<double> const& c,
00381 vnl_matrix<double>& Bij)
00382 {
00383 vcl_cerr << "Warning: jac_Bij() called but not implemented in derived class\n";
00384 }
00385
00386
00387 void
00388 vnl_sparse_lst_sqr_function::jac_Cij(int i, int j,
00389 vnl_vector<double> const& ai,
00390 vnl_vector<double> const& bj,
00391 vnl_vector<double> const& c,
00392 vnl_matrix<double>& Cij)
00393 {
00394 if(num_params_c_ > 0)
00395 vcl_cerr << "Warning: jac_Cij() called but not implemented in derived class\n";
00396 }
00397
00398
00399 void
00400 vnl_sparse_lst_sqr_function::fd_jac_Aij(int i, int j,
00401 vnl_vector<double> const& ai,
00402 vnl_vector<double> const& bj,
00403 vnl_vector<double> const& c,
00404 vnl_matrix<double>& Aij,
00405 double stepsize)
00406 {
00407 const unsigned int dim = ai.size();
00408 const unsigned int n = Aij.rows();
00409 assert(dim == number_of_params_a(i));
00410 assert(n == number_of_residuals(i,j));
00411 assert(dim == Aij.columns());
00412
00413 vnl_vector<double> tai = ai;
00414 vnl_vector<double> fplus(n);
00415 vnl_vector<double> fminus(n);
00416
00417
00418 for (unsigned int ii = 0; ii < dim; ++ii)
00419 {
00420
00421 double tplus = tai[ii] = ai[ii] + stepsize;
00422 this->fij(i,j,tai,bj,c,fplus);
00423
00424
00425 double tminus = tai[ii] = ai[ii] - stepsize;
00426 this->fij(i,j,tai,bj,c,fminus);
00427
00428 double h = 1.0 / (tplus - tminus);
00429 for (unsigned int jj = 0; jj < n; ++jj)
00430 Aij(jj,ii) = (fplus[jj] - fminus[jj]) * h;
00431
00432
00433 tai[ii] = ai[ii];
00434 }
00435 }
00436
00437
00438
00439 void
00440 vnl_sparse_lst_sqr_function::fd_jac_Bij(int i, int j,
00441 vnl_vector<double> const& ai,
00442 vnl_vector<double> const& bj,
00443 vnl_vector<double> const& c,
00444 vnl_matrix<double>& Bij,
00445 double stepsize)
00446 {
00447 const unsigned int dim = bj.size();
00448 const unsigned int n = Bij.rows();
00449 assert(dim == number_of_params_b(j));
00450 assert(n == number_of_residuals(i,j));
00451 assert(dim == Bij.columns());
00452
00453 vnl_vector<double> tbj = bj;
00454 vnl_vector<double> fplus(n);
00455 vnl_vector<double> fminus(n);
00456
00457
00458 for (unsigned int ii = 0; ii < dim; ++ii)
00459 {
00460
00461 double tplus = tbj[ii] = bj[ii] + stepsize;
00462 this->fij(i,j,ai,tbj,c,fplus);
00463
00464
00465 double tminus = tbj[ii] = bj[ii] - stepsize;
00466 this->fij(i,j,ai,tbj,c,fminus);
00467
00468 double h = 1.0 / (tplus - tminus);
00469 for (unsigned int jj = 0; jj < n; ++jj)
00470 Bij(jj,ii) = (fplus[jj] - fminus[jj]) * h;
00471
00472
00473 tbj[ii] = bj[ii];
00474 }
00475 }
00476
00477
00478
00479 void
00480 vnl_sparse_lst_sqr_function::fd_jac_Cij(int i, int j,
00481 vnl_vector<double> const& ai,
00482 vnl_vector<double> const& bj,
00483 vnl_vector<double> const& c,
00484 vnl_matrix<double>& Cij,
00485 double stepsize)
00486 {
00487 const unsigned int dim = c.size();
00488 const unsigned int n = Cij.rows();
00489 assert(dim == number_of_params_c());
00490 assert(n == number_of_residuals(i,j));
00491 assert(dim == Cij.columns());
00492
00493 vnl_vector<double> tc = c;
00494 vnl_vector<double> fplus(n);
00495 vnl_vector<double> fminus(n);
00496
00497
00498 for (unsigned int ii = 0; ii < dim; ++ii)
00499 {
00500
00501 double tplus = tc[ii] = c[ii] + stepsize;
00502 this->fij(i,j,ai,bj,tc,fplus);
00503
00504
00505 double tminus = tc[ii] = c[ii] - stepsize;
00506 this->fij(i,j,ai,bj,tc,fminus);
00507
00508 double h = 1.0 / (tplus - tminus);
00509 for (unsigned int jj = 0; jj < n; ++jj)
00510 Cij(jj,ii) = (fplus[jj] - fminus[jj]) * h;
00511
00512
00513 tc[ii] = c[ii];
00514 }
00515 }
00516
00517
00518
00519
00520
00521 void
00522 vnl_sparse_lst_sqr_function::compute_weight_ij(int , int ,
00523 vnl_vector<double> const& ,
00524 vnl_vector<double> const& ,
00525 vnl_vector<double> const& ,
00526 vnl_vector<double> const& ,
00527 double& weight)
00528 {
00529 weight = 1.0;
00530 }
00531
00532
00533
00534
00535 void
00536 vnl_sparse_lst_sqr_function::apply_weight_ij(int , int ,
00537 double const& weight,
00538 vnl_vector<double>& fij)
00539 {
00540 fij *= weight;
00541 }
00542
00543
00544
00545
00546 void
00547 vnl_sparse_lst_sqr_function::apply_weight_ij(int , int ,
00548 double const& weight,
00549 vnl_matrix<double>& Aij,
00550 vnl_matrix<double>& Bij,
00551 vnl_matrix<double>& Cij)
00552 {
00553 Aij *= weight;
00554 Bij *= weight;
00555 Cij *= weight;
00556 }
00557
00558
00559 void vnl_sparse_lst_sqr_function::trace(int ,
00560 vnl_vector<double> const& ,
00561 vnl_vector<double> const& ,
00562 vnl_vector<double> const& ,
00563 vnl_vector<double> const& )
00564 {
00565
00566 }
00567
00568