From 39cd13583fff532f7502477735e9ed776e8d905a Mon Sep 17 00:00:00 2001
From: Alexey Spizhevoy <no@email>
Date: Mon, 9 Apr 2012 11:58:56 +0000
Subject: [PATCH] Fixed bug in motion stabilization pipeline and updated wobble
 stabilizer (videostab)

---
 modules/videostab/src/motion_stabilizing.cpp |  5 +++-
 modules/videostab/src/stabilizer.cpp         | 25 ++++++++++++++++++++
 modules/videostab/src/wobble_suppression.cpp | 11 +++++----
 3 files changed, 35 insertions(+), 6 deletions(-)

diff --git a/modules/videostab/src/motion_stabilizing.cpp b/modules/videostab/src/motion_stabilizing.cpp
index e255ff8cd8..169da998b5 100644
--- a/modules/videostab/src/motion_stabilizing.cpp
+++ b/modules/videostab/src/motion_stabilizing.cpp
@@ -56,7 +56,10 @@ void MotionStabilizationPipeline::stabilize(
         int size, const vector<Mat> &motions, pair<int,int> range,
         Mat *stabilizationMotions) const
 {
-    vector<Mat> updatedMotions(motions);
+    vector<Mat> updatedMotions(motions.size());
+    for (size_t i = 0; i < motions.size(); ++i)
+        updatedMotions[i] = motions[i].clone();
+
     vector<Mat> stabilizationMotions_(size);
 
     for (int i = 0; i < size; ++i)
diff --git a/modules/videostab/src/stabilizer.cpp b/modules/videostab/src/stabilizer.cpp
index 2544f9c3b2..728fae6b2e 100644
--- a/modules/videostab/src/stabilizer.cpp
+++ b/modules/videostab/src/stabilizer.cpp
@@ -371,6 +371,31 @@ void TwoPassStabilizer::runPrePassIfNecessary()
         motionStabilizer_->stabilize(
             frameCount_, motions_, make_pair(0, frameCount_ - 1), &stabilizationMotions_[0]);
 
+        /*ofstream fm("log_motions.csv");
+        for (int i = 0; i < frameCount_ - 1; ++i)
+        {
+            Mat_<float> M = at(i, motions_);
+            fm << M(0,0) << " " << M(0,1) << " " << M(0,2) << " "
+               << M(1,0) << " " << M(1,1) << " " << M(1,2) << " "
+               << M(2,0) << " " << M(2,1) << " " << M(2,2) << endl;
+        }
+        ofstream fo("log_orig.csv");
+        for (int i = 0; i < frameCount_; ++i)
+        {
+            Mat_<float> M = getMotion(0, i, motions_);
+            fo << M(0,0) << " " << M(0,1) << " " << M(0,2) << " "
+               << M(1,0) << " " << M(1,1) << " " << M(1,2) << " "
+               << M(2,0) << " " << M(2,1) << " " << M(2,2) << endl;
+        }
+        ofstream fs("log_stab.csv");
+        for (int i = 0; i < frameCount_; ++i)
+        {
+            Mat_<float> M = stabilizationMotions_[i] * getMotion(0, i, motions_);
+            fs << M(0,0) << " " << M(0,1) << " " << M(0,2) << " "
+               << M(1,0) << " " << M(1,1) << " " << M(1,2) << " "
+               << M(2,0) << " " << M(2,1) << " " << M(2,2) << endl;
+        }*/
+
         if (mustEstTrimRatio_)
         {
             trimRatio_ = 0;
diff --git a/modules/videostab/src/wobble_suppression.cpp b/modules/videostab/src/wobble_suppression.cpp
index d997e0c827..1a0521177f 100644
--- a/modules/videostab/src/wobble_suppression.cpp
+++ b/modules/videostab/src/wobble_suppression.cpp
@@ -104,24 +104,25 @@ void MoreAccurateMotionWobbleSuppressor::suppress(int idx, const Mat &frame, Mat
             yl = ML(1,0)*x + ML(1,1)*y + ML(1,2);
             zl = ML(2,0)*x + ML(2,1)*y + ML(2,2);
             xl /= zl; yl /= zl;
-            wl = 1.f / (sqrt(sqr(x - xl) + sqr(y - yl)) + 1e-5f);
+            wl = idx - k1;
 
             xr = MR(0,0)*x + MR(0,1)*y + MR(0,2);
             yr = MR(1,0)*x + MR(1,1)*y + MR(1,2);
             zr = MR(2,0)*x + MR(2,1)*y + MR(2,2);
             xr /= zr; yr /= zr;
-            wr = 1.f / (sqrt(sqr(x - xr) + sqr(y - yr)) + 1e-5f);
+            wr = k2 - idx;
 
-            mapx_(y,x) = (wl * xl + wr * xr) / (wl + wr);
-            mapy_(y,x) = (wl * yl + wr * yr) / (wl + wr);
+            mapx_(y,x) = (wr * xl + wl * xr) / (wl + wr);
+            mapy_(y,x) = (wr * yl + wl * yr) / (wl + wr);
         }
     }
 
     if (result.data == frame.data)
         result = Mat(frame.size(), frame.type());
 
-    remap(frame, result, mapx_, mapy_, INTER_LINEAR);
+    remap(frame, result, mapx_, mapy_, INTER_LANCZOS4, BORDER_REPLICATE);
 }
 
 } // namespace videostab
 } // namespace cv
+