Added find_similarity_transform()

dlib/geometry/point_transforms.h

@@ -234,6 +234,71 @@ namespace dlib
         return point_transform_affine(subm(m,0,0,2,2), colm(m,2));
     }
 
+// ----------------------------------------------------------------------------------------
+
+    template <typename T>
+    point_transform_affine find_similarity_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() >= 2,
+            "\t point_transform_affine find_similarity_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()
+            );
+
+        // We use the formulas from the paper: Least-squares estimation of transformation
+        // parameters between two point patterns by Umeyama.  They are equations 34 through
+        // 42.
+
+        dlib::vector<double,2> mean_from, mean_to;
+        double sigma_from = 0, sigma_to = 0;
+        matrix<double,2,2> cov;
+        cov = 0;
+
+        for (unsigned long i = 0; i < from_points.size(); ++i)
+        {
+            mean_from += from_points[i];
+            mean_to += to_points[i];
+        }
+        mean_from /= from_points.size();
+        mean_to   /= from_points.size();
+
+        for (unsigned long i = 0; i < from_points.size(); ++i)
+        {
+            sigma_from += length_squared(from_points[i] - mean_from);
+            sigma_to += length_squared(to_points[i] - mean_to);
+            cov += (to_points[i] - mean_to)*trans(from_points[i] - mean_from);
+        }
+
+        sigma_from /= from_points.size();
+        sigma_to   /= from_points.size();
+        cov        /= from_points.size();
+
+        matrix<double,2,2> u, v, s, d;
+        svd(cov, u,d,v);
+        s = identity_matrix(cov);
+        if (det(cov) < 0)
+        {
+            if (d(1,1) < d(0,0))
+                s(1,1) = -1;
+            else
+                s(0,0) = -1;
+        }
+
+        matrix<double,2,2> r = u*s*trans(v);
+        double c = 1; 
+        if (sigma_from != 0)
+            c = 1.0/sigma_from * trace(d*s);
+        vector<double,2> t = mean_to - c*r*mean_from;
+
+        return point_transform_affine(c*r, t);
+    }
+
 // ----------------------------------------------------------------------------------------
 
     class point_transform_projective

dlib/geometry/point_transforms_abstract.h

@@ -107,6 +107,29 @@ namespace dlib
               all possible solutions).
     !*/
 
+// ----------------------------------------------------------------------------------------
+
+    template <typename T>
+    point_transform_affine find_similarity_transform (
+        const std::vector<dlib::vector<T,2> >& from_points,
+        const std::vector<dlib::vector<T,2> >& to_points
+    );
+    /*!
+        requires
+            - from_points.size() == to_points.size()
+            - from_points.size() >= 2
+        ensures
+            - This function is just like find_affine_transform() except it finds the best
+              similarity transform instead of a full affine transform.  This means that it
+              optimizes over only the space of rotations, scale changes, and translations.
+              So for example, if you mapped the 3 vertices of a triangle through a
+              similarity transform then the output would still be the same triangle.
+              However, the triangle itself may be larger or smaller, rotated, or at a
+              different location in the coordinate system.  This is not the case for a
+              general affine transform which can stretch points in ways that cause, for
+              example, an equilateral triangle to turn into an isosceles triangle.
+    !*/
+
 // ----------------------------------------------------------------------------------------
 
     class point_transform_projective

dlib/test/geometry.cpp

@@ -728,6 +728,50 @@ namespace
         return pass_rate.mean();
     }
 
+// ----------------------------------------------------------------------------------------
+
+    template <typename T>
+    void test_find_similarity_transform()
+    {
+        print_spinner();
+        std::vector<dlib::vector<T,2> > from_points, to_points;
+
+        from_points.push_back(dlib::vector<T,2>(0,0));
+        from_points.push_back(dlib::vector<T,2>(0,1));
+        from_points.push_back(dlib::vector<T,2>(1,0));
+
+        to_points.push_back(dlib::vector<T,2>(8,0));
+        to_points.push_back(dlib::vector<T,2>(6,0));
+        to_points.push_back(dlib::vector<T,2>(8,2));
+
+        point_transform_affine tform = find_similarity_transform(from_points, to_points);
+
+        for (unsigned long i = 0; i < from_points.size(); ++i)
+        {
+            DLIB_TEST(length(tform(from_points[i]) - to_points[i]) < 1e-14);
+        }
+    }
+
+    template <typename T>
+    void test_find_similarity_transform2()
+    {
+        print_spinner();
+        std::vector<dlib::vector<T,2> > from_points, to_points;
+
+        from_points.push_back(dlib::vector<T,2>(0,0));
+        from_points.push_back(dlib::vector<T,2>(0,1));
+
+        to_points.push_back(dlib::vector<T,2>(8,0));
+        to_points.push_back(dlib::vector<T,2>(6,0));
+
+        point_transform_affine tform = find_similarity_transform(from_points, to_points);
+
+        for (unsigned long i = 0; i < from_points.size(); ++i)
+        {
+            DLIB_TEST(length(tform(from_points[i]) - to_points[i]) < 1e-14);
+        }
+    }
+
 // ----------------------------------------------------------------------------------------
 
     class geometry_tester : public tester
@@ -747,6 +791,11 @@ namespace
             test_find_affine_transform();
             DLIB_TEST(projective_transform_pass_rate(0.1) > 0.99);
             DLIB_TEST(projective_transform_pass_rate(0.0) == 1);
+
+            test_find_similarity_transform<double>(); 
+            test_find_similarity_transform2<double>(); 
+            test_find_similarity_transform<float>(); 
+            test_find_similarity_transform2<float>(); 
         }
     } a;