00001
00002 #ifndef vgl_h_matrix_2d_optimize_h_
00003 #define vgl_h_matrix_2d_optimize_h_
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include <vcl_vector.h>
00016 #include <vcl_cassert.h>
00017 #include <vnl/vnl_least_squares_function.h>
00018 #include <vgl/vgl_homg_point_2d.h>
00019 #include <vgl/vgl_homg_line_2d.h>
00020 #include <vgl/algo/vgl_h_matrix_2d.h>
00021
00022
00023
00024
00025 class projection_lsqf : public vnl_least_squares_function
00026 {
00027 unsigned n_;
00028 vcl_vector<vgl_homg_point_2d<double> > from_points_;
00029 vcl_vector<vgl_point_2d<double> > to_points_;
00030
00031 public:
00032 projection_lsqf(vcl_vector<vgl_homg_point_2d<double> > const& from_points,
00033 vcl_vector<vgl_homg_point_2d<double> > const& to_points)
00034 : vnl_least_squares_function(9, 2*from_points.size() + 1, no_gradient)
00035 {
00036 n_ = from_points.size();
00037 assert(n_==to_points.size());
00038 for (unsigned i = 0; i<n_; ++i)
00039 {
00040 from_points_.push_back(from_points[i]);
00041 to_points_.push_back(to_points[i]);
00042 }
00043 }
00044
00045 ~projection_lsqf() {}
00046
00047
00048
00049
00050
00051 void f(const vnl_vector<double>& hv, vnl_vector<double>& proj_err)
00052 {
00053 assert(hv.size()==9);
00054 assert(proj_err.size()==2*n_+1);
00055
00056 vgl_h_matrix_2d<double> h(hv.data_block());
00057 unsigned k = 0;
00058 for (unsigned i = 0; i<n_; ++i, k+=2)
00059 {
00060 vgl_homg_point_2d<double> to_proj = h(from_points_[i]);
00061 vgl_point_2d<double> p_proj(to_proj);
00062 double xp = to_points_[i].x(), yp = to_points_[i].y();
00063 double xproj = p_proj.x(), yproj = p_proj.y();
00064 proj_err[k]=(xp-xproj); proj_err[k+1]=(yp-yproj);
00065 }
00066 proj_err[2*n_]=1.0-hv.magnitude();
00067 }
00068 };
00069
00070
00071 class vgl_h_matrix_2d_optimize
00072 {
00073 public:
00074 vgl_h_matrix_2d_optimize(vgl_h_matrix_2d<double> const& initial_h)
00075 : verbose_(false), trace_(false), ftol_(1e-9), gtol_(1e-9),
00076 htol_(1e-9), max_iter_(2000), initial_h_(initial_h){}
00077
00078 virtual ~vgl_h_matrix_2d_optimize() {}
00079
00080
00081 void set_verbose(bool v) { verbose_ = v; }
00082
00083
00084 void set_trace(bool trace){trace_ = trace;}
00085
00086
00087 void set_htol(const double htol){htol_ = htol;}
00088
00089
00090
00091
00092 void set_ftol(const double ftol){ftol_ = ftol;}
00093
00094
00095 void set_gtol(const double gtol){gtol_ = gtol;}
00096
00097 virtual int minimum_number_of_correspondences() const = 0;
00098
00099
00100
00101
00102
00103
00104
00105
00106 bool optimize(vcl_vector<vgl_homg_point_2d<double> > const& points1,
00107 vcl_vector<vgl_homg_point_2d<double> > const& points2,
00108 vgl_h_matrix_2d<double>& H)
00109 {
00110 return optimize_p(points1, points2, H);
00111 }
00112
00113
00114 bool optimize(vcl_vector<vgl_homg_line_2d<double> > const& lines1,
00115 vcl_vector<vgl_homg_line_2d<double> > const& lines2,
00116 vgl_h_matrix_2d<double>& H)
00117 {
00118 return optimize_l(lines1, lines2, H);
00119 }
00120
00121
00122 bool optimize(vcl_vector<vgl_homg_point_2d<double> > const& points1,
00123 vcl_vector<vgl_homg_point_2d<double> > const& points2,
00124 vcl_vector<vgl_homg_line_2d<double> > const& lines1,
00125 vcl_vector<vgl_homg_line_2d<double> > const& lines2,
00126 vgl_h_matrix_2d<double>& H)
00127 {
00128 return optimize_pl(points1, points2, lines1, lines2, H);
00129 }
00130
00131
00132 vgl_h_matrix_2d<double>
00133 optimize(vcl_vector<vgl_homg_point_2d<double> > const& p1,
00134 vcl_vector<vgl_homg_point_2d<double> > const& p2)
00135 { vgl_h_matrix_2d<double> H; optimize_p(p1, p2, H); return H; }
00136
00137
00138 vgl_h_matrix_2d<double>
00139 optimize(vcl_vector<vgl_homg_line_2d<double> > const& l1,
00140 vcl_vector<vgl_homg_line_2d<double> > const& l2)
00141 { vgl_h_matrix_2d<double> H; optimize_l(l1, l2, H); return H; }
00142
00143
00144 vgl_h_matrix_2d<double>
00145 optimize(vcl_vector<vgl_homg_point_2d<double> > const& p1,
00146 vcl_vector<vgl_homg_point_2d<double> > const& p2,
00147 vcl_vector<vgl_homg_line_2d<double> > const& l1,
00148 vcl_vector<vgl_homg_line_2d<double> > const& l2)
00149 { vgl_h_matrix_2d<double> H; optimize_pl(p1, p2, l1, l2, H); return H; }
00150
00151 protected:
00152 bool verbose_;
00153 bool trace_;
00154 double ftol_;
00155 double gtol_;
00156 double htol_;
00157 int max_iter_;
00158 vgl_h_matrix_2d<double> initial_h_;
00159 virtual bool optimize_p(vcl_vector<vgl_homg_point_2d<double> > const& points1,
00160 vcl_vector<vgl_homg_point_2d<double> > const& points2,
00161 vgl_h_matrix_2d<double>& H) = 0;
00162
00163 virtual bool optimize_l(vcl_vector<vgl_homg_line_2d<double> > const& lines1,
00164 vcl_vector<vgl_homg_line_2d<double> > const& lines2,
00165 vgl_h_matrix_2d<double>& H) = 0;
00166
00167 virtual bool optimize_pl(vcl_vector<vgl_homg_point_2d<double> >const& points1,
00168 vcl_vector<vgl_homg_point_2d<double> >const& points2,
00169 vcl_vector<vgl_homg_line_2d<double> > const& lines1,
00170 vcl_vector<vgl_homg_line_2d<double> > const& lines2,
00171 vgl_h_matrix_2d<double>& H) = 0;
00172 };
00173
00174 #endif // vgl_h_matrix_2d_optimize_h_