1/*
2 * Copyright (C) 2011 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 *      http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17#include <stdlib.h>
18#include "dbstabsmooth.h"
19
20///// TODO TODO ////////// Replace this with the actual definition from Jayan's reply /////////////
21#define vp_copy_motion_no_id vp_copy_motion
22///////////////////////////////////////////////////////////////////////////////////////////////////
23
24static bool vpmotion_add(VP_MOTION *in1, VP_MOTION *in2, VP_MOTION *out);
25static bool vpmotion_multiply(VP_MOTION *in1, double factor, VP_MOTION *out);
26
27db_StabilizationSmoother::db_StabilizationSmoother()
28{
29    Init();
30}
31
32void db_StabilizationSmoother::Init()
33{
34    f_smoothOn = true;
35    f_smoothReset = false;
36    f_smoothFactor = 1.0f;
37    f_minDampingFactor = 0.2f;
38    f_zoom = 1.0f;
39    VP_MOTION_ID(f_motLF);
40    VP_MOTION_ID(f_imotLF);
41    f_hsize = 0;
42    f_vsize = 0;
43
44    VP_MOTION_ID(f_disp_mot);
45    VP_MOTION_ID(f_src_mot);
46    VP_MOTION_ID(f_diff_avg);
47
48    for( int i = 0; i < MOTION_ARRAY-1; i++) {
49        VP_MOTION_ID(f_hist_mot_speed[i]);
50        VP_MOTION_ID(f_hist_mot[i]);
51        VP_MOTION_ID(f_hist_diff_mot[i]);
52    }
53    VP_MOTION_ID(f_hist_mot[MOTION_ARRAY-1]);
54
55}
56
57db_StabilizationSmoother::~db_StabilizationSmoother()
58{}
59
60
61bool db_StabilizationSmoother::smoothMotion(VP_MOTION *inmot, VP_MOTION *outmot)
62{
63    VP_MOTION_ID(f_motLF);
64    VP_MOTION_ID(f_imotLF);
65    f_motLF.insid = inmot->refid;
66    f_motLF.refid = inmot->insid;
67
68    if(f_smoothOn) {
69        if(!f_smoothReset) {
70            MXX(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MXX(f_motLF) + (1.0-f_smoothFactor)* (double) MXX(*inmot));
71            MXY(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MXY(f_motLF) + (1.0-f_smoothFactor)* (double) MXY(*inmot));
72            MXZ(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MXZ(f_motLF) + (1.0-f_smoothFactor)* (double) MXZ(*inmot));
73            MXW(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MXW(f_motLF) + (1.0-f_smoothFactor)* (double) MXW(*inmot));
74
75            MYX(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MYX(f_motLF) + (1.0-f_smoothFactor)* (double) MYX(*inmot));
76            MYY(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MYY(f_motLF) + (1.0-f_smoothFactor)* (double) MYY(*inmot));
77            MYZ(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MYZ(f_motLF) + (1.0-f_smoothFactor)* (double) MYZ(*inmot));
78            MYW(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MYW(f_motLF) + (1.0-f_smoothFactor)* (double) MYW(*inmot));
79
80            MZX(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MZX(f_motLF) + (1.0-f_smoothFactor)* (double) MZX(*inmot));
81            MZY(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MZY(f_motLF) + (1.0-f_smoothFactor)* (double) MZY(*inmot));
82            MZZ(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MZZ(f_motLF) + (1.0-f_smoothFactor)* (double) MZZ(*inmot));
83            MZW(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MZW(f_motLF) + (1.0-f_smoothFactor)* (double) MZW(*inmot));
84
85            MWX(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MWX(f_motLF) + (1.0-f_smoothFactor)* (double) MWX(*inmot));
86            MWY(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MWY(f_motLF) + (1.0-f_smoothFactor)* (double) MWY(*inmot));
87            MWZ(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MWZ(f_motLF) + (1.0-f_smoothFactor)* (double) MWZ(*inmot));
88            MWW(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MWW(f_motLF) + (1.0-f_smoothFactor)* (double) MWW(*inmot));
89        }
90        else
91            vp_copy_motion_no_id(inmot, &f_motLF); // f_smoothFactor = 0.0
92
93        // Only allow LF motion to be compensated. Remove HF motion from
94        // the output transformation
95        if(!vp_invert_motion(&f_motLF, &f_imotLF))
96            return false;
97
98        if(!vp_cascade_motion(&f_imotLF, inmot, outmot))
99            return false;
100    }
101    else {
102        vp_copy_motion_no_id(inmot, outmot);
103    }
104
105    return true;
106}
107
108bool db_StabilizationSmoother::smoothMotionAdaptive(/*VP_BIMG *bimg,*/int hsize, int vsize, VP_MOTION *inmot, VP_MOTION *outmot)
109{
110    VP_MOTION tmpMotion, testMotion;
111    VP_PAR p1x, p2x, p3x, p4x;
112    VP_PAR p1y, p2y, p3y, p4y;
113    double smoothFactor;
114    double minSmoothFactor = f_minDampingFactor;
115
116//  int hsize = bimg->w;
117//  int vsize = bimg->h;
118    double border_factor = 0.01;//0.2;
119    double border_x = border_factor * hsize;
120    double border_y = border_factor * vsize;
121
122    VP_MOTION_ID(f_motLF);
123    VP_MOTION_ID(f_imotLF);
124    VP_MOTION_ID(testMotion);
125    VP_MOTION_ID(tmpMotion);
126
127    if (f_smoothOn) {
128        VP_MOTION identityMotion;
129        VP_MOTION_ID(identityMotion); // initialize the motion
130        vp_copy_motion(inmot/*in*/, &testMotion/*out*/);
131        VP_PAR delta = vp_motion_cornerdiff(&testMotion, &identityMotion, 0, 0,(int)hsize, (int)vsize);
132
133        smoothFactor = 0.99 - 0.0015 * delta;
134
135        if(smoothFactor < minSmoothFactor)
136            smoothFactor = minSmoothFactor;
137
138        // Find the amount of motion that must be compensated so that no "border" pixels are seen in the stable video
139        for (smoothFactor = smoothFactor; smoothFactor >= minSmoothFactor; smoothFactor -= 0.01) {
140            // Compute the smoothed motion
141            if(!smoothMotion(inmot, &tmpMotion, smoothFactor))
142                break;
143
144            // TmpMotion, or Qsi where s is the smoothed display reference and i is the
145            // current image, tells us how points in the S co-ordinate system map to
146            // points in the I CS.  We would like to check whether the four corners of the
147            // warped and smoothed display reference lies entirely within the I co-ordinate
148            // system.  If yes, then the amount of smoothing is sufficient so that NO
149            // border pixels are seen at the output.  We test for f_smoothFactor terms
150            // between 0.9 and 1.0, in steps of 0.01, and between 0.5 ands 0.9 in steps of 0.1
151
152            (void) vp_zoom_motion2d(&tmpMotion, &testMotion, 1, hsize, vsize, (double)f_zoom); // needs to return bool
153
154            VP_WARP_POINT_2D(0, 0, testMotion, p1x, p1y);
155            VP_WARP_POINT_2D(hsize - 1, 0, testMotion, p2x, p2y);
156            VP_WARP_POINT_2D(hsize - 1, vsize - 1, testMotion, p3x, p3y);
157            VP_WARP_POINT_2D(0, vsize - 1, testMotion, p4x, p4y);
158
159            if (!is_point_in_rect((double)p1x,(double)p1y,-border_x,-border_y,(double)(hsize+2.0*border_x),(double)(vsize+2.0*border_y))) {
160                continue;
161            }
162            if (!is_point_in_rect((double)p2x, (double)p2y,-border_x,-border_y,(double)(hsize+2.0*border_x),(double)(vsize+2.0*border_y))) {
163                continue;
164            }
165            if (!is_point_in_rect((double)p3x,(double)p3y,-border_x,-border_y,(double)(hsize+2.0*border_x),(double)(vsize+2.0*border_y))) {
166                continue;
167            }
168            if (!is_point_in_rect((double)p4x, (double)p4y,-border_x,-border_y,(double)(hsize+2.0*border_x),(double)(vsize+2.0*border_y))) {
169                continue;
170            }
171
172            // If we get here, then all the points are in the rectangle.
173            // Therefore, break out of this loop
174            break;
175        }
176
177        // if we get here and f_smoothFactor <= fMinDampingFactor, reset the stab reference
178        if (smoothFactor < f_minDampingFactor)
179            smoothFactor = f_minDampingFactor;
180
181        // use the smoothed motion for stabilization
182        vp_copy_motion_no_id(&tmpMotion/*in*/, outmot/*out*/);
183    }
184    else
185    {
186        vp_copy_motion_no_id(inmot, outmot);
187    }
188
189    return true;
190}
191
192bool db_StabilizationSmoother::smoothMotion(VP_MOTION *inmot, VP_MOTION *outmot, double smooth_factor)
193{
194    f_motLF.insid = inmot->refid;
195    f_motLF.refid = inmot->insid;
196
197    if(f_smoothOn) {
198        if(!f_smoothReset) {
199            MXX(f_motLF) = (VP_PAR) (smooth_factor*(double) MXX(f_motLF) + (1.0-smooth_factor)* (double) MXX(*inmot));
200            MXY(f_motLF) = (VP_PAR) (smooth_factor*(double) MXY(f_motLF) + (1.0-smooth_factor)* (double) MXY(*inmot));
201            MXZ(f_motLF) = (VP_PAR) (smooth_factor*(double) MXZ(f_motLF) + (1.0-smooth_factor)* (double) MXZ(*inmot));
202            MXW(f_motLF) = (VP_PAR) (smooth_factor*(double) MXW(f_motLF) + (1.0-smooth_factor)* (double) MXW(*inmot));
203
204            MYX(f_motLF) = (VP_PAR) (smooth_factor*(double) MYX(f_motLF) + (1.0-smooth_factor)* (double) MYX(*inmot));
205            MYY(f_motLF) = (VP_PAR) (smooth_factor*(double) MYY(f_motLF) + (1.0-smooth_factor)* (double) MYY(*inmot));
206            MYZ(f_motLF) = (VP_PAR) (smooth_factor*(double) MYZ(f_motLF) + (1.0-smooth_factor)* (double) MYZ(*inmot));
207            MYW(f_motLF) = (VP_PAR) (smooth_factor*(double) MYW(f_motLF) + (1.0-smooth_factor)* (double) MYW(*inmot));
208
209            MZX(f_motLF) = (VP_PAR) (smooth_factor*(double) MZX(f_motLF) + (1.0-smooth_factor)* (double) MZX(*inmot));
210            MZY(f_motLF) = (VP_PAR) (smooth_factor*(double) MZY(f_motLF) + (1.0-smooth_factor)* (double) MZY(*inmot));
211            MZZ(f_motLF) = (VP_PAR) (smooth_factor*(double) MZZ(f_motLF) + (1.0-smooth_factor)* (double) MZZ(*inmot));
212            MZW(f_motLF) = (VP_PAR) (smooth_factor*(double) MZW(f_motLF) + (1.0-smooth_factor)* (double) MZW(*inmot));
213
214            MWX(f_motLF) = (VP_PAR) (smooth_factor*(double) MWX(f_motLF) + (1.0-smooth_factor)* (double) MWX(*inmot));
215            MWY(f_motLF) = (VP_PAR) (smooth_factor*(double) MWY(f_motLF) + (1.0-smooth_factor)* (double) MWY(*inmot));
216            MWZ(f_motLF) = (VP_PAR) (smooth_factor*(double) MWZ(f_motLF) + (1.0-smooth_factor)* (double) MWZ(*inmot));
217            MWW(f_motLF) = (VP_PAR) (smooth_factor*(double) MWW(f_motLF) + (1.0-smooth_factor)* (double) MWW(*inmot));
218        }
219        else
220            vp_copy_motion_no_id(inmot, &f_motLF); // smooth_factor = 0.0
221
222        // Only allow LF motion to be compensated. Remove HF motion from
223        // the output transformation
224        if(!vp_invert_motion(&f_motLF, &f_imotLF))
225            return false;
226
227        if(!vp_cascade_motion(&f_imotLF, inmot, outmot))
228            return false;
229    }
230    else {
231        vp_copy_motion_no_id(inmot, outmot);
232    }
233
234    return true;
235}
236
237//! Overloaded smoother function that takes in user-specidied smoothing factor
238bool
239db_StabilizationSmoother::smoothMotion1(VP_MOTION *inmot, VP_MOTION *outmot, VP_MOTION *motLF, VP_MOTION *imotLF, double factor)
240{
241
242    if(!f_smoothOn) {
243        vp_copy_motion(inmot, outmot);
244        return true;
245    }
246    else {
247        if(!f_smoothReset) {
248            MXX(*motLF) = (VP_PAR) (factor*(double) MXX(*motLF) + (1.0-factor)* (double) MXX(*inmot));
249            MXY(*motLF) = (VP_PAR) (factor*(double) MXY(*motLF) + (1.0-factor)* (double) MXY(*inmot));
250            MXZ(*motLF) = (VP_PAR) (factor*(double) MXZ(*motLF) + (1.0-factor)* (double) MXZ(*inmot));
251            MXW(*motLF) = (VP_PAR) (factor*(double) MXW(*motLF) + (1.0-factor)* (double) MXW(*inmot));
252
253            MYX(*motLF) = (VP_PAR) (factor*(double) MYX(*motLF) + (1.0-factor)* (double) MYX(*inmot));
254            MYY(*motLF) = (VP_PAR) (factor*(double) MYY(*motLF) + (1.0-factor)* (double) MYY(*inmot));
255            MYZ(*motLF) = (VP_PAR) (factor*(double) MYZ(*motLF) + (1.0-factor)* (double) MYZ(*inmot));
256            MYW(*motLF) = (VP_PAR) (factor*(double) MYW(*motLF) + (1.0-factor)* (double) MYW(*inmot));
257
258            MZX(*motLF) = (VP_PAR) (factor*(double) MZX(*motLF) + (1.0-factor)* (double) MZX(*inmot));
259            MZY(*motLF) = (VP_PAR) (factor*(double) MZY(*motLF) + (1.0-factor)* (double) MZY(*inmot));
260            MZZ(*motLF) = (VP_PAR) (factor*(double) MZZ(*motLF) + (1.0-factor)* (double) MZZ(*inmot));
261            MZW(*motLF) = (VP_PAR) (factor*(double) MZW(*motLF) + (1.0-factor)* (double) MZW(*inmot));
262
263            MWX(*motLF) = (VP_PAR) (factor*(double) MWX(*motLF) + (1.0-factor)* (double) MWX(*inmot));
264            MWY(*motLF) = (VP_PAR) (factor*(double) MWY(*motLF) + (1.0-factor)* (double) MWY(*inmot));
265            MWZ(*motLF) = (VP_PAR) (factor*(double) MWZ(*motLF) + (1.0-factor)* (double) MWZ(*inmot));
266            MWW(*motLF) = (VP_PAR) (factor*(double) MWW(*motLF) + (1.0-factor)* (double) MWW(*inmot));
267        }
268        else {
269            vp_copy_motion(inmot, motLF);
270        }
271        // Only allow LF motion to be compensated. Remove HF motion from the output transformation
272        if(!vp_invert_motion(motLF, imotLF)) {
273#if DEBUG_PRINT
274            printfOS("Invert failed \n");
275#endif
276            return false;
277        }
278        if(!vp_cascade_motion(imotLF, inmot, outmot)) {
279#if DEBUG_PRINT
280            printfOS("cascade failed \n");
281#endif
282            return false;
283        }
284    }
285    return true;
286}
287
288
289
290
291bool db_StabilizationSmoother::is_point_in_rect(double px, double py, double rx, double ry, double w, double h)
292{
293    if (px < rx)
294        return(false);
295    if (px >= rx + w)
296        return(false);
297    if (py < ry)
298        return(false);
299    if (py >= ry + h)
300        return(false);
301
302    return(true);
303}
304
305
306
307static bool vpmotion_add(VP_MOTION *in1, VP_MOTION *in2, VP_MOTION *out)
308{
309    int i;
310    if(in1 == NULL || in2 == NULL || out == NULL)
311        return false;
312
313    for(i = 0; i < VP_MAX_MOTION_PAR; i++)
314        out->par[i] = in1->par[i] + in2->par[i];
315
316    return true;
317}
318
319static bool vpmotion_multiply(VP_MOTION *in1, double factor, VP_MOTION *out)
320{
321    int i;
322    if(in1 == NULL || out == NULL)
323        return false;
324
325    for(i = 0; i < VP_MAX_MOTION_PAR; i++)
326        out->par[i] = in1->par[i] * factor;
327
328    return true;
329}
330
331