Updated wobble suppression code in videostab module

modules/videostab/include/opencv2/videostab/global_motion.hpp

@@ -131,7 +131,7 @@ private:
 class CV_EXPORTS PyrLkRobustMotionEstimator : public GlobalMotionEstimatorBase
 {
 public:
-    PyrLkRobustMotionEstimator();
+    PyrLkRobustMotionEstimator(MotionModel model = AFFINE);
 
     void setDetector(Ptr<FeatureDetector> val) { detector_ = val; }
     Ptr<FeatureDetector> detector() const { return detector_; }

modules/videostab/include/opencv2/videostab/wobble_suppression.hpp

@@ -97,7 +97,16 @@ public:
 class CV_EXPORTS MoreAccurateMotionWobbleSuppressor : public WobbleSuppressorBase
 {
 public:
+    MoreAccurateMotionWobbleSuppressor();
+
+    void setPeriod(int val) { period_ = val; }
+    int period() const { return period_; }
+
     virtual void suppress(int idx, const Mat &frame, Mat &result);
+
+private:
+    int period_;
+    Mat_<float> mapx_, mapy_;
 };
 
 } // namespace videostab

modules/videostab/src/global_motion.cpp

@@ -323,12 +323,21 @@ Mat ToFileMotionWriter::estimate(const Mat &frame0, const Mat &frame1)
 }
 
 
-PyrLkRobustMotionEstimator::PyrLkRobustMotionEstimator()
-    : ransacParams_(RansacParams::affine2dMotionStd())
+PyrLkRobustMotionEstimator::PyrLkRobustMotionEstimator(MotionModel model)
 {
     setDetector(new GoodFeaturesToTrackDetector());
     setOptFlowEstimator(new SparsePyrLkOptFlowEstimator());
-    setMotionModel(AFFINE);
+    setMotionModel(model);
+    if (model == TRANSLATION)
+        setRansacParams(RansacParams::translation2dMotionStd());
+    else if (model == TRANSLATION_AND_SCALE)
+        setRansacParams(RansacParams::translationAndScale2dMotionStd());
+    else if (model == LINEAR_SIMILARITY)
+        setRansacParams(RansacParams::linearSimilarity2dMotionStd());
+    else if (model == AFFINE)
+        setRansacParams(RansacParams::affine2dMotionStd());
+    else if (model == HOMOGRAPHY)
+        setRansacParams(RansacParams::homography2dMotionStd());
     setMaxRmse(0.5f);
     setMinInlierRatio(0.1f);
 }

modules/videostab/src/wobble_suppression.cpp

@@ -51,8 +51,7 @@ namespace cv
 namespace videostab
 {
 
-WobbleSuppressorBase::WobbleSuppressorBase()
-    : motions_(0), stabilizationMotions_(0)
+WobbleSuppressorBase::WobbleSuppressorBase() : motions_(0), stabilizationMotions_(0)
 {
     PyrLkRobustMotionEstimator *est = new PyrLkRobustMotionEstimator();
     est->setMotionModel(HOMOGRAPHY);
@@ -67,14 +66,61 @@ void NullWobbleSuppressor::suppress(int /*idx*/, const Mat &frame, Mat &result)
 }
 
 
+MoreAccurateMotionWobbleSuppressor::MoreAccurateMotionWobbleSuppressor()
+{
+    setPeriod(30);
+}
+
+
 void MoreAccurateMotionWobbleSuppressor::suppress(int idx, const Mat &frame, Mat &result)
 {
     CV_Assert(motions_ && stabilizationMotions_);
 
-    // TODO implement
-    CV_Error(CV_StsNotImplemented, "MoreAccurateMotionWobbleSuppressor");
+    if (idx % period_ == 0)
+    {
+        result = frame;
+        return;
+    }
 
-    result = frame;
+    int k1 = idx / period_ * period_;
+    int k2 = std::min(k1 + period_, frameCount_ - 1);
+
+    Mat S1 = (*stabilizationMotions_)[idx];
+
+    Mat_<float> ML = S1 * getMotion(k1, idx, *motions2_) * getMotion(k1, idx, *motions_).inv() * S1.inv();
+    Mat_<float> MR = S1 * getMotion(idx, k2, *motions2_).inv() * getMotion(idx, k2, *motions_) * S1.inv();
+
+    mapx_.create(frame.size());
+    mapy_.create(frame.size());
+
+    float xl, yl, zl, wl;
+    float xr, yr, zr, wr;
+
+    for (int y = 0; y < frame.rows; ++y)
+    {
+        for (int x = 0; x < frame.cols; ++x)
+        {
+            xl = ML(0,0)*x + ML(0,1)*y + ML(0,2);
+            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);
+
+            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);
+
+            mapx_(y,x) = (wl * xl + wr * xr) / (wl + wr);
+            mapy_(y,x) = (wl * yl + wr * yr) / (wl + wr);
+        }
+    }
+
+    if (result.data == frame.data)
+        result = Mat(frame.size(), frame.type());
+
+    remap(frame, result, mapx_, mapy_, INTER_LINEAR);
 }
 
 } // namespace videostab