modules/videostab/src/global_motion.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14 // Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved.
  15 // Third party copyrights are property of their respective owners.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,
  24 //     this list of conditions and the following disclaimer in the documentation
  25 //     and/or other materials provided with the distribution.
  26 //
  27 //   * The name of the copyright holders may not be used to endorse or promote products
  28 //     derived from this software without specific prior written permission.
  29 //
  30 // This software is provided by the copyright holders and contributors "as is" and
  31 // any express or implied warranties, including, but not limited to, the implied
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,
  34 // indirect, incidental, special, exemplary, or consequential damages
  35 // (including, but not limited to, procurement of substitute goods or services;
  36 // loss of use, data, or profits; or business interruption) however caused
  37 // and on any theory of liability, whether in contract, strict liability,
  38 // or tort (including negligence or otherwise) arising in any way out of
  39 // the use of this software, even if advised of the possibility of such damage.
  40 //
  41 //M*/
  42
  43 #include "precomp.hpp"
  44 #include "opencv2/highgui/highgui.hpp"
  45 #include "opencv2/videostab/global_motion.hpp"
  46
  47 using namespace std;
  48
  49 namespace cv
  50 {
  51 namespace videostab
  52 {
  53
  54 static Mat estimateGlobMotionLeastSquaresTranslation(
  55         int npoints, const Point2f *points0, const Point2f *points1, float *rmse)
  56 {
  57     Mat_<float> M = Mat::eye(3, 3, CV_32F);
  58     for (int i = 0; i < npoints; ++i)
  59     {
  60         M(0,2) += points1[i].x - points0[i].x;
  61         M(1,2) += points1[i].y - points0[i].y;
  62     }
  63     M(0,2) /= npoints;
  64     M(1,2) /= npoints;
  65     if (rmse)
  66     {
  67         *rmse = 0;
  68         for (int i = 0; i < npoints; ++i)
  69             *rmse += sqr(points1[i].x - points0[i].x - M(0,2)) +
  70                      sqr(points1[i].y - points0[i].y - M(1,2));
  71         *rmse = sqrt(*rmse / npoints);
  72     }
  73     return M;
  74 }
  75
  76
  77 static Mat estimateGlobMotionLeastSquaresTranslationAndScale(
  78         int npoints, const Point2f *points0, const Point2f *points1, float *rmse)
  79 {
  80     Mat_<float> A(2*npoints, 3), b(2*npoints, 1);
  81     float *a0, *a1;
  82     Point2f p0, p1;
  83
  84     for (int i = 0; i < npoints; ++i)
  85     {
  86         a0 = A[2*i];
  87         a1 = A[2*i+1];
  88         p0 = points0[i];
  89         p1 = points1[i];
  90         a0[0] = p0.x; a0[1] = 1; a0[2] = 0;
  91         a1[0] = p0.y; a1[1] = 0; a1[2] = 1;
  92         b(2*i,0) = p1.x;
  93         b(2*i+1,0) = p1.y;
  94     }
  95
  96     Mat_<float> sol;
  97     solve(A, b, sol, DECOMP_SVD);
  98
  99     if (rmse)
 100         *rmse = norm(A*sol, b, NORM_L2) / sqrt(static_cast<float>(npoints));
 101
 102     Mat_<float> M = Mat::eye(3, 3, CV_32F);
 103     M(0,0) = M(1,1) = sol(0,0);
 104     M(0,2) = sol(1,0);
 105     M(1,2) = sol(2,0);
 106     return M;
 107 }
 108
 109
 110 static Mat estimateGlobMotionLeastSquaresAffine(
 111         int npoints, const Point2f *points0, const Point2f *points1, float *rmse)
 112 {
 113     Mat_<float> A(2*npoints, 6), b(2*npoints, 1);
 114     float *a0, *a1;
 115     Point2f p0, p1;
 116
 117     for (int i = 0; i < npoints; ++i)
 118     {
 119         a0 = A[2*i];
 120         a1 = A[2*i+1];
 121         p0 = points0[i];
 122         p1 = points1[i];
 123         a0[0] = p0.x; a0[1] = p0.y; a0[2] = 1; a0[3] = a0[4] = a0[5] = 0;
 124         a1[0] = a1[1] = a1[2] = 0; a1[3] = p0.x; a1[4] = p0.y; a1[5] = 1;
 125         b(2*i,0) = p1.x;
 126         b(2*i+1,0) = p1.y;
 127     }
 128
 129     Mat_<float> sol;
 130     solve(A, b, sol, DECOMP_SVD);
 131
 132     if (rmse)
 133         *rmse = norm(A*sol, b, NORM_L2) / sqrt(static_cast<float>(npoints));
 134
 135     Mat_<float> M = Mat::eye(3, 3, CV_32F);
 136     for (int i = 0, k = 0; i < 2; ++i)
 137         for (int j = 0; j < 3; ++j, ++k)
 138             M(i,j) = sol(k,0);
 139
 140     return M;
 141 }
 142
 143
 144 Mat estimateGlobalMotionLeastSquares(
 145         const vector<Point2f> &points0, const vector<Point2f> &points1, int model, float *rmse)
 146 {
 147     CV_Assert(points0.size() == points1.size());
 148
 149     typedef Mat (*Impl)(int, const Point2f*, const Point2f*, float*);
 150     static Impl impls[] = { estimateGlobMotionLeastSquaresTranslation,
 151                             estimateGlobMotionLeastSquaresTranslationAndScale,
 152                             estimateGlobMotionLeastSquaresAffine };
 153
 154     int npoints = static_cast<int>(points0.size());
 155     return impls[model](npoints, &points0[0], &points1[0], rmse);
 156 }
 157
 158
 159 Mat estimateGlobalMotionRobust(
 160         const vector<Point2f> &points0, const vector<Point2f> &points1, int model, const RansacParams &params,
 161         float *rmse, int *ninliers)
 162 {
 163     CV_Assert(points0.size() == points1.size());
 164
 165     typedef Mat (*Impl)(int, const Point2f*, const Point2f*, float*);
 166     static Impl impls[] = { estimateGlobMotionLeastSquaresTranslation,
 167                             estimateGlobMotionLeastSquaresTranslationAndScale,
 168                             estimateGlobMotionLeastSquaresAffine };
 169
 170     const int npoints = static_cast<int>(points0.size());
 171     const int niters = static_cast<int>(ceil(log(1 - params.prob) / log(1 - pow(1 - params.eps, params.size))));
 172
 173     RNG rng(0);
 174     vector<int> indices(params.size);
 175     vector<Point2f> subset0(params.size), subset1(params.size);
 176     vector<Point2f> subset0best(params.size), subset1best(params.size);
 177     Mat_<float> bestM;
 178     int ninliersMax = -1;
 179     Point2f p0, p1;
 180     float x, y;
 181
 182     for (int iter = 0; iter < niters; ++iter)
 183     {
 184         for (int i = 0; i < params.size; ++i)
 185         {
 186             bool ok = false;
 187             while (!ok)
 188             {
 189                 ok = true;
 190                 indices[i] = static_cast<unsigned>(rng) % npoints;
 191                 for (int j = 0; j < i; ++j)
 192                     if (indices[i] == indices[j])
 193                         { ok = false; break; }
 194             }
 195         }
 196         for (int i = 0; i < params.size; ++i)
 197         {
 198             subset0[i] = points0[indices[i]];
 199             subset1[i] = points1[indices[i]];
 200         }
 201
 202         Mat_<float> M = impls[model](params.size, &subset0[0], &subset1[0], 0);
 203
 204         int ninliers = 0;
 205         for (int i = 0; i < npoints; ++i)
 206         {
 207             p0 = points0[i]; p1 = points1[i];
 208             x = M(0,0)*p0.x + M(0,1)*p0.y + M(0,2);
 209             y = M(1,0)*p0.x + M(1,1)*p0.y + M(1,2);
 210             if (sqr(x - p1.x) + sqr(y - p1.y) < params.thresh * params.thresh)
 211                 ninliers++;
 212         }
 213         if (ninliers >= ninliersMax)
 214         {
 215             bestM = M;
 216             ninliersMax = ninliers;
 217             subset0best.swap(subset0);
 218             subset1best.swap(subset1);
 219         }
 220     }
 221
 222     if (ninliersMax < params.size)
 223         // compute rmse
 224         bestM = impls[model](params.size, &subset0best[0], &subset1best[0], rmse);
 225     else
 226     {
 227         subset0.resize(ninliersMax);
 228         subset1.resize(ninliersMax);
 229         for (int i = 0, j = 0; i < npoints; ++i)
 230         {
 231             p0 = points0[i]; p1 = points1[i];
 232             x = bestM(0,0)*p0.x + bestM(0,1)*p0.y + bestM(0,2);
 233             y = bestM(1,0)*p0.x + bestM(1,1)*p0.y + bestM(1,2);
 234             if (sqr(x - p1.x) + sqr(y - p1.y) < params.thresh * params.thresh)
 235             {
 236                 subset0[j] = p0;
 237                 subset1[j] = p1;
 238                 j++;
 239             }
 240         }
 241         bestM = impls[model](ninliersMax, &subset0[0], &subset1[0], rmse);
 242     }
 243
 244     if (ninliers)
 245         *ninliers = ninliersMax;
 246
 247     return bestM;
 248 }
 249
 250
 251 PyrLkRobustMotionEstimator::PyrLkRobustMotionEstimator() : ransacParams_(RansacParams::affine2dMotionStd())
 252 {
 253     setDetector(new GoodFeaturesToTrackDetector());
 254     setOptFlowEstimator(new SparsePyrLkOptFlowEstimator());
 255     setMotionModel(AFFINE);
 256     setMaxRmse(0.5f);
 257     setMinInlierRatio(0.1f);
 258 }
 259
 260
 261 Mat PyrLkRobustMotionEstimator::estimate(const Mat &frame0, const Mat &frame1)
 262 {
 263     detector_->detect(frame0, keypointsPrev_);
 264
 265     pointsPrev_.resize(keypointsPrev_.size());
 266     for (size_t i = 0; i < keypointsPrev_.size(); ++i)
 267         pointsPrev_[i] = keypointsPrev_[i].pt;
 268
 269     optFlowEstimator_->run(frame0, frame1, pointsPrev_, points_, status_, noArray());
 270
 271     size_t npoints = points_.size();
 272     pointsPrevGood_.clear();
 273     pointsPrevGood_.reserve(npoints);
 274     pointsGood_.clear();
 275     pointsGood_.reserve(npoints);
 276     for (size_t i = 0; i < npoints; ++i)
 277     {
 278         if (status_[i])
 279         {
 280             pointsPrevGood_.push_back(pointsPrev_[i]);
 281             pointsGood_.push_back(points_[i]);
 282         }
 283     }
 284
 285     float rmse;
 286     int ninliers;
 287     Mat M = estimateGlobalMotionRobust(
 288             pointsPrevGood_, pointsGood_, motionModel_, ransacParams_, &rmse, &ninliers);
 289
 290     if (rmse > maxRmse_ || static_cast<float>(ninliers) / pointsGood_.size() < minInlierRatio_)
 291         M = Mat::eye(3, 3, CV_32F);
 292
 293     return M;
 294 }
 295
 296
 297 Mat getMotion(int from, int to, const vector<Mat> &motions)
 298 {
 299     Mat M = Mat::eye(3, 3, CV_32F);
 300     if (to > from)
 301     {
 302         for (int i = from; i < to; ++i)
 303             M = at(i, motions) * M;
 304     }
 305     else if (from > to)
 306     {
 307         for (int i = to; i < from; ++i)
 308             M = at(i, motions) * M;
 309         M = M.inv();
 310     }
 311     return M;
 312 }
 313
 314
 315 static inline int areaSign(Point2f a, Point2f b, Point2f c)
 316 {
 317     double area = (b-a).cross(c-a);
 318     if (area < -1e-5) return -1;
 319     if (area > 1e-5) return 1;
 320     return 0;
 321 }
 322
 323
 324 static inline bool segmentsIntersect(Point2f a, Point2f b, Point2f c, Point2f d)
 325 {
 326     return areaSign(a,b,c) * areaSign(a,b,d) < 0 &&
 327            areaSign(c,d,a) * areaSign(c,d,b) < 0;
 328 }
 329
 330
 331 // Checks if rect a (with sides parallel to axis) is inside rect b (arbitrary).
 332 // Rects must be passed in the [(0,0), (w,0), (w,h), (0,h)] order.
 333 static inline bool isRectInside(const Point2f a[4], const Point2f b[4])
 334 {
 335     for (int i = 0; i < 4; ++i)
 336         if (b[i].x > a[0].x && b[i].x < a[2].x && b[i].y > a[0].y && b[i].y < a[2].y)
 337             return false;
 338     for (int i = 0; i < 4; ++i)
 339     for (int j = 0; j < 4; ++j)
 340         if (segmentsIntersect(a[i], a[(i+1)%4], b[j], b[(j+1)%4]))
 341             return false;
 342     return true;
 343 }
 344
 345
 346 static inline bool isGoodMotion(const float M[], float w, float h, float dx, float dy)
 347 {
 348     Point2f pt[4] = {Point2f(0,0), Point2f(w,0), Point2f(w,h), Point2f(0,h)};
 349     Point2f Mpt[4];
 350
 351     for (int i = 0; i < 4; ++i)
 352     {
 353         Mpt[i].x = M[0]*pt[i].x + M[1]*pt[i].y + M[2];
 354         Mpt[i].y = M[3]*pt[i].x + M[4]*pt[i].y + M[5];
 355     }
 356
 357     pt[0] = Point2f(dx, dy);
 358     pt[1] = Point2f(w - dx, dy);
 359     pt[2] = Point2f(w - dx, h - dy);
 360     pt[3] = Point2f(dx, h - dy);
 361
 362     return isRectInside(pt, Mpt);
 363 }
 364
 365
 366 static inline void relaxMotion(const float M[], float t, float res[])
 367 {
 368     res[0] = M[0]*(1.f-t) + t;
 369     res[1] = M[1]*(1.f-t);
 370     res[2] = M[2]*(1.f-t);
 371     res[3] = M[3]*(1.f-t);
 372     res[4] = M[4]*(1.f-t) + t;
 373     res[5] = M[5]*(1.f-t);
 374 }
 375
 376
 377 Mat ensureInclusionConstraint(const Mat &M, Size size, float trimRatio)
 378 {
 379     CV_Assert(M.size() == Size(3,3) && M.type() == CV_32F);
 380
 381     const float w = static_cast<float>(size.width);
 382     const float h = static_cast<float>(size.height);
 383     const float dx = floor(w * trimRatio);
 384     const float dy = floor(h * trimRatio);
 385     const float srcM[6] =
 386             {M.at<float>(0,0), M.at<float>(0,1), M.at<float>(0,2),
 387              M.at<float>(1,0), M.at<float>(1,1), M.at<float>(1,2)};
 388
 389     float curM[6];
 390     float t = 0;
 391     relaxMotion(srcM, t, curM);
 392     if (isGoodMotion(curM, w, h, dx, dy))
 393         return M;
 394
 395     float l = 0, r = 1;
 396     while (r - l > 1e-3f)
 397     {
 398         t = (l + r) * 0.5f;
 399         relaxMotion(srcM, t, curM);
 400         if (isGoodMotion(curM, w, h, dx, dy))
 401             r = t;
 402         else
 403             l = t;
 404         t = r;
 405         relaxMotion(srcM, r, curM);
 406     }
 407
 408     return (1 - r) * M + r * Mat::eye(3, 3, CV_32F);
 409 }
 410
 411
 412 // TODO can be estimated for O(1) time
 413 float estimateOptimalTrimRatio(const Mat &M, Size size)
 414 {
 415     CV_Assert(M.size() == Size(3,3) && M.type() == CV_32F);
 416
 417     const float w = static_cast<float>(size.width);
 418     const float h = static_cast<float>(size.height);
 419     Mat_<float> M_(M);
 420
 421     Point2f pt[4] = {Point2f(0,0), Point2f(w,0), Point2f(w,h), Point2f(0,h)};
 422     Point2f Mpt[4];
 423
 424     for (int i = 0; i < 4; ++i)
 425     {
 426         Mpt[i].x = M_(0,0)*pt[i].x + M_(0,1)*pt[i].y + M_(0,2);
 427         Mpt[i].y = M_(1,0)*pt[i].x + M_(1,1)*pt[i].y + M_(1,2);
 428     }
 429
 430     float l = 0, r = 0.5f;
 431     while (r - l > 1e-3f)
 432     {
 433         float t = (l + r) * 0.5f;
 434         float dx = floor(w * t);
 435         float dy = floor(h * t);
 436         pt[0] = Point2f(dx, dy);
 437         pt[1] = Point2f(w - dx, dy);
 438         pt[2] = Point2f(w - dx, h - dy);
 439         pt[3] = Point2f(dx, h - dy);
 440         if (isRectInside(pt, Mpt))
 441             r = t;
 442         else
 443             l = t;
 444     }
 445
 446     return r;
 447 }
 448
 449
 450 float alignementError(const Mat &M, const Mat &frame0, const Mat &mask0, const Mat &frame1)
 451 {
 452     CV_Assert(frame0.type() == CV_8UC3 && frame1.type() == CV_8UC3);
 453     CV_Assert(mask0.type() == CV_8U && mask0.size() == frame0.size());
 454     CV_Assert(frame0.size() == frame1.size());
 455     CV_Assert(M.size() == Size(3,3) && M.type() == CV_32F);
 456
 457     Mat_<uchar> mask0_(mask0);
 458     Mat_<float> M_(M);
 459     float err = 0;
 460
 461     for (int y0 = 0; y0 < frame0.rows; ++y0)
 462     {
 463         for (int x0 = 0; x0 < frame0.cols; ++x0)
 464         {
 465             if (mask0_(y0,x0))
 466             {
 467                 int x1 = cvRound(M_(0,0)*x0 + M_(0,1)*y0 + M_(0,2));
 468                 int y1 = cvRound(M_(1,0)*x0 + M_(1,1)*y0 + M_(1,2));
 469                 if (y1 >= 0 && y1 < frame1.rows && x1 >= 0 && x1 < frame1.cols)
 470                     err += std::abs(intensity(frame1.at<Point3_<uchar> >(y1,x1)) -
 471                                     intensity(frame0.at<Point3_<uchar> >(y0,x0)));
 472             }
 473         }
 474     }
 475
 476     return err;
 477 }
 478
 479 } // namespace videostab
 480 } // namespace cv