contrib/rpl/rgrl/rgrl_est_homo2d_lm.cxx
Go to the documentation of this file.
00001 #include "rgrl_est_homo2d_lm.h"
00002 //:
00003 // \file
00004 #include <rgrl/rgrl_est_homography2d.h>
00005 #include <rgrl/rgrl_trans_homography2d.h>
00006 #include <rgrl/rgrl_match_set.h>
00007 #include <rgrl/rgrl_internal_util.h>
00008 
00009 #include <vnl/vnl_double_2.h>
00010 #include <vnl/vnl_double_3.h>
00011 #include <vnl/vnl_double_3x3.h>
00012 #include <vnl/vnl_matrix_fixed.h>
00013 #include <vnl/vnl_math.h>
00014 #include <vnl/vnl_det.h>
00015 #include <vnl/vnl_fastops.h>
00016 #include <vnl/vnl_transpose.h>
00017 #include <vnl/vnl_least_squares_function.h>
00018 #include <vnl/algo/vnl_orthogonal_complement.h>
00019 #include <vnl/algo/vnl_levenberg_marquardt.h>
00020 #include <vnl/algo/vnl_svd.h>
00021 
00022 #include <vcl_cassert.h>
00023 
00024 static
00025 inline
00026 void
00027 H2h( vnl_matrix_fixed<double,3,3> const& H, vnl_vector<double>& h )
00028 {
00029   for ( unsigned i=0; i<H.rows(); ++i )
00030     for ( unsigned j=0; j<H.cols(); ++j )
00031       h( i*3+j ) = H(i,j);
00032 }
00033 
00034 static
00035 inline
00036 void
00037 h2H( vnl_vector<double> const& h, vnl_matrix_fixed<double,3,3>& H )
00038 {
00039   for ( unsigned i=0; i<3; ++i )
00040     for ( unsigned j=0; j<3; ++j )
00041       H(i,j) = h( i*3+j );
00042 }
00043 
00044 inline
00045 void
00046 map_homo_point( vnl_double_3& mapped, vnl_vector<double> const& p, vnl_vector<double> const& loc )
00047 {
00048   mapped[0] = p[0]*loc[0] + p[1]*loc[1] + p[2];
00049   mapped[1] = p[3]*loc[0] + p[4]*loc[1] + p[5];
00050   mapped[2] = p[6]*loc[0] + p[7]*loc[1] + p[8];
00051 }
00052 
00053 inline
00054 void
00055 map_inhomo_point( vnl_double_2& mapped, vnl_vector<double> const& p, vnl_vector<double> const& loc )
00056 {
00057   vnl_double_3 tmp;
00058   map_homo_point( tmp, p, loc );
00059   mapped[0] = tmp[0]/tmp[2];
00060   mapped[1] = tmp[1]/tmp[2];
00061 }
00062 
00063 class rgrl_homo2d_func
00064 : public vnl_least_squares_function
00065 {
00066  public:
00067   //: ctor
00068   rgrl_homo2d_func( rgrl_set_of<rgrl_match_set_sptr> const& matches,
00069                     int num_res, bool with_grad = true )
00070   : vnl_least_squares_function( 9, num_res, with_grad ? use_gradient : no_gradient ),
00071     matches_ptr_( &matches ),
00072     from_centre_(2, 0.0), to_centre_(2, 0.0)
00073   {      }
00074 
00075   void set_centres( vnl_vector<double> const& fc, vnl_vector<double> const& tc )
00076   {
00077     assert( fc.size() == 2 && tc.size() == 2 );
00078     from_centre_ = fc;
00079     to_centre_ = tc;
00080   }
00081 
00082   //: obj func value
00083   void f(vnl_vector<double> const& x, vnl_vector<double>& fx);
00084 
00085   //: Jacobian
00086   void gradf(vnl_vector<double> const& x, vnl_matrix<double>& jacobian);
00087 
00088  protected:
00089   typedef rgrl_match_set::const_from_iterator FIter;
00090   typedef FIter::to_iterator TIter;
00091 
00092   rgrl_set_of<rgrl_match_set_sptr> const* matches_ptr_;
00093   vnl_double_2                            from_centre_, to_centre_;
00094 };
00095 
00096 void
00097 rgrl_homo2d_func::
00098 f(vnl_vector<double> const& x, vnl_vector<double>& fx)
00099 {
00100   vnl_double_2 mapped;
00101   vnl_matrix_fixed<double,2,2> error_proj_sqrt;
00102   unsigned int ind = 0;
00103   for ( unsigned ms = 0; ms<matches_ptr_->size(); ++ms )
00104     if ( (*matches_ptr_)[ms] != 0 ) { // if pointer is valid
00105       rgrl_match_set const& one_set = *((*matches_ptr_)[ms]);
00106       for ( FIter fi=one_set.from_begin(); fi!=one_set.from_end(); ++fi ) {
00107         // map from point
00108         vnl_double_2 from = fi.from_feature()->location();
00109         from -= from_centre_;
00110         map_inhomo_point( mapped, x, from.as_ref() );
00111 
00112         for ( TIter ti=fi.begin(); ti!=fi.end(); ++ti ) {
00113           vnl_double_2 to = ti.to_feature()->location();
00114           to -= to_centre_;
00115           error_proj_sqrt = ti.to_feature()->error_projector_sqrt();
00116           double const wgt = vcl_sqrt(ti.cumulative_weight());
00117           vnl_double_2 diff = error_proj_sqrt * (mapped - to);
00118 
00119           // fill in
00120           fx(ind) = wgt*diff[0];
00121           fx(ind+1) = wgt*diff[1];
00122           ind+=2;
00123         }
00124       }
00125   }
00126 
00127   // check
00128   assert( ind == get_number_of_residuals() );
00129 }
00130 
00131 void
00132 rgrl_homo2d_func::
00133 gradf(vnl_vector<double> const& x, vnl_matrix<double>& jacobian)
00134 {
00135   assert( jacobian.rows() == get_number_of_residuals() && jacobian.cols() == 9 );
00136 
00137   vnl_double_3 homo;
00138   vnl_matrix_fixed<double,2,2> error_proj_sqrt;
00139   vnl_matrix_fixed<double,2,9> base_jac, jac;
00140   vnl_matrix_fixed<double,3,9> jf(0.0); // homogeneous coordinate
00141   vnl_matrix_fixed<double,2,3> jg(0.0); // inhomo, [u/w, v/w]^T
00142   unsigned int ind = 0;
00143   for ( unsigned ms = 0; ms<matches_ptr_->size(); ++ms )
00144     if ( (*matches_ptr_)[ms] ) { // if pointer is valid
00145       rgrl_match_set const& one_set = *((*matches_ptr_)[ms]);
00146       for ( FIter fi=one_set.from_begin(); fi!=one_set.from_end(); ++fi ) {
00147         // map from point
00148         vnl_double_2 from = fi.from_feature()->location();
00149         from -= from_centre_;
00150         map_homo_point( homo, x, from.as_ref() );
00151         // homogeneous coordinate
00152         jf(0,0) = jf(1,3) = jf(2,6) = from[0]; // x
00153         jf(0,1) = jf(1,4) = jf(2,7) = from[1]; // y
00154         jf(0,2) = jf(1,5) = jf(2,8) = 1.0;
00155         // make division
00156         jg(0,0) = 1.0/homo[2];
00157         jg(0,2) = -homo[0]/vnl_math::sqr(homo[2]);
00158         jg(1,1) = 1.0/homo[2];
00159         jg(1,2) = -homo[1]/vnl_math::sqr(homo[2]);
00160         // since Jab_g(f(p)) = Jac_g * Jac_f
00161         base_jac = jg * jf;
00162 
00163         for ( TIter ti=fi.begin(); ti!=fi.end(); ++ti ) {
00164           //vnl_double_2 to = ti.to_feature()->location();
00165           error_proj_sqrt = ti.to_feature()->error_projector_sqrt();
00166           double const wgt = vcl_sqrt(ti.cumulative_weight());
00167           jac = error_proj_sqrt * base_jac;
00168           jac *= wgt;
00169 
00170           // fill in
00171           for ( unsigned i=0; i<9; i++ ) {
00172             jacobian(ind, i)   = jac(0, i);
00173             jacobian(ind+1, i) = jac(1, i);
00174           }
00175           ind+=2;
00176         }
00177       }
00178   }
00179 }
00180 
00181 
00182 // --------------------------------------------------------------------
00183 
00184 rgrl_est_homo2d_lm::
00185 rgrl_est_homo2d_lm( bool with_grad )
00186   : with_grad_( with_grad )
00187 {
00188    rgrl_estimator::set_param_dof( 8 );
00189 
00190   // default value
00191   rgrl_nonlinear_estimator::set_max_num_iter( 50 );
00192   rgrl_nonlinear_estimator::set_rel_thres( 1e-5 );
00193 }
00194 
00195 rgrl_transformation_sptr
00196 rgrl_est_homo2d_lm::
00197 estimate( rgrl_set_of<rgrl_match_set_sptr> const& matches,
00198           rgrl_transformation const& cur_transform ) const
00199 {
00200   // get initialization
00201   vnl_matrix_fixed<double, 3, 3> init_H;
00202 
00203   if ( !rgrl_internal_util_upgrade_to_homography2D( init_H, cur_transform ) ) {
00204 
00205     // use normalized DLT to initialize
00206     DebugMacro( 0, "Use normalized DLT to initialize" );
00207     rgrl_est_homography2d est_homo;
00208     rgrl_transformation_sptr tmp_trans= est_homo.estimate( matches, cur_transform );
00209     if ( !tmp_trans )
00210       return 0;
00211     rgrl_trans_homography2d const& trans = static_cast<rgrl_trans_homography2d const&>( *tmp_trans );
00212     init_H = trans.H();
00213   }
00214 
00215   // Determine the weighted centres for computing the more stable
00216   // covariance matrix of homography parameters
00217   //
00218   vnl_vector<double> from_centre;
00219   vnl_vector<double> to_centre;
00220   if ( !rgrl_est_compute_weighted_centres( matches, from_centre, to_centre ) )
00221     return 0;
00222    DebugMacro( 3, "From center: " << from_centre
00223                <<"  To center: " << to_centre << vcl_endl );
00224 
00225   // make the init homography as a CENTERED one
00226   {
00227     // centered H_ = to_matrix * H * from_matrix^-1
00228     //
00229     vnl_matrix<double> to_trans( 3, 3, vnl_matrix_identity );
00230     to_trans(0,2) = -to_centre[0];
00231     to_trans(1,2) = -to_centre[1];
00232 
00233     vnl_matrix<double> from_inv( 3, 3, vnl_matrix_identity );
00234     from_inv(0,2) = from_centre[0];
00235     from_inv(1,2) = from_centre[1];
00236 
00237     init_H = to_trans * init_H * from_inv;
00238   }
00239   // convert to vector form
00240   vnl_vector<double> p(9);
00241   H2h( init_H, p );
00242 
00243   // the number of residuals in a vector form
00244   const unsigned tot_num = rgrl_est_matches_residual_number(matches);
00245 
00246   // construct least square cost function
00247   rgrl_homo2d_func homo_func( matches,
00248                               tot_num,
00249                               with_grad_ );
00250   homo_func.set_centres( from_centre, to_centre );
00251 
00252   vnl_levenberg_marquardt lm( homo_func );
00253   //lm.set_trace( true );
00254   //lm.set_check_derivatives( 10 );
00255   // we don't need it to be super accurate
00256   lm.set_f_tolerance( relative_threshold_ );
00257   lm.set_max_function_evals( max_num_iterations_ );
00258 
00259   bool ret;
00260   if ( with_grad_ )
00261     ret = lm.minimize_using_gradient(p);
00262   else
00263     ret = lm.minimize_without_gradient(p);
00264   if ( !ret ) {
00265     WarningMacro( "Levenberg-Marquardt failed" );
00266     return 0;
00267   }
00268   // lm.diagnose_outcome(vcl_cout);
00269 
00270   // normalize H
00271   p /= p.two_norm();
00272   // convert parameters back into matrix form
00273   h2H( p, init_H );
00274   vnl_vector<double> centre(2,0.0);
00275 
00276   // check rank of H
00277   vnl_double_3x3 tmpH(init_H);
00278   if ( vcl_abs(vnl_det(tmpH)) < 1e-8 )
00279     return 0;
00280 
00281   // compute covariance
00282   // JtJ is INVERSE of jacobian
00283   //
00284   // vnl_svd<double> svd( lm.get_JtJ(), 1e-4 );
00285   // Cannot use get_JtJ() because it is affected by the
00286   // scale in homography parameter vector
00287   // Thus, use the normalized p vector to compute Jacobian again
00288   vnl_matrix<double> jac(tot_num, 9), jtj(9, 9);
00289   homo_func.gradf( p, jac );
00290   //
00291   //vnl_matrix<double>  proj(9, 9, vnl_matrix_identity);
00292   //proj -= outer_product( p, p );
00293   //jac *= proj;
00294   vnl_fastops::AtA( jtj, jac );
00295   // vcl_cout << "Jacobian:\n" << jac << "\n\nJtJ:\n" << jtj << "\n\nReal JtJ:\n" << jac.transpose() * jac << vcl_endl;
00296 
00297   // compute inverse
00298   //
00299 #if 1
00300   const vnl_matrix<double> compliment = vnl_orthogonal_complement( p );
00301 
00302   vnl_svd<double> svd( vnl_transpose(compliment) * jtj *compliment, 1e-6 );
00303   if ( svd.rank() < 8 ) {
00304     WarningMacro( "The covariance of homography ranks less than 8! ");
00305     return 0;
00306   }
00307 
00308   vnl_matrix<double>covar = compliment * svd.inverse() * compliment.transpose();
00309 
00310 #else
00311   vnl_svd<double> svd( jtj, 1e-6 );
00312   DebugMacro(3, "SVD of JtJ: " << svd << vcl_endl);
00313   // the second least singular value shall be greater than 0
00314   // or Rank 8
00315   if ( svd.rank() < 8 ) {
00316     WarningMacro( "The covariance of homography ranks less than 8! ");
00317     return 0;
00318   }
00319   // pseudo inverse only use first 8 singular values
00320   vnl_matrix<double> covar ( svd.pinverse(8) );
00321   DebugMacro(3, "Covariance: " << covar << vcl_endl );
00322 
00323   //vnl_vector<double> tmp_f(tot_num,-1.0);
00324   //homo_func.f( svd.nullvector(), tmp_f );
00325   //vcl_cout << "using null vector: " << tmp_f.two_norm() << vcl_endl;
00326   //homo_func.f( p, tmp_f );
00327   //vcl_cout << "using estimate   : " << tmp_f.two_norm() << vcl_endl;
00328   double abs_dot = vcl_abs( dot_product( p, svd.nullvector() ) );
00329   if ( abs_dot < 0.9 ) {
00330     WarningMacro("The null vector of covariance matrix of homography is too DIFFERENT\n"
00331                  << "compared to estimate of homography: dot_product=" << abs_dot << vcl_endl );
00332     // it is considered as failure
00333     return 0;
00334   }
00335 #endif
00336 
00337   DebugMacro(2, "null vector: " << svd.nullvector() << "   estimate: " << p << vcl_endl );
00338 
00339   return new rgrl_trans_homography2d( init_H.as_ref(), covar, from_centre, to_centre );
00340 }
00341 
00342 
00343 const vcl_type_info&
00344 rgrl_est_homo2d_lm::
00345 transformation_type() const
00346 {
00347   return rgrl_trans_homography2d::type_id();
00348 }