1 //:
2 // \file
3 #include "rgrl_est_homo2d_proj_rad.h"
4
5 #include <rgrl/rgrl_est_homography2d.h>
6 #include <rgrl/rgrl_est_proj_rad_func.h>
7 #include <rgrl/rgrl_trans_homography2d.h>
8 #include <rgrl/rgrl_trans_homo2d_proj_rad.h>
9 #include <rgrl/rgrl_trans_rad_dis_homo2d.h>
10 #include <rgrl/rgrl_match_set.h>
11 #include <rgrl/rgrl_internal_util.h>
12
13 #include "vnl/vnl_double_2.h"
14 #include "vul/vul_sprintf.h"
15
16 // --------------------------------------------------------------------
17
18 rgrl_est_homo2d_proj_rad::
rgrl_est_homo2d_proj_rad(unsigned int rad_order,vnl_vector_fixed<double,2> const & to_camera_centre,bool with_grad)19 rgrl_est_homo2d_proj_rad( unsigned int rad_order,
20 vnl_vector_fixed<double, 2> const& to_camera_centre,
21 bool with_grad )
22 : to_camera_centre_( to_camera_centre ),
23 camera_dof_( rad_order ),
24 with_grad_( with_grad )
25 {
26 rgrl_estimator::set_param_dof( 8+camera_dof_ );
27
28 // default value
29 rgrl_nonlinear_estimator::set_max_num_iter( 50 );
30 rgrl_nonlinear_estimator::set_rel_thres( 1e-5 );
31
32 // setting up transform name
33 transform_name_ = vul_sprintf( "rgrl_trans_homo2d_proj_rad+radial-%d", camera_dof_ );
34 }
35
36 rgrl_transformation_sptr
37 rgrl_est_homo2d_proj_rad::
estimate(rgrl_set_of<rgrl_match_set_sptr> const & matches,rgrl_transformation const & cur_transform) const38 estimate( rgrl_set_of<rgrl_match_set_sptr> const& matches,
39 rgrl_transformation const& cur_transform ) const
40 {
41 // get initialization
42 vnl_matrix_fixed<double, 3, 3> init_H;
43 std::vector<double> radk( camera_dof_, 0.0 );
44
45 if ( cur_transform.is_type( rgrl_trans_homo2d_proj_rad::type_id() ) )
46 {
47 auto const& trans = static_cast<rgrl_trans_homo2d_proj_rad const&>( cur_transform );
48 init_H = trans.H();
49 const std::vector<double>& k = trans.normalized_radial_params();
50 for ( unsigned int i=0; i<k.size()&&i<camera_dof_; ++i )
51 radk[i] = k[i];
52 }
53 else if ( cur_transform.is_type( rgrl_trans_rad_dis_homo2d::type_id() ) )
54 {
55 auto const& trans = static_cast<rgrl_trans_rad_dis_homo2d const&>( cur_transform );
56 init_H = trans.uncenter_H_matrix();
57 const double k1_to = trans.k1_to();
58 radk[0] = k1_to;
59 }
60 else
61 {
62 if ( !rgrl_internal_util_upgrade_to_homography2D( init_H, cur_transform ) )
63 {
64 // use normalized DLT to initialize
65 DebugMacro( 0, "Use normalized DLT to initialize" );
66 rgrl_est_homography2d est_homo;
67 rgrl_transformation_sptr tmp_trans= est_homo.estimate( matches, cur_transform );
68 if ( !tmp_trans )
69 return nullptr;
70 auto const& trans = static_cast<rgrl_trans_homography2d const&>( *tmp_trans );
71 init_H = trans.uncenter_H_matrix();
72 }
73 }
74
75 // construct least square cost function
76 rgrl_est_proj_rad_func<2,2> homo_func( matches, camera_dof_, with_grad_ );
77 homo_func.set_max_num_iter( max_num_iterations_ );
78 homo_func.set_rel_thres( relative_threshold_ );
79
80 // apply estimation
81 vnl_double_2 from_centre, to_centre;
82 vnl_matrix<double> covar;
83 if ( !homo_func.projective_estimate( init_H, radk,
84 covar,
85 from_centre, to_centre,
86 to_camera_centre_ ) ) {
87 WarningMacro( "L-M estimation failed." << std::endl );
88 return nullptr;
89 }
90
91 return new rgrl_trans_homo2d_proj_rad( radk,
92 init_H.as_ref(),
93 to_camera_centre_.as_ref(),
94 covar,
95 from_centre.as_ref(), to_centre.as_ref() );
96 }
97
98
99 const std::type_info&
100 rgrl_est_homo2d_proj_rad::
transformation_type() const101 transformation_type() const
102 {
103 return rgrl_trans_homo2d_proj_rad::type_id();
104 }
105