global_motion.cpp 11.2 KB
Newer Older
wester committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43
/*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"
wester committed
44
#include "opencv2/highgui/highgui.hpp"
wester committed
45 46
#include "opencv2/videostab/global_motion.hpp"

wester committed
47
using namespace std;
wester committed
48 49 50 51 52 53 54

namespace cv
{
namespace videostab
{

static Mat estimateGlobMotionLeastSquaresTranslation(
wester committed
55
        int npoints, const Point2f *points0, const Point2f *points1, float *rmse)
wester committed
56 57 58 59 60 61 62 63 64 65 66 67 68 69 70
{
    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));
wester committed
71
        *rmse = sqrt(*rmse / npoints);
wester committed
72 73 74 75 76 77
    }
    return M;
}


static Mat estimateGlobMotionLeastSquaresTranslationAndScale(
wester committed
78
        int npoints, const Point2f *points0, const Point2f *points1, float *rmse)
wester committed
79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96
{
    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;
wester committed
97
    solve(A, b, sol, DECOMP_SVD);
wester committed
98 99

    if (rmse)
wester committed
100
        *rmse = static_cast<float>(norm(A*sol, b, NORM_L2) / sqrt(static_cast<double>(npoints)));
wester committed
101 102 103 104 105 106 107 108 109

    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 M;
}


wester committed
110 111
static Mat estimateGlobMotionLeastSquaresLinearSimilarity(
        int npoints, const Point2f *points0, const Point2f *points1, float *rmse)
wester committed
112 113 114 115 116 117 118 119 120 121 122
{
    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];
wester committed
123
        a0[0] = p0.x; a0[1] = p0.y; a0[2] = 1;  a0[3] = 0;
wester committed
124 125 126 127 128 129
        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;
wester committed
130
    solve(A, b, sol, DECOMP_SVD);
wester committed
131 132

    if (rmse)
wester committed
133
        *rmse = static_cast<float>(norm(A*sol, b, NORM_L2) / sqrt(static_cast<double>(npoints)));
wester committed
134 135 136 137 138 139 140

    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);
wester committed
141
    return M;
wester committed
142 143 144 145
}


static Mat estimateGlobMotionLeastSquaresAffine(
wester committed
146
        int npoints, const Point2f *points0, const Point2f *points1, float *rmse)
wester committed
147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164
{
    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;
wester committed
165
    solve(A, b, sol, DECOMP_SVD);
wester committed
166 167

    if (rmse)
wester committed
168
        *rmse = static_cast<float>(norm(A*sol, b, NORM_L2) / sqrt(static_cast<double>(npoints)));
wester committed
169 170 171 172 173 174

    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);

wester committed
175
    return M;
wester committed
176 177 178 179
}


Mat estimateGlobalMotionLeastSquares(
wester committed
180
        const vector<Point2f> &points0, const vector<Point2f> &points1, int model, float *rmse)
wester committed
181
{
wester committed
182
    CV_Assert(points0.size() == points1.size());
wester committed
183

wester committed
184
    typedef Mat (*Impl)(int, const Point2f*, const Point2f*, float*);
wester committed
185 186
    static Impl impls[] = { estimateGlobMotionLeastSquaresTranslation,
                            estimateGlobMotionLeastSquaresTranslationAndScale,
wester committed
187
                            estimateGlobMotionLeastSquaresLinearSimilarity,
wester committed
188 189
                            estimateGlobMotionLeastSquaresAffine };

wester committed
190 191
    int npoints = static_cast<int>(points0.size());
    return impls[model](npoints, &points0[0], &points1[0], rmse);
wester committed
192 193 194
}


wester committed
195 196 197
Mat estimateGlobalMotionRobust(
        const vector<Point2f> &points0, const vector<Point2f> &points1, int model,
        const RansacParams &params, float *rmse, int *ninliers)
wester committed
198
{
wester committed
199 200 201 202 203 204 205
    CV_Assert(points0.size() == points1.size());

    typedef Mat (*Impl)(int, const Point2f*, const Point2f*, float*);
    static Impl impls[] = { estimateGlobMotionLeastSquaresTranslation,
                            estimateGlobMotionLeastSquaresTranslationAndScale,
                            estimateGlobMotionLeastSquaresLinearSimilarity,
                            estimateGlobMotionLeastSquaresAffine };
wester committed
206

wester committed
207
    const int npoints = static_cast<int>(points0.size());
wester committed
208 209 210
    if (npoints < params.size)
        return Mat::eye(3, 3, CV_32F);

wester committed
211 212
    const int niters = static_cast<int>(ceil(log(1 - params.prob) /
                                             log(1 - pow(1 - params.eps, params.size))));
wester committed
213

wester committed
214 215 216 217
    RNG rng(0);
    vector<int> indices(params.size);
    vector<Point2f> subset0(params.size), subset1(params.size);
    vector<Point2f> subset0best(params.size), subset1best(params.size);
wester committed
218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238
    Mat_<float> bestM;
    int ninliersMax = -1;
    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)
        {
wester committed
239 240
            subset0[i] = points0[indices[i]];
            subset1[i] = points1[indices[i]];
wester committed
241 242
        }

wester committed
243
        Mat_<float> M = impls[model](params.size, &subset0[0], &subset1[0], 0);
wester committed
244

wester committed
245
        int _ninliers = 0;
wester committed
246 247
        for (int i = 0; i < npoints; ++i)
        {
wester committed
248
            p0 = points0[i]; p1 = points1[i];
wester committed
249 250 251
            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)
wester committed
252
                _ninliers++;
wester committed
253
        }
wester committed
254
        if (_ninliers >= ninliersMax)
wester committed
255 256
        {
            bestM = M;
wester committed
257
            ninliersMax = _ninliers;
a  
Kai Westerkamp committed
258 259
            subset0best.swap(subset0);
            subset1best.swap(subset1);
wester committed
260 261 262 263
        }
    }

    if (ninliersMax < params.size)
wester committed
264 265
        // compute rmse
        bestM = impls[model](params.size, &subset0best[0], &subset1best[0], rmse);
wester committed
266 267 268 269
    else
    {
        subset0.resize(ninliersMax);
        subset1.resize(ninliersMax);
wester committed
270
        for (int i = 0, j = 0; i < npoints; ++i)
wester committed
271
        {
wester committed
272
            p0 = points0[i]; p1 = points1[i];
wester committed
273 274 275 276 277 278 279 280 281
            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++;
            }
        }
wester committed
282
        bestM = impls[model](ninliersMax, &subset0[0], &subset1[0], rmse);
wester committed
283 284 285 286 287 288 289 290 291
    }

    if (ninliers)
        *ninliers = ninliersMax;

    return bestM;
}


wester committed
292 293
PyrLkRobustMotionEstimator::PyrLkRobustMotionEstimator()
    : ransacParams_(RansacParams::affine2dMotionStd())
wester committed
294
{
wester committed
295 296 297 298
    setDetector(new GoodFeaturesToTrackDetector());
    setOptFlowEstimator(new SparsePyrLkOptFlowEstimator());
    setMotionModel(AFFINE);
    setMaxRmse(0.5f);
wester committed
299 300 301 302
    setMinInlierRatio(0.1f);
}


wester committed
303
Mat PyrLkRobustMotionEstimator::estimate(const Mat &frame0, const Mat &frame1)
wester committed
304 305 306 307 308 309 310 311 312 313 314
{
    detector_->detect(frame0, keypointsPrev_);
    if (keypointsPrev_.empty())
        return Mat::eye(3, 3, CV_32F);

    pointsPrev_.resize(keypointsPrev_.size());
    for (size_t i = 0; i < keypointsPrev_.size(); ++i)
        pointsPrev_[i] = keypointsPrev_[i].pt;

    optFlowEstimator_->run(frame0, frame1, pointsPrev_, points_, status_, noArray());

wester committed
315 316 317 318 319 320
    size_t npoints = points_.size();
    pointsPrevGood_.clear();
    pointsPrevGood_.reserve(npoints);
    pointsGood_.clear();
    pointsGood_.reserve(npoints);
    for (size_t i = 0; i < npoints; ++i)
wester committed
321 322 323 324 325 326 327 328
    {
        if (status_[i])
        {
            pointsPrevGood_.push_back(pointsPrev_[i]);
            pointsGood_.push_back(points_[i]);
        }
    }

wester committed
329 330 331 332
    float rmse;
    int ninliers;
    Mat M = estimateGlobalMotionRobust(
            pointsPrevGood_, pointsGood_, motionModel_, ransacParams_, &rmse, &ninliers);
wester committed
333

wester committed
334 335
    if (rmse > maxRmse_ || static_cast<float>(ninliers) / pointsGood_.size() < minInlierRatio_)
        M = Mat::eye(3, 3, CV_32F);
wester committed
336

wester committed
337
    return M;
wester committed
338 339 340
}


wester committed
341
Mat getMotion(int from, int to, const Mat *motions, int size)
wester committed
342 343 344 345 346
{
    Mat M = Mat::eye(3, 3, CV_32F);
    if (to > from)
    {
        for (int i = from; i < to; ++i)
wester committed
347
            M = at(i, motions, size) * M;
wester committed
348 349 350 351
    }
    else if (from > to)
    {
        for (int i = to; i < from; ++i)
wester committed
352
            M = at(i, motions, size) * M;
wester committed
353 354 355 356 357
        M = M.inv();
    }
    return M;
}

wester committed
358 359 360 361 362 363

Mat getMotion(int from, int to, const vector<Mat> &motions)
{
    return getMotion(from, to, &motions[0], (int)motions.size());
}

wester committed
364 365
} // namespace videostab
} // namespace cv