Added LP based global motion estimation (videostab)

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

@@ -57,6 +57,8 @@
   #include "opencv2/gpu/gpu.hpp"
 #endif
 
+// TODO remove code duplications (in cpp too)
+
 namespace cv
 {
 namespace videostab
@@ -91,6 +93,7 @@ class CV_EXPORTS FromFileMotionReader : public GlobalMotionEstimatorBase
 {
 public:
     FromFileMotionReader(const std::string &path);
+
     virtual Mat estimate(const Mat &frame0, const Mat &frame1, bool *ok = 0);
 
 private:
@@ -101,6 +104,7 @@ class CV_EXPORTS ToFileMotionWriter : public GlobalMotionEstimatorBase
 {
 public:
     ToFileMotionWriter(const std::string &path, Ptr<GlobalMotionEstimatorBase> estimator);
+
     virtual Mat estimate(const Mat &frame0, const Mat &frame1, bool *ok = 0);
 
 private:
@@ -108,30 +112,10 @@ private:
     Ptr<GlobalMotionEstimatorBase> estimator_;
 };
 
-class CV_EXPORTS PyrLkRobustMotionEstimatorBase : public GlobalMotionEstimatorBase
-{
-public:
-    virtual void setRansacParams(const RansacParams &val) { ransacParams_ = val; }
-    virtual RansacParams ransacParams() const { return ransacParams_; }
-
-    virtual void setOutlierRejector(Ptr<IOutlierRejector> val) { outlierRejector_ = val; }
-    virtual Ptr<IOutlierRejector> outlierRejector() const { return outlierRejector_; }
-
-    virtual void setMinInlierRatio(float val) { minInlierRatio_ = val; }
-    virtual float minInlierRatio() const { return minInlierRatio_; }
-
-protected:
-    PyrLkRobustMotionEstimatorBase(MotionModel model);
-
-    RansacParams ransacParams_;
-    Ptr<IOutlierRejector> outlierRejector_;
-    float minInlierRatio_;
-};
-
-class CV_EXPORTS PyrLkRobustMotionEstimator : public PyrLkRobustMotionEstimatorBase
+class CV_EXPORTS RansacMotionEstimator : public GlobalMotionEstimatorBase
 {
 public:
-    PyrLkRobustMotionEstimator(MotionModel model = MM_AFFINE);
+    RansacMotionEstimator(MotionModel model = MM_AFFINE);
 
     void setDetector(Ptr<FeatureDetector> val) { detector_ = val; }
     Ptr<FeatureDetector> detector() const { return detector_; }
@@ -142,12 +126,24 @@ public:
     void setGridSize(Size val) { gridSize_ = val; }
     Size gridSize() const { return gridSize_; }
 
+    void setRansacParams(const RansacParams &val) { ransacParams_ = val; }
+    RansacParams ransacParams() const { return ransacParams_; }
+
+    void setOutlierRejector(Ptr<IOutlierRejector> val) { outlierRejector_ = val; }
+    Ptr<IOutlierRejector> outlierRejector() const { return outlierRejector_; }
+
+    void setMinInlierRatio(float val) { minInlierRatio_ = val; }
+    float minInlierRatio() const { return minInlierRatio_; }
+
     virtual Mat estimate(const Mat &frame0, const Mat &frame1, bool *ok = 0);
 
 private:
     Ptr<FeatureDetector> detector_;
     Ptr<ISparseOptFlowEstimator> optFlowEstimator_;
     Size gridSize_;
+    RansacParams ransacParams_;
+    Ptr<IOutlierRejector> outlierRejector_;
+    float minInlierRatio_;
 
     std::vector<uchar> status_;
     std::vector<KeyPoint> keypointsPrev_;
@@ -156,10 +152,19 @@ private:
 };
 
 #if HAVE_OPENCV_GPU
-class CV_EXPORTS PyrLkRobustMotionEstimatorGpu : public PyrLkRobustMotionEstimatorBase
+class CV_EXPORTS RansacMotionEstimatorGpu : public GlobalMotionEstimatorBase
 {
 public:
-    PyrLkRobustMotionEstimatorGpu(MotionModel model = MM_AFFINE);
+    RansacMotionEstimatorGpu(MotionModel model = MM_AFFINE);
+
+    void setRansacParams(const RansacParams &val) { ransacParams_ = val; }
+    RansacParams ransacParams() const { return ransacParams_; }
+
+    void setOutlierRejector(Ptr<IOutlierRejector> val) { outlierRejector_ = val; }
+    Ptr<IOutlierRejector> outlierRejector() const { return outlierRejector_; }
+
+    void setMinInlierRatio(float val) { minInlierRatio_ = val; }
+    float minInlierRatio() const { return minInlierRatio_; }
 
     virtual Mat estimate(const Mat &frame0, const Mat &frame1, bool *ok = 0);
     Mat estimate(const gpu::GpuMat &frame0, const gpu::GpuMat &frame1, bool *ok = 0);
@@ -167,6 +172,9 @@ public:
 private:
     gpu::GoodFeaturesToTrackDetector_GPU detector_;
     SparsePyrLkOptFlowEstimatorGpu optFlowEstimator_;
+    RansacParams ransacParams_;
+    Ptr<IOutlierRejector> outlierRejector_;
+    float minInlierRatio_;
 
     gpu::GpuMat frame0_, grayFrame0_, frame1_;
     gpu::GpuMat pointsPrev_, points_;
@@ -178,6 +186,44 @@ private:
 };
 #endif
 
+class CV_EXPORTS LpBasedMotionEstimator : public GlobalMotionEstimatorBase
+{
+public:
+    LpBasedMotionEstimator(MotionModel model = MM_AFFINE);
+
+    void setDetector(Ptr<FeatureDetector> val) { detector_ = val; }
+    Ptr<FeatureDetector> detector() const { return detector_; }
+
+    void setOptFlowEstimator(Ptr<ISparseOptFlowEstimator> val) { optFlowEstimator_ = val; }
+    Ptr<ISparseOptFlowEstimator> optFlowEstimator() const { return optFlowEstimator_; }
+
+    void setOutlierRejector(Ptr<IOutlierRejector> val) { outlierRejector_ = val; }
+    Ptr<IOutlierRejector> outlierRejector() const { return outlierRejector_; }
+
+    virtual Mat estimate(const Mat &frame0, const Mat &frame1, bool *ok);
+
+private:
+    Ptr<FeatureDetector> detector_;
+    Ptr<ISparseOptFlowEstimator> optFlowEstimator_;
+    Ptr<IOutlierRejector> outlierRejector_;
+
+    std::vector<uchar> status_;
+    std::vector<KeyPoint> keypointsPrev_;
+    std::vector<Point2f> pointsPrev_, points_;
+    std::vector<Point2f> pointsPrevGood_, pointsGood_;
+
+    std::vector<double> obj_, collb_, colub_;
+    std::vector<int> rows_, cols_;
+    std::vector<double> elems_, rowlb_, rowub_;
+
+    void set(int row, int col, double coef)
+    {
+        rows_.push_back(row);
+        cols_.push_back(col);
+        elems_.push_back(coef);
+    }
+};
+
 CV_EXPORTS Mat getMotion(int from, int to, const std::vector<Mat> &motions);
 
 } // namespace videostab

modules/videostab/src/clp.hpp

+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#ifndef __OPENCV_VIDEOSTAB_CLP_HPP__
+#define __OPENCV_VIDEOSTAB_CLP_HPP__
+
+#ifdef HAVE_CLP
+  #include "ClpSimplex.hpp"
+  #include "ClpPresolve.hpp"
+  #include "ClpPrimalColumnSteepest.hpp"
+  #include "ClpDualRowSteepest.hpp"
+  #define INF 1e10
+#endif
+
+// Clp replaces min and max with ?: globally, we can't use std::min and std::max in case
+// when HAVE_CLP is true. We create the defines by ourselves when HAVE_CLP == 0.
+#ifndef min
+  #define min(a,b) std::min(a,b)
+#endif
+#ifndef max
+  #define max(a,b) std::max(a,b)
+#endif
+
+#endif

modules/videostab/src/global_motion.cpp

@@ -45,6 +45,7 @@
 #include "opencv2/videostab/ring_buffer.hpp"
 #include "opencv2/videostab/outlier_rejection.hpp"
 #include "opencv2/opencv_modules.hpp"
+#include "clp.hpp"
 
 using namespace std;
 
@@ -434,25 +435,19 @@ Mat ToFileMotionWriter::estimate(const Mat &frame0, const Mat &frame1, bool *ok)
 }
 
 
