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