contrib/rpl/rgrl/rgrl_est_proj_rad

Go to the documentation of this file.
00001 #ifndef rgrl_est_proj_rad_func_h_
00002 #define rgrl_est_proj_rad_func_h_
00003 
00004 //:
00005 // \file
00006 // \author Gehua Yang
00007 // \date   March 2007
00008 // \brief  a generic class to estimate a homogeneous projection matrix with radial distortion parameter(s)  using L-M
00009 
00010 #include <vnl/vnl_vector_fixed.h>
00011 #include <rgrl/rgrl_fwd.h>
00012 #include <rgrl/rgrl_match_set_sptr.h>
00013 #include <rgrl/rgrl_est_proj_func.h>
00014 #include <vcl_vector.h>
00015 
00016 template <unsigned int Tdim, unsigned int Fdim>
00017 class rgrl_est_proj_rad_func
00018 : public rgrl_est_proj_func<Tdim, Fdim>
00019 {
00020  public:
00021   //: ctor
00022   rgrl_est_proj_rad_func( rgrl_set_of<rgrl_match_set_sptr> const& matches,
00023                           unsigned int camera_dof,
00024                           bool with_grad = true );
00025 
00026   //: ctor without matches
00027   rgrl_est_proj_rad_func( unsigned int camera_dof,
00028                           bool with_grad = true );
00029 
00030   //: estimate the projective transformation and the associated covariance
00031   bool
00032   projective_estimate( vnl_matrix_fixed<double, Tdim+1, Fdim+1>& proj,
00033                        vcl_vector<double>& rad_dist,
00034                        vnl_matrix<double>& full_covar,
00035                        vnl_vector_fixed<double, Fdim>& from_centre,
00036                        vnl_vector_fixed<double, Tdim>& to_centre,
00037                        vnl_vector_fixed<double, Tdim> const& camera_centre);
00038 
00039   //: obj func value
00040   void f(vnl_vector<double> const& x, vnl_vector<double>& fx);
00041 
00042   //: Jacobian
00043   void gradf(vnl_vector<double> const& x, vnl_matrix<double>& jacobian);
00044 
00045   //: set image centre
00046   void
00047   set_image_centre( vnl_vector_fixed<double, Tdim> const& image_centre )
00048   {
00049     centre_mag_norm_const_
00050       = image_centre.squared_magnitude() / 400;
00051     if( centre_mag_norm_const_ < 1.0 )
00052       centre_mag_norm_const_ = 1.0;
00053    image_centre_ = image_centre-this->to_centre_;
00054   }
00055 
00056   //: set from, to, and image centres
00057   void
00058   set_centres( vnl_vector_fixed<double, Fdim> const& from_centre,
00059                vnl_vector_fixed<double, Tdim> const& to_centre,
00060                vnl_vector_fixed<double, Tdim> const& image_centre )
00061   {
00062     rgrl_est_proj_func<Tdim, Fdim>::set_centres( from_centre, to_centre );
00063     centre_mag_norm_const_
00064       = image_centre.squared_magnitude() / 400;
00065     if( centre_mag_norm_const_ < 1.0 )
00066       centre_mag_norm_const_ = 1.0;
00067     image_centre_ = image_centre-this->to_centre_;
00068   }
00069 
00070   //: get centred coordinate normalization constant
00071   double
00072   centre_mag_norm_const () const
00073   { return centre_mag_norm_const_; }
00074 
00075   //: map a location
00076   inline
00077   void
00078   map_loc( vnl_vector_fixed<double, Tdim>& mapped,
00079            vnl_matrix_fixed<double, Tdim+1, Fdim+1> const& proj,
00080            vcl_vector<double> const& rad_k,
00081            vnl_vector_fixed<double, Fdim> const& from  ) const
00082   {
00083 
00084     vnl_vector_fixed<double, Tdim> proj_mapped;
00085     rgrl_est_proj_map_inhomo_point<Tdim, Fdim>( proj_mapped, proj, from-this->from_centre_ );
00086 
00087     // apply radial distortion
00088     apply_radial_distortion( mapped, proj_mapped, rad_k );
00089 
00090     mapped += this->to_centre_;
00091   }
00092 
00093 protected:
00094 
00095   //: compute jacobian
00096   void
00097   reduced_proj_rad_jacobian( vnl_matrix<double>                            & base_jac,
00098                              vnl_matrix_fixed<double, Tdim+1, Fdim+1> const& proj,
00099                              vcl_vector<double>                       const& rad_k,
00100                              vnl_vector_fixed<double, Fdim>           const& from ) const;
00101 
00102   //: compute the full jacobian
00103   void
00104   full_proj_rad_jacobian( vnl_matrix<double>                            & base_jac,
00105                           vnl_matrix_fixed<double, Tdim+1, Fdim+1> const& proj,
00106                           vcl_vector<double>                       const& rad_k,
00107                           vnl_vector_fixed<double, Fdim>           const& from ) const;
00108 
00109   //: compute jacobian w.r.t. location
00110   void
00111   proj_jac_wrt_loc( vnl_matrix_fixed<double, Tdim, Fdim>          & jac_loc,
00112                     vnl_matrix_fixed<double, Tdim+1, Fdim+1> const& proj,
00113                     vcl_vector<double>                       const& rad_k,
00114                     vnl_vector_fixed<double, Fdim>           const& from ) const;
00115 
00116   //: convert parameters
00117   void
00118   convert_parameters( vnl_vector<double>& params,
00119                       vnl_matrix_fixed<double, Tdim+1, Fdim+1> const& proj_matrix,
00120                       vcl_vector<double>             const& rad_dist,
00121                       vnl_vector_fixed<double, Fdim> const& fc,
00122                       vnl_vector_fixed<double, Tdim> const& tc,
00123                       vnl_vector_fixed<double, Tdim> const& camera_centre );
00124 
00125   void
00126   apply_radial_distortion( vnl_vector_fixed<double, Tdim>      & mapped,
00127                            vnl_vector_fixed<double, Tdim> const& p,
00128                            vcl_vector<double> const& rad_k ) const;
00129 
00130   //: transfer parameters into the temp vector
00131   void
00132   transfer_radial_params_into_temp_storage( vnl_vector<double> const& params ) const;
00133 
00134 protected:
00135   unsigned int                    camera_dof_;
00136   vnl_vector_fixed<double, Tdim>  image_centre_;
00137   double                          centre_mag_norm_const_;
00138   mutable vcl_vector<double>  temp_rad_k_;
00139 };
00140 
00141 #endif //rgrl_est_proj_rad_func_h_