contrib/rpl/rgrl/rgrl_est_homo2d_proj

Go to the documentation of this file.
00001 //:
00002 // \file
00003 #include "rgrl_est_homo2d_proj_rad.h"
00004 
00005 #include <rgrl/rgrl_est_homography2d.h>
00006 #include <rgrl/rgrl_est_proj_rad_func.h>
00007 #include <rgrl/rgrl_trans_homography2d.h>
00008 #include <rgrl/rgrl_trans_homo2d_proj_rad.h>
00009 #include <rgrl/rgrl_trans_rad_dis_homo2d.h>
00010 #include <rgrl/rgrl_match_set.h>
00011 #include <rgrl/rgrl_internal_util.h>
00012 
00013 #include <vnl/vnl_double_2.h>
00014 #include <vul/vul_sprintf.h>
00015 
00016 // --------------------------------------------------------------------
00017 
00018 rgrl_est_homo2d_proj_rad::
00019 rgrl_est_homo2d_proj_rad( unsigned int rad_order,
00020                           vnl_vector_fixed<double, 2> const& to_camera_centre,
00021                           bool with_grad )
00022   : to_camera_centre_( to_camera_centre ),
00023     camera_dof_( rad_order ),
00024     with_grad_( with_grad )
00025 {
00026   rgrl_estimator::set_param_dof( 8+camera_dof_ );
00027 
00028   // default value
00029   rgrl_nonlinear_estimator::set_max_num_iter( 50 );
00030   rgrl_nonlinear_estimator::set_rel_thres( 1e-5 );
00031 
00032   // setting up transform name
00033   transform_name_ = vul_sprintf( "rgrl_trans_homo2d_proj_rad+radial-%d", camera_dof_ );
00034 }
00035 
00036 rgrl_transformation_sptr
00037 rgrl_est_homo2d_proj_rad::
00038 estimate( rgrl_set_of<rgrl_match_set_sptr> const& matches,
00039           rgrl_transformation const& cur_transform ) const
00040 {
00041   // get initialization
00042   vnl_matrix_fixed<double, 3, 3> init_H;
00043   vcl_vector<double> radk( camera_dof_, 0.0 );
00044 
00045   if ( cur_transform.is_type( rgrl_trans_homo2d_proj_rad::type_id() ) )
00046   {
00047     rgrl_trans_homo2d_proj_rad const& trans = static_cast<rgrl_trans_homo2d_proj_rad const&>( cur_transform );
00048     init_H = trans.H();
00049     const vcl_vector<double> k = trans.normalized_radial_params();
00050     for ( unsigned int i=0; i<k.size()&&i<camera_dof_; ++i )
00051       radk[i] = k[i];
00052   }
00053   else if ( cur_transform.is_type( rgrl_trans_rad_dis_homo2d::type_id() ) )
00054   {
00055     rgrl_trans_rad_dis_homo2d const& trans = static_cast<rgrl_trans_rad_dis_homo2d const&>( cur_transform );
00056     init_H = trans.uncenter_H_matrix();
00057     const double k1_to   = trans.k1_to();
00058     radk[0] = k1_to;
00059   }
00060   else
00061   {
00062     if ( !rgrl_internal_util_upgrade_to_homography2D( init_H, cur_transform ) )
00063     {
00064       // use normalized DLT to initialize
00065       DebugMacro( 0, "Use normalized DLT to initialize" );
00066       rgrl_est_homography2d est_homo;
00067       rgrl_transformation_sptr tmp_trans= est_homo.estimate( matches, cur_transform );
00068       if ( !tmp_trans )
00069         return 0;
00070       rgrl_trans_homography2d const& trans = static_cast<rgrl_trans_homography2d const&>( *tmp_trans );
00071       init_H = trans.uncenter_H_matrix();
00072     }
00073   }
00074 
00075   // construct least square cost function
00076   rgrl_est_proj_rad_func<2,2> homo_func( matches, camera_dof_, with_grad_ );
00077   homo_func.set_max_num_iter( max_num_iterations_ );
00078   homo_func.set_rel_thres( relative_threshold_ );
00079 
00080   // apply estimation
00081   vnl_double_2 from_centre, to_centre;
00082   vnl_matrix<double> covar;
00083   if ( !homo_func.projective_estimate( init_H, radk,
00084                                        covar,
00085                                        from_centre, to_centre,
00086                                        to_camera_centre_ ) ) {
00087     WarningMacro( "L-M estimation failed." << vcl_endl );
00088     return 0;
00089   }
00090 
00091   return new rgrl_trans_homo2d_proj_rad( radk,
00092                                          init_H.as_ref(),
00093                                          to_camera_centre_.as_ref(),
00094                                          covar,
00095                                          from_centre.as_ref(), to_centre.as_ref() );
00096 }
00097 
00098 
00099 const vcl_type_info&
00100 rgrl_est_homo2d_proj_rad::
00101 transformation_type() const
00102 {
00103   return rgrl_trans_homo2d_proj_rad::type_id();
00104 }