Go to the documentation of this file.00001 #include "rgrl_scale_est_all_weights.h"
00002
00003
00004
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& ,
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
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
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
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
00132
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;
00139 }