text: /*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*/
  
  #include "precomp.hpp"
  #include "opencv2/videostab/global_motion.hpp"
  #include "opencv2/videostab/ring_buffer.hpp"
  #include "opencv2/videostab/outlier_rejection.hpp"
  #include BLIK_OPENCV_U_opencv2__opencv_modules_hpp //original-code:"opencv2/opencv_modules.hpp"
  #include "clp.hpp"
  
  #include BLIK_OPENCV_U_opencv2__core__private_D_cuda_hpp //original-code:"opencv2/core/private.cuda.hpp"
  
  #if defined(HAVE_OPENCV_CUDAIMGPROC) && defined(HAVE_OPENCV_CUDAOPTFLOW)
      #if !defined HAVE_CUDA || defined(CUDA_DISABLER)
          namespace cv { namespace cuda {
              static void compactPoints(GpuMat&, GpuMat&, const GpuMat&) { throw_no_cuda(); }
          }}
      #else
          namespace cv { namespace cuda { namespace device { namespace globmotion {
              int compactPoints(int N, float *points0, float *points1, const uchar *mask);
          }}}}
          namespace cv { namespace cuda {
              static void compactPoints(GpuMat &points0, GpuMat &points1, const GpuMat &mask)
              {
                  CV_Assert(points0.rows == 1 && points1.rows == 1 && mask.rows == 1);
                  CV_Assert(points0.type() == CV_32FC2 && points1.type() == CV_32FC2 && mask.type() == CV_8U);
                  CV_Assert(points0.cols == mask.cols && points1.cols == mask.cols);
  
                  int npoints = points0.cols;
                  int remaining = cv::cuda::device::globmotion::compactPoints(
                          npoints, (float*)points0.data, (float*)points1.data, mask.data);
  
                  points0 = points0.colRange(0, remaining);
                  points1 = points1.colRange(0, remaining);
              }
          }}
      #endif
  #endif
  
  namespace cv
  {
  namespace videostab
  {
  
  // does isotropic normalization
  static Mat normalizePoints(int npoints, Point2f *points)
  {
      float cx = 0.f, cy = 0.f;
      for (int i = 0; i < npoints; ++i)
      {
          cx += points[i].x;
          cy += points[i].y;
      }
      cx /= npoints;
      cy /= npoints;
  
      float d = 0.f;
      for (int i = 0; i < npoints; ++i)
      {
          points[i].x -= cx;
          points[i].y -= cy;
          d += std::sqrt(sqr(points[i].x) + sqr(points[i].y));
      }
      d /= npoints;
  
      float s = std::sqrt(2.f) / d;
      for (int i = 0; i < npoints; ++i)
      {
          points[i].x *= s;
          points[i].y *= s;
      }
  
      Mat_<float> T = Mat::eye(3, 3, CV_32F);
      T(0,0) = T(1,1) = s;
      T(0,2) = -cx*s;
      T(1,2) = -cy*s;
      return T;
  }
  
  
  static Mat estimateGlobMotionLeastSquaresTranslation(
          int npoints, Point2f *points0, Point2f *points1, float *rmse)
  {
      Mat_<float> M = Mat::eye(3, 3, CV_32F);
      for (int i = 0; i < npoints; ++i)
      {
          M(0,2) += points1[i].x - points0[i].x;
          M(1,2) += points1[i].y - points0[i].y;
      }
      M(0,2) /= npoints;
      M(1,2) /= npoints;
  
      if (rmse)
      {
          *rmse = 0;
          for (int i = 0; i < npoints; ++i)
              *rmse += sqr(points1[i].x - points0[i].x - M(0,2)) +
                       sqr(points1[i].y - points0[i].y - M(1,2));
          *rmse = std::sqrt(*rmse / npoints);
      }
  
      return M;
  }
  
  
  static Mat estimateGlobMotionLeastSquaresTranslationAndScale(
          int npoints, Point2f *points0, Point2f *points1, float *rmse)
  {
      Mat_<float> T0 = normalizePoints(npoints, points0);
      Mat_<float> T1 = normalizePoints(npoints, points1);
  
      Mat_<float> A(2*npoints, 3), b(2*npoints, 1);
      float *a0, *a1;
      Point2f p0, p1;
  
      for (int i = 0; i < npoints; ++i)
      {
          a0 = A[2*i];
          a1 = A[2*i+1];
          p0 = points0[i];
          p1 = points1[i];
          a0[0] = p0.x; a0[1] = 1; a0[2] = 0;
          a1[0] = p0.y; a1[1] = 0; a1[2] = 1;
          b(2*i,0) = p1.x;
          b(2*i+1,0) = p1.y;
      }
  
      Mat_<float> sol;
      solve(A, b, sol, DECOMP_NORMAL | DECOMP_LU);
  
      if (rmse)
          *rmse = static_cast<float>(norm(A*sol, b, NORM_L2) / std::sqrt(static_cast<double>(npoints)));
  
      Mat_<float> M = Mat::eye(3, 3, CV_32F);
      M(0,0) = M(1,1) = sol(0,0);
      M(0,2) = sol(1,0);
      M(1,2) = sol(2,0);
  
      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 = std::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 = std::sqrt(*rmse / npoints);
      }
  
      return M;
  }
  
  static Mat  estimateGlobMotionLeastSquaresRigid(
          int npoints, Point2f *points0, Point2f *points1, float *rmse)
  {
      Point2f mean0(0.f, 0.f);
      Point2f mean1(0.f, 0.f);
  
      for (int i = 0; i < npoints; ++i)
      {
          mean0 += points0[i];
          mean1 += points1[i];
      }
  
      mean0 *= 1.f / npoints;
      mean1 *= 1.f / npoints;
  
      Mat_<float> A = Mat::zeros(2, 2, CV_32F);
      Point2f pt0, pt1;
  
      for (int i = 0; i < npoints; ++i)
      {
          pt0 = points0[i] - mean0;
          pt1 = points1[i] - mean1;
          A(0,0) += pt1.x * pt0.x;
          A(0,1) += pt1.x * pt0.y;
          A(1,0) += pt1.y * pt0.x;
          A(1,1) += pt1.y * pt0.y;
      }
  
      Mat_<float> M = Mat::eye(3, 3, CV_32F);
  
      SVD svd(A);
      Mat_<float> R = svd.u * svd.vt;
      Mat tmp(M(Rect(0,0,2,2)));
      R.copyTo(tmp);
  
      M(0,2) = mean1.x - R(0,0)*mean0.x - R(0,1)*mean0.y;
      M(1,2) = mean1.y - R(1,0)*mean0.x - R(1,1)*mean0.y;
  
      if (rmse)
      {
          *rmse = 0;
          for (int i = 0; i < npoints; ++i)
          {
              pt0 = points0[i];
              pt1 = points1[i];
              *rmse += sqr(pt1.x - M(0,0)*pt0.x - M(0,1)*pt0.y - M(0,2)) +
                       sqr(pt1.y - M(1,0)*pt0.x - M(1,1)*pt0.y - M(1,2));
          }
          *rmse = std::sqrt(*rmse / npoints);
      }
  
      return M;
  }
  
  
  static Mat estimateGlobMotionLeastSquaresSimilarity(
          int npoints, Point2f *points0, Point2f *points1, float *rmse)
  {
      Mat_<float> T0 = normalizePoints(npoints, points0);
      Mat_<float> T1 = normalizePoints(npoints, points1);
  
      Mat_<float> A(2*npoints, 4), b(2*npoints, 1);
      float *a0, *a1;
      Point2f p0, p1;
  
      for (int i = 0; i < npoints; ++i)
      {
          a0 = A[2*i];
          a1 = A[2*i+1];
          p0 = points0[i];
          p1 = points1[i];
          a0[0] = p0.x; a0[1] = p0.y; a0[2] = 1; a0[3] = 0;
          a1[0] = p0.y; a1[1] = -p0.x; a1[2] = 0; a1[3] = 1;
          b(2*i,0) = p1.x;
          b(2*i+1,0) = p1.y;
      }
  
      Mat_<float> sol;
      solve(A, b, sol, DECOMP_NORMAL | DECOMP_LU);
  
      if (rmse)
          *rmse = static_cast<float>(norm(A*sol, b, NORM_L2) / std::sqrt(static_cast<double>(npoints)));
  
      Mat_<float> M = Mat::eye(3, 3, CV_32F);
      M(0,0) = M(1,1) = sol(0,0);
      M(0,1) = sol(1,0);
      M(1,0) = -sol(1,0);
      M(0,2) = sol(2,0);
      M(1,2) = sol(3,0);
  
      return T1.inv() * M * T0;
  }
  
  
  static Mat estimateGlobMotionLeastSquaresAffine(
          int npoints, Point2f *points0, Point2f *points1, float *rmse)
  {
      Mat_<float> T0 = normalizePoints(npoints, points0);
      Mat_<float> T1 = normalizePoints(npoints, points1);
  
      Mat_<float> A(2*npoints, 6), b(2*npoints, 1);
      float *a0, *a1;
      Point2f p0, p1;
  
      for (int i = 0; i < npoints; ++i)
      {
          a0 = A[2*i];
          a1 = A[2*i+1];
          p0 = points0[i];
          p1 = points1[i];
          a0[0] = p0.x; a0[1] = p0.y; a0[2] = 1; a0[3] = a0[4] = a0[5] = 0;
          a1[0] = a1[1] = a1[2] = 0; a1[3] = p0.x; a1[4] = p0.y; a1[5] = 1;
          b(2*i,0) = p1.x;
          b(2*i+1,0) = p1.y;
      }
  
      Mat_<float> sol;
      solve(A, b, sol, DECOMP_NORMAL | DECOMP_LU);
  
      if (rmse)
          *rmse = static_cast<float>(norm(A*sol, b, NORM_L2) / std::sqrt(static_cast<double>(npoints)));
  
      Mat_<float> M = Mat::eye(3, 3, CV_32F);
      for (int i = 0, k = 0; i < 2; ++i)
          for (int j = 0; j < 3; ++j, ++k)
              M(i,j) = sol(k,0);
  
      return T1.inv() * M * T0;
  }
  
  
  Mat estimateGlobalMotionLeastSquares(
          InputOutputArray points0, InputOutputArray points1, int model, float *rmse)
  {
      CV_Assert(model <= MM_AFFINE);
      CV_Assert(points0.type() == points1.type());
      const int npoints = points0.getMat().checkVector(2);
      CV_Assert(points1.getMat().checkVector(2) == npoints);
  
      typedef Mat (*Impl)(int, Point2f*, Point2f*, float*);
      static Impl impls[] = { estimateGlobMotionLeastSquaresTranslation,
                              estimateGlobMotionLeastSquaresTranslationAndScale,
                              estimateGlobMotionLeastSquaresRotation,
                              estimateGlobMotionLeastSquaresRigid,
                              estimateGlobMotionLeastSquaresSimilarity,
                              estimateGlobMotionLeastSquaresAffine };
  
      Point2f *points0_ = points0.getMat().ptr<Point2f>();
      Point2f *points1_ = points1.getMat().ptr<Point2f>();
  
      return impls[model](npoints, points0_, points1_, rmse);
  }
  
  
  Mat estimateGlobalMotionRansac(
          InputArray points0, InputArray points1, int model, const RansacParams &params,
          float *rmse, int *ninliers)
  {
      CV_Assert(model <= MM_AFFINE);
      CV_Assert(points0.type() == points1.type());
      const int npoints = points0.getMat().checkVector(2);
      CV_Assert(points1.getMat().checkVector(2) == npoints);
  
      if (npoints < params.size)
          return Mat::eye(3, 3, CV_32F);
  
      const Point2f *points0_ = points0.getMat().ptr<Point2f>();
      const Point2f *points1_ = points1.getMat().ptr<Point2f>();
      const int niters = params.niters();
  
      // current hypothesis
      std::vector<int> indices(params.size);
      std::vector<Point2f> subset0(params.size);
      std::vector<Point2f> subset1(params.size);
  
      // best hypothesis
      std::vector<Point2f> subset0best(params.size);
      std::vector<Point2f> subset1best(params.size);
      Mat_<float> bestM;
      int ninliersMax = -1;
  
      RNG rng(0);
      Point2f p0, p1;
      float x, y;
  
      for (int iter = 0; iter < niters; ++iter)
      {
          for (int i = 0; i < params.size; ++i)
          {
              bool ok = false;
              while (!ok)
              {
                  ok = true;
                  indices[i] = static_cast<unsigned>(rng) % npoints;
                  for (int j = 0; j < i; ++j)
                      if (indices[i] == indices[j])
                          { ok = false; break; }
              }
          }
          for (int i = 0; i < params.size; ++i)
          {
              subset0[i] = points0_[indices[i]];
              subset1[i] = points1_[indices[i]];
          }
  
          Mat_<float> M = estimateGlobalMotionLeastSquares(subset0, subset1, model, 0);
  
          int numinliers = 0;
          for (int i = 0; i < npoints; ++i)
          {
              p0 = points0_[i];
              p1 = points1_[i];
              x = M(0,0)*p0.x + M(0,1)*p0.y + M(0,2);
              y = M(1,0)*p0.x + M(1,1)*p0.y + M(1,2);
              if (sqr(x - p1.x) + sqr(y - p1.y) < params.thresh * params.thresh)
                  numinliers++;
          }
          if (numinliers >= ninliersMax)
          {
              bestM = M;
              ninliersMax = numinliers;
              subset0best.swap(subset0);
              subset1best.swap(subset1);
          }
      }
  
      if (ninliersMax < params.size)
          // compute RMSE
          bestM = estimateGlobalMotionLeastSquares(subset0best, subset1best, model, rmse);
      else
      {
          subset0.resize(ninliersMax);
          subset1.resize(ninliersMax);
          for (int i = 0, j = 0; i < npoints && j < ninliersMax ; ++i)
          {
              p0 = points0_[i];
              p1 = points1_[i];
              x = bestM(0,0)*p0.x + bestM(0,1)*p0.y + bestM(0,2);
              y = bestM(1,0)*p0.x + bestM(1,1)*p0.y + bestM(1,2);
              if (sqr(x - p1.x) + sqr(y - p1.y) < params.thresh * params.thresh)
              {
                  subset0[j] = p0;
                  subset1[j] = p1;
                  j++;
              }
          }
          bestM = estimateGlobalMotionLeastSquares(subset0, subset1, model, rmse);
      }
  
      if (ninliers)
          *ninliers = ninliersMax;
  
      return bestM;
  }
  
  
  MotionEstimatorRansacL2::MotionEstimatorRansacL2(MotionModel model)
      : MotionEstimatorBase(model)
  {
      setRansacParams(RansacParams::default2dMotion(model));
      setMinInlierRatio(0.1f);
  }
  
  
  Mat MotionEstimatorRansacL2::estimate(InputArray points0, InputArray points1, bool *ok)
  {
      CV_Assert(points0.type() == points1.type());
      const int npoints = points0.getMat().checkVector(2);
      CV_Assert(points1.getMat().checkVector(2) == npoints);
  
      // find motion
  
      int ninliers = 0;
      Mat_<float> M;
  
      if (motionModel() != MM_HOMOGRAPHY)
          M = estimateGlobalMotionRansac(
                  points0, points1, motionModel(), ransacParams_, 0, &ninliers);
      else
      {
          std::vector<uchar> mask;
          M = findHomography(points0, points1, mask, LMEDS);
          for (int i  = 0; i < npoints; ++i)
              if (mask[i]) ninliers++;
      }
  
      // check if we're confident enough in estimated motion
  
      if (ok) *ok = true;
      if (static_cast<float>(ninliers) / npoints < minInlierRatio_)
      {
          M = Mat::eye(3, 3, CV_32F);
          if (ok) *ok = false;
      }
  
      return M;
  }
  
  
  MotionEstimatorL1::MotionEstimatorL1(MotionModel model)
      : MotionEstimatorBase(model)
  {
  }
  
  
  // TODO will estimation of all motions as one LP problem be faster?
  Mat MotionEstimatorL1::estimate(InputArray points0, InputArray points1, bool *ok)
  {
      CV_Assert(points0.type() == points1.type());
      const int npoints = points0.getMat().checkVector(2);
      CV_Assert(points1.getMat().checkVector(2) == npoints);
  
  #ifndef HAVE_CLP
  
      CV_Error(Error::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);
  
      if(npoints <= 0)
          return Mat::eye(3, 3, CV_32F);
  
      // prepare LP problem
  
      const Point2f *points0_ = points0.getMat().ptr<Point2f>();
      const Point2f *points1_ = points1.getMat().ptr<Point2f>();
  
      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 = points0_[i];
          p1 = points1_[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
  }
  
  
  FromFileMotionReader::FromFileMotionReader(const String &path)
      : ImageMotionEstimatorBase(MM_UNKNOWN)
  {
      file_.open(path.c_str());
      CV_Assert(file_.is_open());
  }
  
  
  Mat FromFileMotionReader::estimate(const Mat &/*frame0*/, const Mat &/*frame1*/, bool *ok)
  {
      Mat_<float> M(3, 3);
      bool ok_;
      file_ >> 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) >> ok_;
      if (ok) *ok = ok_;
      return M;
  }
  
  
  ToFileMotionWriter::ToFileMotionWriter(const String &path, Ptr<ImageMotionEstimatorBase> estimator)
      : ImageMotionEstimatorBase(estimator->motionModel()), motionEstimator_(estimator)
  {
      file_.open(path.c_str());
      CV_Assert(file_.is_open());
  }
  
  
  Mat ToFileMotionWriter::estimate(const Mat &frame0, const Mat &frame1, bool *ok)
  {
      bool ok_;
      Mat_<float> M = motionEstimator_->estimate(frame0, frame1, &ok_);
      file_ << 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) << " " << ok_ << std::endl;
      if (ok) *ok = ok_;
      return M;
  }
  
  
  KeypointBasedMotionEstimator::KeypointBasedMotionEstimator(Ptr<MotionEstimatorBase> estimator)
      : ImageMotionEstimatorBase(estimator->motionModel()), motionEstimator_(estimator)
  {
      setDetector(GFTTDetector::create());
      setOpticalFlowEstimator(makePtr<SparsePyrLkOptFlowEstimator>());
      setOutlierRejector(makePtr<NullOutlierRejector>());
  }
  
  
  Mat KeypointBasedMotionEstimator::estimate(const Mat &frame0, const Mat &frame1, bool *ok)
  {
      // find keypoints
      detector_->detect(frame0, keypointsPrev_);
      if (keypointsPrev_.empty())
          return Mat::eye(3, 3, CV_32F);
  
      // 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]);
          }
      }
  
      // perform outlier rejection
  
      IOutlierRejector *outlRejector = outlierRejector_.get();
      if (!dynamic_cast<NullOutlierRejector*>(outlRejector))
      {
          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]);
              }
          }
      }
  
      // estimate motion
      return motionEstimator_->estimate(pointsPrevGood_, pointsGood_, ok);
  }
  
  #if defined(HAVE_OPENCV_CUDAIMGPROC) && defined(HAVE_OPENCV_CUDAOPTFLOW)
  
  KeypointBasedMotionEstimatorGpu::KeypointBasedMotionEstimatorGpu(Ptr<MotionEstimatorBase> estimator)
      : ImageMotionEstimatorBase(estimator->motionModel()), motionEstimator_(estimator)
  {
      detector_ = cuda::createGoodFeaturesToTrackDetector(CV_8UC1);
  
      CV_Assert(cuda::getCudaEnabledDeviceCount() > 0);
      setOutlierRejector(makePtr<NullOutlierRejector>());
  }
  
  
  Mat KeypointBasedMotionEstimatorGpu::estimate(const Mat &frame0, const Mat &frame1, bool *ok)
  {
      frame0_.upload(frame0);
      frame1_.upload(frame1);
      return estimate(frame0_, frame1_, ok);
  }
  
  
  Mat KeypointBasedMotionEstimatorGpu::estimate(const cuda::GpuMat &frame0, const cuda::GpuMat &frame1, bool *ok)
  {
      // convert frame to gray if it's color
  
      cuda::GpuMat grayFrame0;
      if (frame0.channels() == 1)
          grayFrame0 = frame0;
      else
      {
          cuda::cvtColor(frame0, grayFrame0_, COLOR_BGR2GRAY);
          grayFrame0 = grayFrame0_;
      }
  
      // find keypoints
      detector_->detect(grayFrame0, pointsPrev_);
  
      // find correspondences
      optFlowEstimator_.run(frame0, frame1, pointsPrev_, points_, status_);
  
      // leave good correspondences only
      cuda::compactPoints(pointsPrev_, points_, status_);
  
      pointsPrev_.download(hostPointsPrev_);
      points_.download(hostPoints_);
  
      // perform outlier rejection
  
      IOutlierRejector *rejector = outlierRejector_.get();
      if (!dynamic_cast<NullOutlierRejector*>(rejector))
      {
          outlierRejector_->process(frame0.size(), hostPointsPrev_, hostPoints_, rejectionStatus_);
  
          hostPointsPrevTmp_.clear();
          hostPointsPrevTmp_.reserve(hostPoints_.cols);
  
          hostPointsTmp_.clear();
          hostPointsTmp_.reserve(hostPoints_.cols);
  
          for (int i = 0; i < hostPoints_.cols; ++i)
          {
              if (rejectionStatus_[i])
              {
                  hostPointsPrevTmp_.push_back(hostPointsPrev_.at<Point2f>(0,i));
                  hostPointsTmp_.push_back(hostPoints_.at<Point2f>(0,i));
              }
          }
  
          hostPointsPrev_ = Mat(1, (int)hostPointsPrevTmp_.size(), CV_32FC2, &hostPointsPrevTmp_[0]);
          hostPoints_ = Mat(1, (int)hostPointsTmp_.size(), CV_32FC2, &hostPointsTmp_[0]);
      }
  
      // estimate motion
      return motionEstimator_->estimate(hostPointsPrev_, hostPoints_, ok);
  }
  
  #endif // defined(HAVE_OPENCV_CUDAIMGPROC) && defined(HAVE_OPENCV_CUDAOPTFLOW)
  
  
  Mat getMotion(int from, int to, const std::vector<Mat> &motions)
  {
      Mat M = Mat::eye(3, 3, CV_32F);
      if (to > from)
      {
          for (int i = from; i < to; ++i)
              M = at(i, motions) * M;
      }
      else if (from > to)
      {
          for (int i = to; i < from; ++i)
              M = at(i, motions) * M;
          M = M.inv();
      }
      return M;
  }
  
  } // namespace videostab
  } // namespace cv
  