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 27 db_StabilizationSmoother::db_StabilizationSmoother() 28 { 29 Init(); 30 } 31 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 57 db_StabilizationSmoother::~db_StabilizationSmoother() 58 {} 59 60 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 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 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 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 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 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 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
