1 #ifndef rgrl_est_proj_func_h_
2 #define rgrl_est_proj_func_h_
3
4 //:
5 // \file
6 // \author Gehua Yang
7 // \date March 2007
8 // \brief a generic class to estimate a homogeneous projection matrix using L-M
9
10 #include <vnl/vnl_least_squares_function.h>
11 #include <vnl/vnl_vector_fixed.h>
12 #include <rgrl/rgrl_fwd.h>
13 #include <rgrl/rgrl_match_set_sptr.h>
14
15 //: mapping of homogeneous points
16 template <unsigned int Tdim, unsigned int Fdim>
17 inline
18 void
rgrl_est_proj_map_homo_point(vnl_vector_fixed<double,Tdim+1> & mapped,vnl_matrix_fixed<double,Tdim+1,Fdim+1> const & proj,vnl_vector_fixed<double,Fdim> const & loc)19 rgrl_est_proj_map_homo_point( vnl_vector_fixed<double, Tdim+1>& mapped,
20 vnl_matrix_fixed<double, Tdim+1, Fdim+1> const& proj,
21 vnl_vector_fixed<double, Fdim> const& loc )
22 {
23 for ( unsigned i=0; i<Tdim+1; ++i ) {
24 // shift term
25 mapped[i] = proj(i, Fdim);
26
27 for ( unsigned j=0; j<Fdim; ++j )
28 mapped[i] += loc[j] * proj(i,j);
29 }
30 }
31
32 //: mapping of inhomogeneous points
33 template <unsigned int Tdim, unsigned int Fdim>
34 inline
35 void
rgrl_est_proj_map_inhomo_point(vnl_vector_fixed<double,Tdim> & mapped,vnl_matrix_fixed<double,Tdim+1,Fdim+1> const & proj,vnl_vector_fixed<double,Fdim> const & loc)36 rgrl_est_proj_map_inhomo_point( vnl_vector_fixed<double, Tdim>& mapped,
37 vnl_matrix_fixed<double, Tdim+1, Fdim+1> const& proj,
38 vnl_vector_fixed<double, Fdim> const& loc )
39 {
40 vnl_vector_fixed<double, Tdim+1> tmp;
41
42 // map homo point
43 for ( unsigned i=0; i<Tdim+1; ++i ) {
44 // shift term
45 tmp[i] = proj(i, Fdim);
46
47 for ( unsigned j=0; j<Fdim; ++j )
48 tmp[i] += loc[j] * proj(i,j);
49 }
50
51 // get inhomo point
52 for ( unsigned i=0; i<Tdim; ++i )
53 mapped[i] = tmp[i]/tmp[Tdim];
54 }
55
56 //:a generic class to estimate a homogeneous projection matrix using L-M
57 template <unsigned int Tdim, unsigned int Fdim>
58 class rgrl_est_proj_func
59 : public vnl_least_squares_function
60 {
61 public:
62 //: ctor
63 rgrl_est_proj_func( rgrl_set_of<rgrl_match_set_sptr> const& matches,
64 bool with_grad = true );
65
66 //: ctor without matches
67 rgrl_est_proj_func( bool with_grad = true );
68
set_centres(vnl_vector_fixed<double,Fdim> const & fc,vnl_vector_fixed<double,Tdim> const & tc)69 void set_centres( vnl_vector_fixed<double, Fdim> const& fc,
70 vnl_vector_fixed<double, Tdim> const& tc )
71 {
72 from_centre_ = fc;
73 to_centre_ = tc;
74 }
75
76 //: set max number of iterations
set_max_num_iter(int max)77 void set_max_num_iter( int max )
78 { max_num_iterations_ = max; }
79
80 //: return max number of iterations
max_num_iter()81 int max_num_iter() const
82 { return max_num_iterations_; }
83
84 //: set relative threshold for parameters change
set_rel_thres(double thres)85 void set_rel_thres( double thres )
86 { relative_threshold_ = thres; }
87
88 //: relative threshold
rel_thres()89 double rel_thres() const
90 { return relative_threshold_; }
91
92 //: set zero singular value threshold for SVD
set_zero_svd_thres(double thres)93 void set_zero_svd_thres( double thres )
94 { zero_svd_thres_ = thres; }
95
96 //: zero singular value threshold
zero_svd_thres()97 double zero_svd_thres() const
98 { return zero_svd_thres_; }
99
100 //: estimate the projective transformation and the associated covariance
101 bool
102 projective_estimate( vnl_matrix_fixed<double, Tdim+1, Fdim+1>& proj,
103 vnl_matrix<double>& full_covar,
104 vnl_vector_fixed<double, Fdim>& from_centre,
105 vnl_vector_fixed<double, Tdim>& to_centre );
106
107 //: obj func value
108 void f(vnl_vector<double> const& x, vnl_vector<double>& fx) override;
109
110 //: Jacobian
111 void gradf(vnl_vector<double> const& x, vnl_matrix<double>& jacobian) override;
112
113 //: uncentre projection matrix
114 vnl_matrix_fixed<double, Tdim+1, Fdim+1>
115 uncentre_proj( vnl_matrix_fixed<double, Tdim+1, Fdim+1> const& proj ) const;
116
117 //: map a location
118 inline
119 void
map_loc(vnl_vector_fixed<double,Tdim> & mapped,vnl_matrix_fixed<double,Tdim+1,Fdim+1> const & proj,vnl_vector_fixed<double,Fdim> const & from)120 map_loc( vnl_vector_fixed<double, Tdim>& mapped,
121 vnl_matrix_fixed<double, Tdim+1, Fdim+1> const& proj,
122 vnl_vector_fixed<double, Fdim> const& from ) const
123 {
124 rgrl_est_proj_map_inhomo_point<Tdim, Fdim>( mapped, proj, from-from_centre_ );
125 mapped += to_centre_;
126 }
127
128 protected:
129 //: compute jacobian
130 void
131 reduced_proj_jacobian( vnl_matrix_fixed<double, Tdim, (Fdim+1)*(Tdim+1)-1>& base_jac,
132 vnl_matrix_fixed<double, Tdim+1, Fdim+1> const& proj,
133 vnl_vector_fixed<double, Fdim> const& from ) const;
134
135 //: compute jacobian
136 void
137 full_proj_jacobian( vnl_matrix_fixed<double, Tdim, (Fdim+1)*(Tdim+1)>& base_jac,
138 vnl_matrix_fixed<double, Tdim+1, Fdim+1> const& proj,
139 vnl_vector_fixed<double, Fdim> const& from ) const;
140
141 //: compute jacobian w.r.t. location
142 void
143 proj_jac_wrt_loc( vnl_matrix_fixed<double, Tdim, Fdim> & jac_loc,
144 vnl_matrix_fixed<double, Tdim+1, Fdim+1> const& proj,
145 vnl_vector_fixed<double, Fdim> const& from ) const;
146
147 //: convert parameters
148 void convert_parameters( vnl_vector<double>& params,
149 vnl_matrix_fixed<double, Tdim+1, Fdim+1> proj_matrix,
150 vnl_vector_fixed<double, Fdim> const& fc,
151 vnl_vector_fixed<double, Tdim> const& tc );
152 //: convert parameters
153 void restored_centered_proj( vnl_matrix_fixed<double, Tdim+1, Fdim+1>& proj_matrix,
154 vnl_vector<double> const& params ) const;
155
156 protected:
157 static const unsigned int proj_size_ = (Tdim+1)*(Fdim+1);
158
159 rgrl_set_of<rgrl_match_set_sptr> const *matches_ptr_{nullptr};
160 vnl_vector_fixed<double, Fdim> from_centre_;
161 vnl_vector_fixed<double, Tdim> to_centre_;
162
163 unsigned int index_row_;
164 unsigned int index_col_;
165 //: specify the maximum number of iterations for this estimator
166 int max_num_iterations_{50};
167
168 //: The threshold for relative parameter change before termination
169 double relative_threshold_{1e-7};
170
171 //: zero singular value threshold
172 double zero_svd_thres_{1e-5};
173 };
174
175 #if 0
176 //: a generic class to estimate projection matrices using L-M
177 // Includes 2D homography, 3D homography, camera matrix
178
179 template <unsigned int Tdim, unsigned int Fdim>
180 class rgrl_est_proj_lm
181 : public rgrl_nonlinear_estimator
182 {
183 public:
184 //: Default constructor
185 rgrl_est_proj_lm( bool with_grad = true );
186
187
188 // Defines type-related functions
189 rgrl_type_macro( rgrl_est_proj_lm, rgrl_nonlinear_estimator );
190
191 protected:
192
193 bool with_grad_;
194 //vnl_vector_fixed<double, Fdim> from_camera_centre_;
195 //vnl_vector_fixed<double, Tdim> to_camera_centre_;
196 };
197 #endif // 0
198
199 #endif //rgrl_est_proj_func_h_
200