← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/modules/videostab/src/wobble_suppression.cpp

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sw/ext/opencv_bebop/opencv/modules/videostab/src/wobble_suppression.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/videostab/wobble_suppression.hpp"
 
45
#include "opencv2/videostab/ring_buffer.hpp"
 
46
 
 
47
#include "opencv2/core/private.cuda.hpp"
 
48
 
 
49
#ifdef HAVE_OPENCV_CUDAWARPING
 
50
#  include "opencv2/cudawarping.hpp"
 
51
#endif
 
52
 
 
53
#if defined(HAVE_OPENCV_CUDAWARPING)
 
54
    #if !defined HAVE_CUDA || defined(CUDA_DISABLER)
 
55
        namespace cv { namespace cuda {
 
56
            static void calcWobbleSuppressionMaps(int, int, int, Size, const Mat&, const Mat&, GpuMat&, GpuMat&) { throw_no_cuda(); }
 
57
        }}
 
58
    #else
 
59
        namespace cv { namespace cuda { namespace device { namespace globmotion {
 
60
            void calcWobbleSuppressionMaps(
 
61
                    int left, int idx, int right, int width, int height,
 
62
                    const float *ml, const float *mr, PtrStepSzf mapx, PtrStepSzf mapy);
 
63
        }}}}
 
64
        namespace cv { namespace cuda {
 
65
            static void calcWobbleSuppressionMaps(
 
66
                    int left, int idx, int right, Size size, const Mat &ml, const Mat &mr,
 
67
                    GpuMat &mapx, GpuMat &mapy)
 
68
            {
 
69
                CV_Assert(ml.size() == Size(3, 3) && ml.type() == CV_32F && ml.isContinuous());
 
70
                CV_Assert(mr.size() == Size(3, 3) && mr.type() == CV_32F && mr.isContinuous());
 
71
 
 
72
                mapx.create(size, CV_32F);
 
73
                mapy.create(size, CV_32F);
 
74
 
 
75
                cv::cuda::device::globmotion::calcWobbleSuppressionMaps(
 
76
                            left, idx, right, size.width, size.height,
 
77
                            ml.ptr<float>(), mr.ptr<float>(), mapx, mapy);
 
78
            }
 
79
        }}
 
80
    #endif
 
81
#endif
 
82
 
 
83
namespace cv
 
84
{
 
85
namespace videostab
 
86
{
 
87
 
 
88
WobbleSuppressorBase::WobbleSuppressorBase() : motions_(0), stabilizationMotions_(0)
 
89
{
 
90
    setMotionEstimator(makePtr<KeypointBasedMotionEstimator>(makePtr<MotionEstimatorRansacL2>(MM_HOMOGRAPHY)));
 
91
}
 
92
 
 
93
 
 
94
void NullWobbleSuppressor::suppress(int /*idx*/, const Mat &frame, Mat &result)
 
95
{
 
96
    result = frame;
 
97
}
 
98
 
 
99
 
 
100
void MoreAccurateMotionWobbleSuppressor::suppress(int idx, const Mat &frame, Mat &result)
 
101
{
 
102
    CV_Assert(motions_ && stabilizationMotions_);
 
103
 
 
104
    if (idx % period_ == 0)
 
105
    {
 
106
        result = frame;
 
107
        return;
 
108
    }
 
109
 
 
110
    int k1 = idx / period_ * period_;
 
111
    int k2 = std::min(k1 + period_, frameCount_ - 1);
 
112
 
 
113
    Mat S1 = (*stabilizationMotions_)[idx];
 
114
 
 
115
    Mat_<float> ML = S1 * getMotion(k1, idx, *motions2_) * getMotion(k1, idx, *motions_).inv() * S1.inv();
 
116
    Mat_<float> MR = S1 * getMotion(idx, k2, *motions2_).inv() * getMotion(idx, k2, *motions_) * S1.inv();
 
117
 
 
118
    mapx_.create(frame.size());
 
119
    mapy_.create(frame.size());
 
120
 
 
121
    float xl, yl, zl, wl;
 
122
    float xr, yr, zr, wr;
 
123
 
 
124
    for (int y = 0; y < frame.rows; ++y)
 
125
    {
 
126
        for (int x = 0; x < frame.cols; ++x)
 
127
        {
 
128
            xl = ML(0,0)*x + ML(0,1)*y + ML(0,2);
 
129
            yl = ML(1,0)*x + ML(1,1)*y + ML(1,2);
 
130
            zl = ML(2,0)*x + ML(2,1)*y + ML(2,2);
 
131
            xl /= zl; yl /= zl;
 
132
            wl = float(idx - k1);
 
133
 
 
134
            xr = MR(0,0)*x + MR(0,1)*y + MR(0,2);
 
135
            yr = MR(1,0)*x + MR(1,1)*y + MR(1,2);
 
136
            zr = MR(2,0)*x + MR(2,1)*y + MR(2,2);
 
137
            xr /= zr; yr /= zr;
 
138
            wr = float(k2 - idx);
 
139
 
 
140
            mapx_(y,x) = (wr * xl + wl * xr) / (wl + wr);
 
141
            mapy_(y,x) = (wr * yl + wl * yr) / (wl + wr);
 
142
        }
 
143
    }
 
144
 
 
145
    if (result.data == frame.data)
 
146
        result = Mat(frame.size(), frame.type());
 
147
 
 
148
    remap(frame, result, mapx_, mapy_, INTER_LINEAR, BORDER_REPLICATE);
 
149
}
 
150
 
 
151
#if defined(HAVE_OPENCV_CUDAWARPING)
 
152
void MoreAccurateMotionWobbleSuppressorGpu::suppress(int idx, const cuda::GpuMat &frame, cuda::GpuMat &result)
 
153
{
 
154
    CV_Assert(motions_ && stabilizationMotions_);
 
155
 
 
156
    if (idx % period_ == 0)
 
157
    {
 
158
        result = frame;
 
159
        return;
 
160
    }
 
161
 
 
162
    int k1 = idx / period_ * period_;
 
163
    int k2 = std::min(k1 + period_, frameCount_ - 1);
 
164
 
 
165
    Mat S1 = (*stabilizationMotions_)[idx];
 
166
 
 
167
    Mat ML = S1 * getMotion(k1, idx, *motions2_) * getMotion(k1, idx, *motions_).inv() * S1.inv();
 
168
    Mat MR = S1 * getMotion(idx, k2, *motions2_).inv() * getMotion(idx, k2, *motions_) * S1.inv();
 
169
 
 
170
    cuda::calcWobbleSuppressionMaps(k1, idx, k2, frame.size(), ML, MR, mapx_, mapy_);
 
171
 
 
172
    if (result.data == frame.data)
 
173
        result = cuda::GpuMat(frame.size(), frame.type());
 
174
 
 
175
    cuda::remap(frame, result, mapx_, mapy_, INTER_LINEAR, BORDER_REPLICATE);
 
176
}
 
177
 
 
178
 
 
179
void MoreAccurateMotionWobbleSuppressorGpu::suppress(int idx, const Mat &frame, Mat &result)
 
180
{
 
181
    frameDevice_.upload(frame);
 
182
    suppress(idx, frameDevice_, resultDevice_);
 
183
    resultDevice_.download(result);
 
184
}
 
185
#endif
 
186
 
 
187
} // namespace videostab
 
188
} // namespace cv