// Copyright (C) 2003 Davis E. King (davis@dlib.net) // License: Boost Software License See LICENSE.txt for the full license. #ifndef DLIB_POINT_TrANSFORMS_H_ #define DLIB_POINT_TrANSFORMS_H_ #include "point_transforms_abstract.h" #include "../algs.h" #include "vector.h" #include "../matrix.h" #include "../matrix/matrix_la.h" #include "../optimization/optimization.h" #include <vector> namespace dlib { // ---------------------------------------------------------------------------------------- class point_rotator { public: point_rotator ( const double& angle ) { sin_angle = std::sin(angle); cos_angle = std::cos(angle); } template <typename T> const dlib::vector<T,2> operator() ( const dlib::vector<T,2>& p ) const { double x = cos_angle*p.x() - sin_angle*p.y(); double y = sin_angle*p.x() + cos_angle*p.y(); return dlib::vector<double,2>(x,y); } const matrix<double,2,2> get_m( ) const { matrix<double,2,2> temp; temp = cos_angle, -sin_angle, sin_angle, cos_angle; return temp; } private: double sin_angle; double cos_angle; }; // ---------------------------------------------------------------------------------------- class point_transform { public: point_transform ( const double& angle, const dlib::vector<double,2>& translate_ ) { sin_angle = std::sin(angle); cos_angle = std::cos(angle); translate = translate_; } template <typename T> const dlib::vector<T,2> operator() ( const dlib::vector<T,2>& p ) const { double x = cos_angle*p.x() - sin_angle*p.y(); double y = sin_angle*p.x() + cos_angle*p.y(); return dlib::vector<double,2>(x,y) + translate; } const matrix<double,2,2> get_m( ) const { matrix<double,2,2> temp; temp = cos_angle, -sin_angle, sin_angle, cos_angle; return temp; } const dlib::vector<double,2> get_b( ) const { return translate; } private: double sin_angle; double cos_angle; dlib::vector<double,2> translate; }; // ---------------------------------------------------------------------------------------- class point_transform_affine { public: point_transform_affine ( const matrix<double,2,2>& m_, const dlib::vector<double,2>& b_ ) :m(m_), b(b_) { } const dlib::vector<double,2> operator() ( const dlib::vector<double,2>& p ) const { return m*p + b; } const matrix<double,2,2>& get_m( ) const { return m; } const dlib::vector<double,2>& get_b( ) const { return b; } private: matrix<double,2,2> m; dlib::vector<double,2> b; }; // ---------------------------------------------------------------------------------------- template <typename T> point_transform_affine find_affine_transform ( const std::vector<dlib::vector<T,2> >& from_points, const std::vector<dlib::vector<T,2> >& to_points ) { // make sure requires clause is not broken DLIB_ASSERT(from_points.size() == to_points.size() && from_points.size() >= 3, "\t point_transform_affine find_affine_transform(from_points, to_points)" << "\n\t Invalid inputs were given to this function." << "\n\t from_points.size(): " << from_points.size() << "\n\t to_points.size(): " << to_points.size() ); matrix<double,3,0> P(3, from_points.size()); matrix<double,2,0> Q(2, from_points.size()); for (unsigned long i = 0; i < from_points.size(); ++i) { P(0,i) = from_points[i].x(); P(1,i) = from_points[i].y(); P(2,i) = 1; Q(0,i) = to_points[i].x(); Q(1,i) = to_points[i].y(); } const matrix<double,2,3> m = Q*pinv(P); return point_transform_affine(subm(m,0,0,2,2), colm(m,2)); } // ---------------------------------------------------------------------------------------- class point_transform_projective { public: point_transform_projective ( const matrix<double,3,3>& m_ ) :m(m_) { } point_transform_projective ( const point_transform_affine& tran ) { set_subm(m, 0,0, 2,2) = tran.get_m(); set_subm(m, 0,2, 2,1) = tran.get_b(); m(2,0) = 0; m(2,1) = 0; m(2,2) = 1; } const dlib::vector<double,2> operator() ( const dlib::vector<double,2>& p ) const { dlib::vector<double,3> temp(p); temp.z() = 1; temp = m*temp; if (temp.z() != 0) temp = temp/temp.z(); return temp; } const matrix<double,3,3>& get_m( ) const { return m; } private: matrix<double,3,3> m; }; // ---------------------------------------------------------------------------------------- namespace impl_proj { inline point_transform_projective find_projective_transform_basic ( const std::vector<dlib::vector<double,2> >& from_points, const std::vector<dlib::vector<double,2> >& to_points ) /*! ensures - Uses the system of equations approach to finding a projective transform. This is "Method 3" from Estimating Projective Transformation Matrix by Zhengyou Zhang. - It should be emphasized that the find_projective_transform_basic() routine, which uses the most popular method for finding projective transformations, doesn't really work well when the minimum error solution doesn't have zero error. In this case, it can deviate by a large amount from the proper minimum mean squared error transformation. Therefore, our overall strategy will be to use the solution from find_projective_transform_basic() as a starting point for a BFGS based non-linear optimizer which will optimize the correct mean squared error criterion. !*/ { // make sure requires clause is not broken DLIB_ASSERT(from_points.size() == to_points.size() && from_points.size() >= 4, "\t point_transform_projective find_projective_transform_basic(from_points, to_points)" << "\n\t Invalid inputs were given to this function." << "\n\t from_points.size(): " << from_points.size() << "\n\t to_points.size(): " << to_points.size() ); matrix<double,9,9> accum, u, v; matrix<double,9,1> w; matrix<double,2,9> B; accum = 0; B = 0; for (unsigned long i = 0; i < from_points.size(); ++i) { dlib::vector<double,3> f = from_points[i]; f.z() = 1; dlib::vector<double,3> t = to_points[i]; t.z() = 1; set_subm(B,0,0,1,3) = t.y()*trans(f); set_subm(B,1,0,1,3) = trans(f); set_subm(B,0,3,1,3) = -t.x()*trans(f); set_subm(B,1,6,1,3) = -t.x()*trans(f); accum += trans(B)*B; } svd2(true, false, accum, u, w, v); long j = index_of_min(w); return point_transform_projective(reshape(colm(u,j),3,3)); } // ---------------------------------------------------------------------------------------- struct obj { /*! WHAT THIS OBJECT REPRESENTS This is the objective function we really want to minimize when looking for a transformation matrix. That is, we would like the transformed points to be as close as possible to their "to" points. Here, closeness is measured using Euclidean distance. !*/ obj( const std::vector<dlib::vector<double,2> >& from_points_, const std::vector<dlib::vector<double,2> >& to_points_ ) : from_points(from_points_) , to_points(to_points_) {} const std::vector<dlib::vector<double,2> >& from_points; const std::vector<dlib::vector<double,2> >& to_points; double operator() ( const matrix<double,9,1>& p ) const { point_transform_projective tran(reshape(p,3,3)); double sum = 0; for (unsigned long i = 0; i < from_points.size(); ++i) { sum += length_squared(tran(from_points[i]) - to_points[i]); } return sum; } }; struct obj_der { /*! WHAT THIS OBJECT REPRESENTS This is the derivative of obj. !*/ obj_der( const std::vector<dlib::vector<double,2> >& from_points_, const std::vector<dlib::vector<double,2> >& to_points_ ) : from_points(from_points_) , to_points(to_points_) {} const std::vector<dlib::vector<double,2> >& from_points; const std::vector<dlib::vector<double,2> >& to_points; matrix<double,9,1> operator() ( const matrix<double,9,1>& p ) const { const matrix<double,3,3> H = reshape(p,3,3); matrix<double,3,3> grad; grad = 0; for (unsigned long i = 0; i < from_points.size(); ++i) { dlib::vector<double,3> from, to; from = from_points[i]; from.z() = 1; to = to_points[i]; to.z() = 1; matrix<double,3,1> w = H*from; const double scale = (w(2) != 0) ? (1.0/w(2)) : (1); w *= scale; matrix<double,3,1> residual = (w-to)*2*scale; grad(0,0) += from.x()*residual(0); grad(0,1) += from.y()*residual(0); grad(0,2) += residual(0); grad(1,0) += from.x()*residual(1); grad(1,1) += from.y()*residual(1); grad(1,2) += residual(1); grad(2,0) += -(from.x()*w(0)*residual(0) + from.x()*w(1)*residual(1)); grad(2,1) += -(from.y()*w(0)*residual(0) + from.y()*w(1)*residual(1)); grad(2,2) += -( w(0)*residual(0) + w(1)*residual(1)); } return reshape_to_column_vector(grad); } }; } // ---------------------------------------------------------------------------------------- inline point_transform_projective find_projective_transform ( const std::vector<dlib::vector<double,2> >& from_points, const std::vector<dlib::vector<double,2> >& to_points ) { using namespace impl_proj; // make sure requires clause is not broken DLIB_ASSERT(from_points.size() == to_points.size() && from_points.size() >= 4, "\t point_transform_projective find_projective_transform(from_points, to_points)" << "\n\t Invalid inputs were given to this function." << "\n\t from_points.size(): " << from_points.size() << "\n\t to_points.size(): " << to_points.size() ); // Find a candidate projective transformation. Also, find the best affine // transform and then compare it with the projective transform estimated using the // direct SVD method. Use whichever one works better as the starting point for a // BFGS based optimizer. If the best solution has large mean squared error and is // also close to affine then find_projective_transform_basic() might give a very // bad initial guess. So also checking for a good affine transformation can // produce a much better final result in many cases. point_transform_projective tran1 = find_projective_transform_basic(from_points, to_points); point_transform_affine tran2 = find_affine_transform(from_points, to_points); // check which is best double error1 = 0; double error2 = 0; for (unsigned long i = 0; i < from_points.size(); ++i) { error1 += length_squared(tran1(from_points[i])-to_points[i]); error2 += length_squared(tran2(from_points[i])-to_points[i]); } matrix<double,9,1> params; // Pick the minimum error solution among the two so far. if (error1 < error2) params = reshape_to_column_vector(tran1.get_m()); else params = reshape_to_column_vector(point_transform_projective(tran2).get_m()); // Now refine the transformation matrix so that we can be sure we have // at least a local minimizer. obj o(from_points, to_points); obj_der der(from_points, to_points); find_min(bfgs_search_strategy(), objective_delta_stop_strategy(1e-6,100), o, der, params, 0); return point_transform_projective(reshape(params,3,3)); } // ---------------------------------------------------------------------------------------- template <typename T> const dlib::vector<T,2> rotate_point ( const dlib::vector<T,2>& center, const dlib::vector<T,2>& p, double angle ) { point_rotator rot(angle); return rot(p-center)+center; } // ---------------------------------------------------------------------------------------- inline matrix<double,2,2> rotation_matrix ( double angle ) { const double ca = std::cos(angle); const double sa = std::sin(angle); matrix<double,2,2> m; m = ca, -sa, sa, ca; return m; } // ---------------------------------------------------------------------------------------- } #endif // DLIB_POINT_TrANSFORMS_H_