1 // This is core/vgl/algo/vgl_h_matrix_2d_optimize.h 2 #ifndef vgl_h_matrix_2d_optimize_h_ 3 #define vgl_h_matrix_2d_optimize_h_ 4 //: 5 // \file 6 // \author J.L. Mundy Jan. 5, 2005 7 // \brief Refine an initial 2d homography by minimizing projection error 8 // 9 // Abstract interface for classes that optimize plane-to-plane 10 // projectivities from point and line correspondences. 11 // 12 // \verbatim 13 // Modifications 14 // \endverbatim 15 #include <vector> 16 #ifdef _MSC_VER 17 # include <vcl_msvc_warnings.h> 18 #endif 19 #include <cassert> 20 #include <vnl/vnl_least_squares_function.h> 21 #include <vgl/vgl_homg_point_2d.h> 22 #include <vgl/vgl_homg_line_2d.h> 23 #include <vgl/algo/vgl_h_matrix_2d.h> 24 25 // Note that the least squares function is over-parametrized 26 // with 9 parameters rather than the minimum 8. 27 // See Hartley and Zisserman p. 94. 28 class projection_lsqf : public vnl_least_squares_function 29 { 30 unsigned n_; 31 std::vector<vgl_homg_point_2d<double> > from_points_; 32 std::vector<vgl_point_2d<double> > to_points_; 33 34 public: projection_lsqf(std::vector<vgl_homg_point_2d<double>> const & from_points,std::vector<vgl_homg_point_2d<double>> const & to_points)35 projection_lsqf(std::vector<vgl_homg_point_2d<double> > const& from_points, 36 std::vector<vgl_homg_point_2d<double> > const& to_points) 37 : vnl_least_squares_function(9, 2*from_points.size() + 1, no_gradient) 38 { 39 n_ = from_points.size(); 40 assert(n_==to_points.size()); 41 for (unsigned i = 0; i<n_; ++i) 42 { 43 from_points_.push_back(from_points[i]); 44 to_points_.emplace_back(to_points[i]); 45 } 46 } 47 48 ~projection_lsqf() override = default; 49 50 //: compute the projection error given a set of h parameters. 51 // The residuals required by f are the Euclidean x and y coordinate 52 // differences between the projected from points and the 53 // corresponding to points. f(const vnl_vector<double> & hv,vnl_vector<double> & proj_err)54 void f(const vnl_vector<double>& hv, vnl_vector<double>& proj_err) override 55 { 56 assert(hv.size()==9); 57 assert(proj_err.size()==2*n_+1); 58 // project and compute residual 59 vgl_h_matrix_2d<double> h(hv.data_block()); 60 unsigned k = 0; 61 for (unsigned i = 0; i<n_; ++i, k+=2) 62 { 63 vgl_homg_point_2d<double> to_proj = h(from_points_[i]); 64 vgl_point_2d<double> p_proj(to_proj); 65 double xp = to_points_[i].x(), yp = to_points_[i].y(); 66 double xproj = p_proj.x(), yproj = p_proj.y(); 67 proj_err[k]=(xp-xproj); proj_err[k+1]=(yp-yproj); 68 } 69 proj_err[2*n_]=1.0-hv.magnitude(); 70 } 71 }; 72 73 74 class vgl_h_matrix_2d_optimize 75 { 76 public: vgl_h_matrix_2d_optimize(vgl_h_matrix_2d<double> const & initial_h)77 vgl_h_matrix_2d_optimize(vgl_h_matrix_2d<double> const& initial_h) 78 : verbose_(false), trace_(false), ftol_(1e-9), gtol_(1e-9), 79 htol_(1e-9), max_iter_(2000), initial_h_(initial_h){} 80 81 virtual ~vgl_h_matrix_2d_optimize() = default; 82 83 //: set this to true for verbose run-time information set_verbose(bool v)84 void set_verbose(bool v) { verbose_ = v; } 85 86 //: Termination tolerance on the gradient of the projection error set_trace(bool trace)87 void set_trace(bool trace){trace_ = trace;} 88 89 //: Termination tolerance on change in the solution set_htol(const double htol)90 void set_htol(const double htol){htol_ = htol;} 91 92 //: Termination tolerance on the sum of squared projection errors 93 // The optimization is done in a normalized coordinate frame with 94 // unity point domain radius. set_ftol(const double ftol)95 void set_ftol(const double ftol){ftol_ = ftol;} 96 97 //: Termination tolerance on the gradient of the projection error set_gtol(const double gtol)98 void set_gtol(const double gtol){gtol_ = gtol;} 99 100 virtual int minimum_number_of_correspondences() const = 0; 101 102 // Optimize methods : 103 // 104 // Some use point correspondences, some use line 105 // correspondences, some use both. They are implemented 106 // in terms of the pure virtual optimize_(p|l|pl) methods. 107 108 //: optimize homography from matched points optimize(std::vector<vgl_homg_point_2d<double>> const & points1,std::vector<vgl_homg_point_2d<double>> const & points2,vgl_h_matrix_2d<double> & H)109 bool optimize(std::vector<vgl_homg_point_2d<double> > const& points1, 110 std::vector<vgl_homg_point_2d<double> > const& points2, 111 vgl_h_matrix_2d<double>& H) 112 { 113 return optimize_p(points1, points2, H); 114 } 115 116 //: optimize homography from matched lines optimize(std::vector<vgl_homg_line_2d<double>> const & lines1,std::vector<vgl_homg_line_2d<double>> const & lines2,vgl_h_matrix_2d<double> & H)117 bool optimize(std::vector<vgl_homg_line_2d<double> > const& lines1, 118 std::vector<vgl_homg_line_2d<double> > const& lines2, 119 vgl_h_matrix_2d<double>& H) 120 { 121 return optimize_l(lines1, lines2, H); 122 } 123 124 //: optimize homography from matched points and lines optimize(std::vector<vgl_homg_point_2d<double>> const & points1,std::vector<vgl_homg_point_2d<double>> const & points2,std::vector<vgl_homg_line_2d<double>> const & lines1,std::vector<vgl_homg_line_2d<double>> const & lines2,vgl_h_matrix_2d<double> & H)125 bool optimize(std::vector<vgl_homg_point_2d<double> > const& points1, 126 std::vector<vgl_homg_point_2d<double> > const& points2, 127 std::vector<vgl_homg_line_2d<double> > const& lines1, 128 std::vector<vgl_homg_line_2d<double> > const& lines2, 129 vgl_h_matrix_2d<double>& H) 130 { 131 return optimize_pl(points1, points2, lines1, lines2, H); 132 } 133 134 //: optimize homography from matched points - return h_matrix 135 vgl_h_matrix_2d<double> optimize(std::vector<vgl_homg_point_2d<double>> const & p1,std::vector<vgl_homg_point_2d<double>> const & p2)136 optimize(std::vector<vgl_homg_point_2d<double> > const& p1, 137 std::vector<vgl_homg_point_2d<double> > const& p2) 138 { vgl_h_matrix_2d<double> H; optimize_p(p1, p2, H); return H; } 139 140 //: optimize homography from matched lines - return h_matrix 141 vgl_h_matrix_2d<double> optimize(std::vector<vgl_homg_line_2d<double>> const & l1,std::vector<vgl_homg_line_2d<double>> const & l2)142 optimize(std::vector<vgl_homg_line_2d<double> > const& l1, 143 std::vector<vgl_homg_line_2d<double> > const& l2) 144 { vgl_h_matrix_2d<double> H; optimize_l(l1, l2, H); return H; } 145 146 //: optimize homography from matched points and lines - return h_matrix 147 vgl_h_matrix_2d<double> optimize(std::vector<vgl_homg_point_2d<double>> const & p1,std::vector<vgl_homg_point_2d<double>> const & p2,std::vector<vgl_homg_line_2d<double>> const & l1,std::vector<vgl_homg_line_2d<double>> const & l2)148 optimize(std::vector<vgl_homg_point_2d<double> > const& p1, 149 std::vector<vgl_homg_point_2d<double> > const& p2, 150 std::vector<vgl_homg_line_2d<double> > const& l1, 151 std::vector<vgl_homg_line_2d<double> > const& l2) 152 { vgl_h_matrix_2d<double> H; optimize_pl(p1, p2, l1, l2, H); return H; } 153 154 protected: 155 bool verbose_; 156 bool trace_; 157 double ftol_; 158 double gtol_; 159 double htol_; 160 int max_iter_; 161 vgl_h_matrix_2d<double> initial_h_; 162 virtual bool optimize_p(std::vector<vgl_homg_point_2d<double> > const& points1, 163 std::vector<vgl_homg_point_2d<double> > const& points2, 164 vgl_h_matrix_2d<double>& H) = 0; 165 166 virtual bool optimize_l(std::vector<vgl_homg_line_2d<double> > const& lines1, 167 std::vector<vgl_homg_line_2d<double> > const& lines2, 168 vgl_h_matrix_2d<double>& H) = 0; 169 170 virtual bool optimize_pl(std::vector<vgl_homg_point_2d<double> >const& points1, 171 std::vector<vgl_homg_point_2d<double> >const& points2, 172 std::vector<vgl_homg_line_2d<double> > const& lines1, 173 std::vector<vgl_homg_line_2d<double> > const& lines2, 174 vgl_h_matrix_2d<double>& H) = 0; 175 }; 176 177 #endif // vgl_h_matrix_2d_optimize_h_ 178