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 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
smoothMotion(VP_MOTION * inmot,VP_MOTION * outmot,double smooth_factor)192 bool 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
238 bool
smoothMotion1(VP_MOTION * inmot,VP_MOTION * outmot,VP_MOTION * motLF,VP_MOTION * imotLF,double factor)239 db_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
is_point_in_rect(double px,double py,double rx,double ry,double w,double h)291 bool 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
vpmotion_add(VP_MOTION * in1,VP_MOTION * in2,VP_MOTION * out)307 static 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
vpmotion_multiply(VP_MOTION * in1,double factor,VP_MOTION * out)319 static 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