-PyrLkRobustMotionEstimatorBase::PyrLkRobustMotionEstimatorBase(MotionModel model)
+RansacMotionEstimator::RansacMotionEstimator(MotionModel model)
     : GlobalMotionEstimatorBase(model)
-{
-    setRansacParams(RansacParams::default2dMotion(model));
-    setOutlierRejector(new NullOutlierRejector());
-    setMinInlierRatio(0.1f);
-}
-
-
-PyrLkRobustMotionEstimator::PyrLkRobustMotionEstimator(MotionModel model)
-    : PyrLkRobustMotionEstimatorBase(model)
 {
     setDetector(new GoodFeaturesToTrackDetector());
     setOptFlowEstimator(new SparsePyrLkOptFlowEstimator());
     setGridSize(Size(0,0));
+    setRansacParams(RansacParams::default2dMotion(model));
+    setOutlierRejector(new NullOutlierRejector());
+    setMinInlierRatio(0.1f);
 }
 
 
-Mat PyrLkRobustMotionEstimator::estimate(const Mat &frame0, const Mat &frame1, bool *ok)
+Mat RansacMotionEstimator::estimate(const Mat &frame0, const Mat &frame1, bool *ok)
 {
     // find keypoints
 
@@ -549,14 +544,17 @@ Mat PyrLkRobustMotionEstimator::estimate(const Mat &frame0, const Mat &frame1, b
 
 
 #if HAVE_OPENCV_GPU
-PyrLkRobustMotionEstimatorGpu::PyrLkRobustMotionEstimatorGpu(MotionModel model)
-    : PyrLkRobustMotionEstimatorBase(model)
+RansacMotionEstimatorGpu::RansacMotionEstimatorGpu(MotionModel model)
+    : GlobalMotionEstimatorBase(model)
 {
     CV_Assert(gpu::getCudaEnabledDeviceCount() > 0);
+    setRansacParams(RansacParams::default2dMotion(model));
+    setOutlierRejector(new NullOutlierRejector());
+    setMinInlierRatio(0.1f);
 }
 
 
-Mat PyrLkRobustMotionEstimatorGpu::estimate(const Mat &frame0, const Mat &frame1, bool *ok)
+Mat RansacMotionEstimatorGpu::estimate(const Mat &frame0, const Mat &frame1, bool *ok)
 {
     frame0_.upload(frame0);
     frame1_.upload(frame1);
@@ -564,7 +562,7 @@ Mat PyrLkRobustMotionEstimatorGpu::estimate(const Mat &frame0, const Mat &frame1
 }
 
 
-Mat PyrLkRobustMotionEstimatorGpu::estimate(const gpu::GpuMat &frame0, const gpu::GpuMat &frame1, bool *ok)
+Mat RansacMotionEstimatorGpu::estimate(const gpu::GpuMat &frame0, const gpu::GpuMat &frame1, bool *ok)
 {
     // convert frame to gray if it's color
 
@@ -652,6 +650,188 @@ Mat PyrLkRobustMotionEstimatorGpu::estimate(const gpu::GpuMat &frame0, const gpu
 #endif // #if HAVE_OPENCV_GPU
 
 
+LpBasedMotionEstimator::LpBasedMotionEstimator(MotionModel model)
+    : GlobalMotionEstimatorBase(model)
+{
+    setDetector(new GoodFeaturesToTrackDetector());
+    setOptFlowEstimator(new SparsePyrLkOptFlowEstimator());
+    setOutlierRejector(new NullOutlierRejector());
+}
+
+// TODO will estimation of all motions as one LP problem be faster?
+
+Mat LpBasedMotionEstimator::estimate(const Mat &frame0, const Mat &frame1, bool *ok)
+{
+    // find keypoints
+
+    detector_->detect(frame0, keypointsPrev_);
+
+    // extract points from keypoints
+
+    pointsPrev_.resize(keypointsPrev_.size());
+    for (size_t i = 0; i < keypointsPrev_.size(); ++i)
+        pointsPrev_[i] = keypointsPrev_[i].pt;
+
+    // find correspondences
+
+    optFlowEstimator_->run(frame0, frame1, pointsPrev_, points_, status_, noArray());
+
+    // leave good correspondences only
+
+    pointsPrevGood_.clear(); pointsPrevGood_.reserve(points_.size());
+    pointsGood_.clear(); pointsGood_.reserve(points_.size());
+
+    for (size_t i = 0; i < points_.size(); ++i)
+    {
+        if (status_[i])
+        {
+            pointsPrevGood_.push_back(pointsPrev_[i]);
+            pointsGood_.push_back(points_[i]);
+        }
+    }
+
+    // perfrom outlier rejection
+
+    IOutlierRejector *outlierRejector = static_cast<IOutlierRejector*>(outlierRejector_);
+    if (!dynamic_cast<NullOutlierRejector*>(outlierRejector))
+    {
+        pointsPrev_.swap(pointsPrevGood_);
+        points_.swap(pointsGood_);
+
+        outlierRejector_->process(frame0.size(), pointsPrev_, points_, status_);
+
+        pointsPrevGood_.clear(); pointsPrevGood_.reserve(points_.size());
+        pointsGood_.clear(); pointsGood_.reserve(points_.size());
+
+        for (size_t i = 0; i < points_.size(); ++i)
+        {
+            if (status_[i])
+            {
+                pointsPrevGood_.push_back(pointsPrev_[i]);
+                pointsGood_.push_back(points_[i]);
+            }
+        }
+    }
+
+    int npoints = static_cast<int>(pointsGood_.size());
+
+    // prepare LP problem
+
+#ifndef HAVE_CLP
+
+    CV_Error(CV_StsError, "The library is built without Clp support");
+    if (ok) *ok = false;
+    return Mat::eye(3, 3, CV_32F);
+
+#else
+
+    CV_Assert(motionModel_ <= MM_AFFINE && motionModel_ != MM_RIGID);
+
+    int ncols = 6 + 2*npoints;
+    int nrows = 4*npoints;
+
+    if (motionModel_ == MM_SIMILARITY)
+        nrows += 2;
+    else if (motionModel_ == MM_TRANSLATION_AND_SCALE)
+        nrows += 3;
+    else if (motionModel_ == MM_TRANSLATION)
+        nrows += 4;
+
+    rows_.clear();
+    cols_.clear();
+    elems_.clear();
+
+    obj_.assign(ncols, 0);
+    collb_.assign(ncols, -INF);
+    colub_.assign(ncols, INF);
+
+    int c = 6;
+
+    for (int i = 0; i < npoints; ++i, c += 2)
+    {
+        obj_[c] = 1;
+        collb_[c] = 0;
+
+        obj_[c+1] = 1;
+        collb_[c+1] = 0;
+    }
+
+    elems_.clear();
+    rowlb_.assign(nrows, -INF);
+    rowub_.assign(nrows, INF);
+
+    int r = 0;
+    Point2f p0, p1;
+
+    for (int i = 0; i < npoints; ++i, r += 4)
+    {
+        p0 = pointsPrevGood_[i];
+        p1 = pointsGood_[i];
+
+        set(r, 0, p0.x); set(r, 1, p0.y); set(r, 2, 1); set(r, 6+2*i, -1);
+        rowub_[r] = p1.x;
+
+        set(r+1, 3, p0.x); set(r+1, 4, p0.y); set(r+1, 5, 1); set(r+1, 6+2*i+1, -1);
+        rowub_[r+1] = p1.y;
+
+        set(r+2, 0, p0.x); set(r+2, 1, p0.y); set(r+2, 2, 1); set(r+2, 6+2*i, 1);
+        rowlb_[r+2] = p1.x;
+
+        set(r+3, 3, p0.x); set(r+3, 4, p0.y); set(r+3, 5, 1); set(r+3, 6+2*i+1, 1);
+        rowlb_[r+3] = p1.y;
+    }
+
+    if (motionModel_ == MM_SIMILARITY)
+    {
+        set(r, 0, 1); set(r, 4, -1); rowlb_[r] = rowub_[r] = 0;
+        set(r+1, 1, 1); set(r+1, 3, 1); rowlb_[r+1] = rowub_[r+1] = 0;
+    }
+    else if (motionModel_ == MM_TRANSLATION_AND_SCALE)
+    {
+        set(r, 0, 1); set(r, 4, -1); rowlb_[r] = rowub_[r] = 0;
+        set(r+1, 1, 1); rowlb_[r+1] = rowub_[r+1] = 0;
+        set(r+2, 3, 1); rowlb_[r+2] = rowub_[r+2] = 0;
+    }
+    else if (motionModel_ == MM_TRANSLATION)
+    {
+        set(r, 0, 1); rowlb_[r] = rowub_[r] = 1;
+        set(r+1, 1, 1); rowlb_[r+1] = rowub_[r+1] = 0;
+        set(r+2, 3, 1); rowlb_[r+2] = rowub_[r+2] = 0;
+        set(r+3, 4, 1); rowlb_[r+3] = rowub_[r+3] = 1;
+    }
+
+    // solve
+
+    CoinPackedMatrix A(true, &rows_[0], &cols_[0], &elems_[0], elems_.size());
+    A.setDimensions(nrows, ncols);
+
+    ClpSimplex model(false);
+    model.loadProblem(A, &collb_[0], &colub_[0], &obj_[0], &rowlb_[0], &rowub_[0]);
+
+    ClpDualRowSteepest dualSteep(1);
+    model.setDualRowPivotAlgorithm(dualSteep);
+    model.scaling(1);
+
+    model.dual();
+
+    // extract motion
+
+    const double *sol = model.getColSolution();
+
+    Mat_<float> M = Mat::eye(3, 3, CV_32F);
+    M(0,0) = sol[0];
+    M(0,1) = sol[1];
+    M(0,2) = sol[2];
+    M(1,0) = sol[3];
+    M(1,1) = sol[4];
+    M(1,2) = sol[5];
+
+    if (ok) *ok = true;
+    return M;
+#endif
+}
+
+
 Mat getMotion(int from, int to, const vector<Mat> &motions)
 {
     Mat M = Mat::eye(3, 3, CV_32F);

modules/videostab/src/motion_stabilizing.cpp

@@ -44,23 +44,7 @@
 #include "opencv2/videostab/motion_stabilizing.hpp"
 #include "opencv2/videostab/global_motion.hpp"
 #include "opencv2/videostab/ring_buffer.hpp"
-
-#ifdef HAVE_CLP
-  #include "ClpSimplex.hpp"
-  #include "ClpPresolve.hpp"
-  #include "ClpPrimalColumnSteepest.hpp"
-  #include "ClpDualRowSteepest.hpp"    
-  #define INF 1e10
-#endif
-
-// Clp replaces min and max with ?: globally, we can't use std::min and std::max in case 
-// when HAVE_CLP is true, otherwise we create the defines by ourselves
-#ifndef min
-  #define min(a,b) std::min(a,b)
-#endif
-#ifndef max
-  #define max(a,b) std::max(a,b)
-#endif
+#include "clp.hpp"
 
 using namespace std;
 
@@ -172,6 +156,10 @@ void LpMotionStabilizer::stabilize(
     int ncols = 4*N + 6*(N-1) + 6*(N-2) + 6*(N-3);
     int nrows = 8*N + 2*6*(N-1) + 2*6*(N-2) + 2*6*(N-3);
 
+    rows_.clear();
+    cols_.clear();
+    elems_.clear();
+
     obj_.assign(ncols, 0);
     collb_.assign(ncols, -INF);
     colub_.assign(ncols, INF);
@@ -574,10 +562,8 @@ void LpMotionStabilizer::stabilize(
         S0(0,2) = sol[c+2];
         S0(1,2) = sol[c+3];
         S[t] = S0;
-    }
+    }    
 }
-
-
 #endif // #ifndef HAVE_CLP
 
 

modules/videostab/src/stabilizer.cpp

@@ -58,7 +58,7 @@ StabilizerBase::StabilizerBase()
 {
     setLog(new LogToStdout());
     setFrameSource(new NullFrameSource());
-    setMotionEstimator(new PyrLkRobustMotionEstimator());
+    setMotionEstimator(new RansacMotionEstimator());
     setDeblurer(new NullDeblurer());
     setInpainter(new NullInpainter());
     setRadius(15);

modules/videostab/src/wobble_suppression.cpp

@@ -53,7 +53,7 @@ namespace videostab
 
 WobbleSuppressorBase::WobbleSuppressorBase() : motions_(0), stabilizationMotions_(0)
 {
-    PyrLkRobustMotionEstimator *est = new PyrLkRobustMotionEstimator();
+    RansacMotionEstimator *est = new RansacMotionEstimator();
     est->setMotionModel(MM_HOMOGRAPHY);
     est->setRansacParams(RansacParams::default2dMotion(MM_HOMOGRAPHY));
 }