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 
24 static bool vpmotion_add(VP_MOTION *in1, VP_MOTION *in2, VP_MOTION *out);
25 static bool vpmotion_multiply(VP_MOTION *in1, double factor, VP_MOTION *out);
26 
db_StabilizationSmoother()27 db_StabilizationSmoother::db_StabilizationSmoother()
28 {
29     Init();
30 }
31 
Init()32 void 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 
~db_StabilizationSmoother()57 db_StabilizationSmoother::~db_StabilizationSmoother()
58 {}
59 
60 
smoothMotion(VP_MOTION * inmot,VP_MOTION * outmot)61 bool 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 
smoothMotionAdaptive(int hsize,int vsize,VP_MOTION * inmot,VP_MOTION * outmot)108 bool 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         // FixLater: avoid floating point loop counters
140         // NOLINTNEXTLINE(clang-analyzer-security.FloatLoopCounter,cert-flp30-c)
141         for (smoothFactor = smoothFactor; smoothFactor >= minSmoothFactor; smoothFactor -= 0.01) {
142             // Compute the smoothed motion
143             if(!smoothMotion(inmot, &tmpMotion, smoothFactor))
144                 break;
145 
146             // TmpMotion, or Qsi where s is the smoothed display reference and i is the
147             // current image, tells us how points in the S co-ordinate system map to
148             // points in the I CS.  We would like to check whether the four corners of the
149             // warped and smoothed display reference lies entirely within the I co-ordinate
150             // system.  If yes, then the amount of smoothing is sufficient so that NO
151             // border pixels are seen at the output.  We test for f_smoothFactor terms
152             // between 0.9 and 1.0, in steps of 0.01, and between 0.5 ands 0.9 in steps of 0.1
153 
154             (void) vp_zoom_motion2d(&tmpMotion, &testMotion, 1, hsize, vsize, (double)f_zoom); // needs to return bool
155 
156             VP_WARP_POINT_2D(0, 0, testMotion, p1x, p1y);
157             VP_WARP_POINT_2D(hsize - 1, 0, testMotion, p2x, p2y);
158             VP_WARP_POINT_2D(hsize - 1, vsize - 1, testMotion, p3x, p3y);
159             VP_WARP_POINT_2D(0, vsize - 1, testMotion, p4x, p4y);
160 
161             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))) {
162                 continue;
163             }
164             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))) {
165                 continue;
166             }
167             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))) {
168                 continue;
169             }
170             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))) {
171                 continue;
172             }
173 
174             // If we get here, then all the points are in the rectangle.
175             // Therefore, break out of this loop
176             break;
177         }
178 
179         // if we get here and f_smoothFactor <= fMinDampingFactor, reset the stab reference
180         if (smoothFactor < f_minDampingFactor)
181             smoothFactor = f_minDampingFactor;
182 
183         // use the smoothed motion for stabilization
184         vp_copy_motion_no_id(&tmpMotion/*in*/, outmot/*out*/);
185     }
186     else
187     {
188         vp_copy_motion_no_id(inmot, outmot);
189     }
190 
191     return true;
192 }
193 
smoothMotion(VP_MOTION * inmot,VP_MOTION * outmot,double smooth_factor)194 bool db_StabilizationSmoother::smoothMotion(VP_MOTION *inmot, VP_MOTION *outmot, double smooth_factor)
195 {
196     f_motLF.insid = inmot->refid;
197     f_motLF.refid = inmot->insid;
198 
199     if(f_smoothOn) {
200         if(!f_smoothReset) {
201             MXX(f_motLF) = (VP_PAR) (smooth_factor*(double) MXX(f_motLF) + (1.0-smooth_factor)* (double) MXX(*inmot));
202             MXY(f_motLF) = (VP_PAR) (smooth_factor*(double) MXY(f_motLF) + (1.0-smooth_factor)* (double) MXY(*inmot));
203             MXZ(f_motLF) = (VP_PAR) (smooth_factor*(double) MXZ(f_motLF) + (1.0-smooth_factor)* (double) MXZ(*inmot));
204             MXW(f_motLF) = (VP_PAR) (smooth_factor*(double) MXW(f_motLF) + (1.0-smooth_factor)* (double) MXW(*inmot));
205 
206             MYX(f_motLF) = (VP_PAR) (smooth_factor*(double) MYX(f_motLF) + (1.0-smooth_factor)* (double) MYX(*inmot));
207             MYY(f_motLF) = (VP_PAR) (smooth_factor*(double) MYY(f_motLF) + (1.0-smooth_factor)* (double) MYY(*inmot));
208             MYZ(f_motLF) = (VP_PAR) (smooth_factor*(double) MYZ(f_motLF) + (1.0-smooth_factor)* (double) MYZ(*inmot));
209             MYW(f_motLF) = (VP_PAR) (smooth_factor*(double) MYW(f_motLF) + (1.0-smooth_factor)* (double) MYW(*inmot));
210 
211             MZX(f_motLF) = (VP_PAR) (smooth_factor*(double) MZX(f_motLF) + (1.0-smooth_factor)* (double) MZX(*inmot));
212             MZY(f_motLF) = (VP_PAR) (smooth_factor*(double) MZY(f_motLF) + (1.0-smooth_factor)* (double) MZY(*inmot));
213             MZZ(f_motLF) = (VP_PAR) (smooth_factor*(double) MZZ(f_motLF) + (1.0-smooth_factor)* (double) MZZ(*inmot));
214             MZW(f_motLF) = (VP_PAR) (smooth_factor*(double) MZW(f_motLF) + (1.0-smooth_factor)* (double) MZW(*inmot));
215 
216             MWX(f_motLF) = (VP_PAR) (smooth_factor*(double) MWX(f_motLF) + (1.0-smooth_factor)* (double) MWX(*inmot));
217             MWY(f_motLF) = (VP_PAR) (smooth_factor*(double) MWY(f_motLF) + (1.0-smooth_factor)* (double) MWY(*inmot));
218             MWZ(f_motLF) = (VP_PAR) (smooth_factor*(double) MWZ(f_motLF) + (1.0-smooth_factor)* (double) MWZ(*inmot));
219             MWW(f_motLF) = (VP_PAR) (smooth_factor*(double) MWW(f_motLF) + (1.0-smooth_factor)* (double) MWW(*inmot));
220         }
221         else
222             vp_copy_motion_no_id(inmot, &f_motLF); // smooth_factor = 0.0
223 
224         // Only allow LF motion to be compensated. Remove HF motion from
225         // the output transformation
226         if(!vp_invert_motion(&f_motLF, &f_imotLF))
227             return false;
228 
229         if(!vp_cascade_motion(&f_imotLF, inmot, outmot))
230             return false;
231     }
232     else {
233         vp_copy_motion_no_id(inmot, outmot);
234     }
235 
236     return true;
237 }
238 
239 //! Overloaded smoother function that takes in user-specidied smoothing factor
240 bool
smoothMotion1(VP_MOTION * inmot,VP_MOTION * outmot,VP_MOTION * motLF,VP_MOTION * imotLF,double factor)241 db_StabilizationSmoother::smoothMotion1(VP_MOTION *inmot, VP_MOTION *outmot, VP_MOTION *motLF, VP_MOTION *imotLF, double factor)
242 {
243 
244     if(!f_smoothOn) {
245         vp_copy_motion(inmot, outmot);
246         return true;
247     }
248     else {
249         if(!f_smoothReset) {
250             MXX(*motLF) = (VP_PAR) (factor*(double) MXX(*motLF) + (1.0-factor)* (double) MXX(*inmot));
251             MXY(*motLF) = (VP_PAR) (factor*(double) MXY(*motLF) + (1.0-factor)* (double) MXY(*inmot));
252             MXZ(*motLF) = (VP_PAR) (factor*(double) MXZ(*motLF) + (1.0-factor)* (double) MXZ(*inmot));
253             MXW(*motLF) = (VP_PAR) (factor*(double) MXW(*motLF) + (1.0-factor)* (double) MXW(*inmot));
254 
255             MYX(*motLF) = (VP_PAR) (factor*(double) MYX(*motLF) + (1.0-factor)* (double) MYX(*inmot));
256             MYY(*motLF) = (VP_PAR) (factor*(double) MYY(*motLF) + (1.0-factor)* (double) MYY(*inmot));
257             MYZ(*motLF) = (VP_PAR) (factor*(double) MYZ(*motLF) + (1.0-factor)* (double) MYZ(*inmot));
258             MYW(*motLF) = (VP_PAR) (factor*(double) MYW(*motLF) + (1.0-factor)* (double) MYW(*inmot));
259 
260             MZX(*motLF) = (VP_PAR) (factor*(double) MZX(*motLF) + (1.0-factor)* (double) MZX(*inmot));
261             MZY(*motLF) = (VP_PAR) (factor*(double) MZY(*motLF) + (1.0-factor)* (double) MZY(*inmot));
262             MZZ(*motLF) = (VP_PAR) (factor*(double) MZZ(*motLF) + (1.0-factor)* (double) MZZ(*inmot));
263             MZW(*motLF) = (VP_PAR) (factor*(double) MZW(*motLF) + (1.0-factor)* (double) MZW(*inmot));
264 
265             MWX(*motLF) = (VP_PAR) (factor*(double) MWX(*motLF) + (1.0-factor)* (double) MWX(*inmot));
266             MWY(*motLF) = (VP_PAR) (factor*(double) MWY(*motLF) + (1.0-factor)* (double) MWY(*inmot));
267             MWZ(*motLF) = (VP_PAR) (factor*(double) MWZ(*motLF) + (1.0-factor)* (double) MWZ(*inmot));
268             MWW(*motLF) = (VP_PAR) (factor*(double) MWW(*motLF) + (1.0-factor)* (double) MWW(*inmot));
269         }
270         else {
271             vp_copy_motion(inmot, motLF);
272         }
273         // Only allow LF motion to be compensated. Remove HF motion from the output transformation
274         if(!vp_invert_motion(motLF, imotLF)) {
275 #if DEBUG_PRINT
276             printfOS("Invert failed \n");
277 #endif
278             return false;
279         }
280         if(!vp_cascade_motion(imotLF, inmot, outmot)) {
281 #if DEBUG_PRINT
282             printfOS("cascade failed \n");
283 #endif
284             return false;
285         }
286     }
287     return true;
288 }
289 
290 
291 
292 
is_point_in_rect(double px,double py,double rx,double ry,double w,double h)293 bool db_StabilizationSmoother::is_point_in_rect(double px, double py, double rx, double ry, double w, double h)
294 {
295     if (px < rx)
296         return(false);
297     if (px >= rx + w)
298         return(false);
299     if (py < ry)
300         return(false);
301     if (py >= ry + h)
302         return(false);
303 
304     return(true);
305 }
306 
307 
308 
vpmotion_add(VP_MOTION * in1,VP_MOTION * in2,VP_MOTION * out)309 static bool vpmotion_add(VP_MOTION *in1, VP_MOTION *in2, VP_MOTION *out)
310 {
311     int i;
312     if(in1 == NULL || in2 == NULL || out == NULL)
313         return false;
314 
315     for(i = 0; i < VP_MAX_MOTION_PAR; i++)
316         out->par[i] = in1->par[i] + in2->par[i];
317 
318     return true;
319 }
320 
vpmotion_multiply(VP_MOTION * in1,double factor,VP_MOTION * out)321 static bool vpmotion_multiply(VP_MOTION *in1, double factor, VP_MOTION *out)
322 {
323     int i;
324     if(in1 == NULL || out == NULL)
325         return false;
326 
327     for(i = 0; i < VP_MAX_MOTION_PAR; i++)
328         out->par[i] = in1->par[i] * factor;
329 
330     return true;
331 }
332 
333