contrib/rpl/rgrl/rgrl_scale_est_all_weights.cxx
Go to the documentation of this file.
00001 #include "rgrl_scale_est_all_weights.h"
00002 //:
00003 // \file
00004 // \author Chuck Stewart
00005 
00006 #include <vcl_cmath.h>
00007 
00008 #include <vnl/vnl_math.h>
00009 #include <vnl/algo/vnl_svd.h>
00010 
00011 #include "rgrl_scale.h"
00012 #include "rgrl_match_set.h"
00013 #include "rgrl_util.h"
00014 #include <vcl_iostream.h>
00015 
00016 rgrl_scale_est_all_weights::
00017 rgrl_scale_est_all_weights( bool do_signature_scale )
00018   : do_signature_scale_( do_signature_scale )
00019 {
00020 }
00021 
00022 
00023 rgrl_scale_sptr
00024 rgrl_scale_est_all_weights::
00025 estimate_weighted( rgrl_match_set const& match_set,
00026                    rgrl_scale_sptr const& /*unused*/,
00027                    bool use_signature_only,
00028                    bool penalize_scaling ) const
00029 {
00030   rgrl_scale_sptr scales = new rgrl_scale;
00031   double scale;
00032   vnl_matrix<double> inv_covar;
00033 
00034   if ( compute_geometric_scale( scale, match_set, use_signature_only, penalize_scaling ) )
00035     scales->set_geometric_scale( scale );
00036 
00037   if ( do_signature_scale_ && compute_signature_inv_covar( inv_covar, match_set ) ) {
00038     scales->set_signature_inv_covar( inv_covar );
00039   }
00040 
00041   return scales;
00042 }
00043 
00044 
00045 bool
00046 rgrl_scale_est_all_weights::
00047 compute_geometric_scale( double& return_scale,
00048                          rgrl_match_set const& match_set,
00049                          bool use_signature_wgt,
00050                          bool penalize_scaling  ) const
00051 {
00052   typedef rgrl_match_set::const_from_iterator from_iter;
00053   typedef from_iter::to_iterator              to_iter;
00054 
00055   double sum_weighted_error = 0;
00056   double sum_weights = 0;
00057 
00058   double scaling = 1;
00059   if ( penalize_scaling ) {
00060     //scaling applied here to penalize xform with distortion
00061     scaling = rgrl_util_geometric_error_scaling( match_set );
00062   }
00063 
00064   DebugMacro(2, '\n');
00065   DebugMacro_abv(2, "from\t to\t residuals\t signature_wgt\t cumulative_wgt\t weight :\n");
00066   for ( from_iter fitr = match_set.from_begin(); fitr != match_set.from_end(); ++fitr ) {
00067     rgrl_feature_sptr mapped_from = fitr.mapped_from_feature();
00068     for ( to_iter titr = fitr.begin(); titr != fitr.end(); ++titr ) {
00069       double error = titr.to_feature()->geometric_error( *mapped_from );
00070       double weight;
00071       if ( use_signature_wgt )
00072         weight = titr.signature_weight();
00073       else
00074         weight = titr.cumulative_weight();
00075 
00076       DebugMacro_abv(2, fitr.from_feature()->location() << "\t ");
00077       DebugMacro_abv(2, titr.to_feature()->location() << "\t " << error << "\t ");
00078       DebugMacro_abv(2, titr.signature_weight() << "\t " );
00079       DebugMacro_abv(2, titr.cumulative_weight() << "\t " << weight <<'\n');
00080 
00081       sum_weighted_error += weight * vnl_math_sqr( error );
00082       sum_weights += weight;
00083     }
00084   }
00085   const double epsilon = 1e-16;
00086   double scale = vcl_sqrt( sum_weighted_error / sum_weights );
00087   // is finite?
00088   if ( !vnl_math_isfinite( scale ) )
00089     return false;
00090 
00091   return_scale = scaling * vnl_math_max( scale, epsilon );
00092 
00093   DebugMacro(1, "  Final geometric scale" << return_scale << vcl_endl );
00094   return true;
00095 
00096 #if 0
00097   double est_scale = vcl_sqrt( sum_weighted_error / sum_weights );
00098   vcl_cout << " rgrl_scale_est_all_weights : scale = " << est_scale << " (lower bound=1.0)\n";
00099   return vnl_math_max( est_scale, 1.0 );
00100 #endif
00101 }
00102 
00103 bool
00104 rgrl_scale_est_all_weights::
00105 compute_signature_inv_covar( vnl_matrix<double>& inv_covar, rgrl_match_set const&  match_set ) const
00106 {
00107   typedef rgrl_match_set::const_from_iterator from_iter;
00108   typedef from_iter::to_iterator              to_iter;
00109 
00110   if ( !match_set.from_size() )  return false;
00111 
00112   from_iter fitr = match_set.from_begin();
00113   const unsigned nrows = fitr.from_feature()->signature_error_dimension( match_set.to_feature_type() );
00114 
00115   // check on the error vector dimension
00116   if ( !nrows ) return false;
00117 
00118   vnl_matrix<double> weighted_covar( nrows, nrows, 0.0 );
00119   double sum_weights = 0.0;
00120 
00121   for ( ; fitr != match_set.from_end(); ++fitr ) {
00122     rgrl_feature_sptr mapped_from = fitr.mapped_from_feature();
00123     for ( to_iter titr = fitr.begin(); titr != fitr.end(); ++titr ) {
00124       vnl_vector<double> error_vec = titr.to_feature()->signature_error_vector( *mapped_from );
00125       double weight = titr.cumulative_weight();
00126       weighted_covar += weight * outer_product( error_vec, error_vec );
00127       sum_weights += weight;
00128     }
00129   }
00130 
00131   // compute the inverse of covariance matrix
00132   // use pseudo inverse in case it is degenerate
00133   weighted_covar /= sum_weights;
00134   vnl_svd<double> svd( weighted_covar );
00135   svd.zero_out_absolute();
00136 
00137   inv_covar = svd.inverse();
00138   return true;   // pseudo-inverse at this point
00139 }