From a8ba2d7f520219834115d627449d9866cf035699 Mon Sep 17 00:00:00 2001
From: Ivan Korolev <ivan.korolev@itseez.com>
Date: Fri, 21 Sep 2012 16:54:21 +0400
Subject: [PATCH] Added Rotation motion mode in the videostab module

---
 .../include/opencv2/videostab/motion_core.hpp      | 13 +++---
 modules/videostab/src/global_motion.cpp            | 46 +++++++++++++++++++++-
 2 files changed, 53 insertions(+), 6 deletions(-)

diff --git a/modules/videostab/include/opencv2/videostab/motion_core.hpp b/modules/videostab/include/opencv2/videostab/motion_core.hpp
index b9f3614..7d9981d 100644
--- a/modules/videostab/include/opencv2/videostab/motion_core.hpp
+++ b/modules/videostab/include/opencv2/videostab/motion_core.hpp
@@ -55,11 +55,12 @@ enum MotionModel
 {
     MM_TRANSLATION = 0,
     MM_TRANSLATION_AND_SCALE = 1,
-    MM_RIGID = 2,
-    MM_SIMILARITY = 3,
-    MM_AFFINE = 4,
-    MM_HOMOGRAPHY = 5,
-    MM_UNKNOWN = 6
+    MM_ROTATION = 2,
+    MM_RIGID = 3,
+    MM_SIMILARITY = 4,
+    MM_AFFINE = 5,
+    MM_HOMOGRAPHY = 6,
+    MM_UNKNOWN = 7
 };
 
 struct CV_EXPORTS RansacParams
@@ -85,6 +86,8 @@ struct CV_EXPORTS RansacParams
             return RansacParams(1, 0.5f, 0.5f, 0.99f);
         if (model == MM_TRANSLATION_AND_SCALE)
             return RansacParams(2, 0.5f, 0.5f, 0.99f);
+        if (model == MM_ROTATION)
+            return RansacParams(1, 0.5f, 0.5f, 0.99f);
         if (model == MM_RIGID)
             return RansacParams(2, 0.5f, 0.5f, 0.99f);
         if (model == MM_SIMILARITY)
diff --git a/modules/videostab/src/global_motion.cpp b/modules/videostab/src/global_motion.cpp
index d7ae1a5..cc4b2a2 100644
--- a/modules/videostab/src/global_motion.cpp
+++ b/modules/videostab/src/global_motion.cpp
@@ -151,8 +151,51 @@ static Mat estimateGlobMotionLeastSquaresTranslationAndScale(
     return T1.inv() * M * T0;
 }
 
+static Mat estimateGlobMotionLeastSquaresRotation(
+        int npoints, Point2f *points0, Point2f *points1, float *rmse)
+{
+    Point2f p0, p1;
+    float A(0), B(0);
+    for(int i=0; i<npoints; ++i)
+    {
+        p0 = points0[i];
+        p1 = points1[i];
+
+        A += p0.x*p1.x + p0.y*p1.y;
+        B += p0.x*p1.y - p1.x*p0.y;
+    }
+
+    // A*sin(alpha) + B*cos(alpha) = 0
+    float C = sqrt(A*A + B*B);
+    Mat_<float> M = Mat::eye(3, 3, CV_32F);
+    if ( C != 0 )
+    {
+        float sinAlpha = - B / C;
+        float cosAlpha = A / C;
+
+        M(0,0) = cosAlpha;
+        M(1,1) = M(0,0);
+        M(0,1) = sinAlpha;
+        M(1,0) = - M(0,1);
+    }
+
+    if (rmse)
+    {
+        *rmse = 0;
+        for (int i = 0; i < npoints; ++i)
+        {
+            p0 = points0[i];
+            p1 = points1[i];
+            *rmse += sqr(p1.x - M(0,0)*p0.x - M(0,1)*p0.y) +
+                     sqr(p1.y - M(1,0)*p0.x - M(1,1)*p0.y);
+        }
+        *rmse = sqrt(*rmse / npoints);
+    }
+
+    return M;
+}
 
-static Mat estimateGlobMotionLeastSquaresRigid(
+static Mat  estimateGlobMotionLeastSquaresRigid(
         int npoints, Point2f *points0, Point2f *points1, float *rmse)
 {
     Point2f mean0(0.f, 0.f);
@@ -294,6 +337,7 @@ Mat estimateGlobalMotionLeastSquares(
     typedef Mat (*Impl)(int, Point2f*, Point2f*, float*);
     static Impl impls[] = { estimateGlobMotionLeastSquaresTranslation,
                             estimateGlobMotionLeastSquaresTranslationAndScale,
+                            estimateGlobMotionLeastSquaresRotation,
                             estimateGlobMotionLeastSquaresRigid,
                             estimateGlobMotionLeastSquaresSimilarity,
                             estimateGlobMotionLeastSquaresAffine };
-- 
2.7.4