diff options
Diffstat (limited to 'jni_mosaic/feature_stab/src/dbreg/vp_motionmodel.c')
| -rw-r--r-- | jni_mosaic/feature_stab/src/dbreg/vp_motionmodel.c | 377 |
1 files changed, 377 insertions, 0 deletions
diff --git a/jni_mosaic/feature_stab/src/dbreg/vp_motionmodel.c b/jni_mosaic/feature_stab/src/dbreg/vp_motionmodel.c new file mode 100644 index 000000000..1f6af15bd --- /dev/null +++ b/jni_mosaic/feature_stab/src/dbreg/vp_motionmodel.c @@ -0,0 +1,377 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* +#sourcefile vpmotion/vp_motionmodel.c +#category motion-model +* +* Copyright 1998 Sarnoff Corporation +* All Rights Reserved +* +* Modification History +* Date: 02/14/98 +* Author: supuns +* Shop Order: 17xxx +* @(#) $Id: vp_motionmodel.c,v 1.4 2011/06/17 14:04:33 mbansal Exp $ +*/ + +/* +* =================================================================== +* Include Files +*/ + +#include <string.h> /* memmove */ +#include <math.h> +#include "vp_motionmodel.h" + +/* Static Functions */ +static +double Det3(double m[3][3]) +{ + double result; + + result = + m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + + m[0][2]*m[1][0]*m[2][1] - m[0][2]*m[1][1]*m[2][0] - + m[0][0]*m[1][2]*m[2][1] - m[0][1]*m[1][0]*m[2][2]; + + return(result); +} + +typedef double MATRIX[4][4]; + +static +double Det4(MATRIX m) +{ + /* ==> This is a poor implementation of determinant. + Writing the formula out in closed form is unnecessarily complicated + and mistakes are easy to make. */ + double result; + + result= + m[0][3] *m[1][2] *m[2][1] *m[3][0] - m[0][2] *m[1][3] *m[2][1] *m[3][0] - m[0][3] *m[1][1] *m[2][2] *m[3][0] + + m[0][1] *m[1][3] *m[2][2] *m[3][0] + m[0][2] *m[1][1] *m[2][3] *m[3][0] - m[0][1] *m[1][2] *m[2][3] *m[3][0] - m[0][3] *m[1][2] *m[2][0] *m[3][1] + + m[0][2] *m[1][3] *m[2][0] *m[3][1] + m[0][3] *m[1][0] *m[2][2] *m[3][1] - m[0][0] *m[1][3] *m[2][2] *m[3][1] - m[0][2] *m[1][0] *m[2][3] *m[3][1] + + m[0][0] *m[1][2] *m[2][3] *m[3][1] + m[0][3] *m[1][1] *m[2][0] *m[3][2] - m[0][1] *m[1][3] *m[2][0] *m[3][2] - m[0][3] *m[1][0] *m[2][1] *m[3][2] + + m[0][0] *m[1][3] *m[2][1] *m[3][2] + m[0][1] *m[1][0] *m[2][3] *m[3][2] - m[0][0] *m[1][1] *m[2][3] *m[3][2] - m[0][2] *m[1][1] *m[2][0] *m[3][3] + + m[0][1] *m[1][2] *m[2][0] *m[3][3] + m[0][2] *m[1][0] *m[2][1] *m[3][3] - m[0][0] *m[1][2] *m[2][1] *m[3][3] - m[0][1] *m[1][0] *m[2][2] *m[3][3] + + m[0][0] *m[1][1] *m[2][2] *m[3][3]; + /* + m[0][0]*m[1][1]*m[2][2]*m[3][3]-m[0][1]*m[1][0]*m[2][2]*m[3][3]+ + m[0][1]*m[1][2]*m[2][0]*m[3][3]-m[0][2]*m[1][1]*m[2][0]*m[3][3]+ + m[0][2]*m[1][0]*m[2][1]*m[3][3]-m[0][0]*m[1][2]*m[2][1]*m[3][3]+ + m[0][0]*m[1][2]*m[2][3]*m[3][1]-m[0][2]*m[1][0]*m[2][3]*m[3][1]+ + m[0][2]*m[1][3]*m[2][0]*m[3][1]-m[0][3]*m[1][2]*m[2][0]*m[3][1]+ + m[0][3]*m[1][0]*m[2][2]*m[3][1]-m[0][0]*m[1][3]*m[2][2]*m[3][1]+ + m[0][0]*m[1][3]*m[2][1]*m[3][2]-m[0][3]*m[1][0]*m[2][3]*m[3][2]+ + m[0][1]*m[1][0]*m[2][3]*m[3][2]-m[0][0]*m[1][1]*m[2][0]*m[3][2]+ + m[0][3]*m[1][1]*m[2][0]*m[3][2]-m[0][1]*m[1][3]*m[2][1]*m[3][2]+ + m[0][1]*m[1][3]*m[2][2]*m[3][0]-m[0][3]*m[1][1]*m[2][2]*m[3][0]+ + m[0][2]*m[1][1]*m[2][3]*m[3][0]-m[0][1]*m[1][2]*m[2][3]*m[3][0]+ + m[0][3]*m[1][2]*m[2][1]*m[3][0]-m[0][2]*m[1][3]*m[2][1]*m[3][0]; + */ + return(result); +} + +static +int inv4Mat(const VP_MOTION* in, VP_MOTION* out) +{ + /* ==> This is a poor implementation of inversion. The determinant + method is O(N^4), i.e. unnecessarily slow, and not numerically accurate. + The real complexity of inversion is O(N^3), and is best done using + LU decomposition. */ + + MATRIX inmat,outmat; + int i, j, k, l, m, n,ntemp; + double mat[3][3], indet, temp; + + /* check for non-empty structures structure */ + if (((VP_MOTION *) NULL == in) || ((VP_MOTION *) NULL == out)) { + return 1; + } + + for(k=0,i=0;i<4;i++) + for(j=0;j<4;j++,k++) + inmat[i][j]=(double)in->par[k]; + + indet = Det4(inmat); + if (indet==0) return(-1); + + for (i=0;i<4;i++) { + for (j=0;j<4;j++) { + m = 0; + for (k=0;k<4;k++) { + if (i != k) { + n = 0; + for (l=0;l<4;l++) + if (j != l) { + mat[m][n] = inmat[k][l]; + n++; + } + m++; + } + } + + temp = -1.; + ntemp = (i +j ) %2; + if( ntemp == 0) temp = 1.; + + outmat[j][i] = temp * Det3(mat)/indet; + } + } + + for(k=0,i=0;i<4;i++) + for(j=0;j<4;j++,k++) + out->par[k]=(VP_PAR)outmat[i][j]; /*lint !e771*/ + + return(0); +} + +/* +* =================================================================== +* Public Functions +#htmlstart +*/ + +/* + * =================================================================== +#fn vp_invert_motion +#ft invert a motion +#fd DEFINITION + Bool + vp_invert_motion(const VP_MOTION* in,VP_MOTION* out) +#fd PURPOSE + This inverts the motion given in 'in'. + All motion models upto VP_MOTION_SEMI_PROJ_3D are supported. + It is assumed that the all 16 parameters are properly + initialized although you may not be using them. You could + use the VP_KEEP_ macro's defined in vp_motionmodel.h to set + the un-initialized parameters. This uses a 4x4 matrix invertion + function internally. + It is SAFE to pass the same pointer as both the 'in' and 'out' + parameters. +#fd INPUTS + in - input motion +#fd OUTPUTS + out - output inverted motion. If singular matrix uninitialized. + if MWW(in) is non-zero it is also normalized. +#fd RETURNS + FALSE - matrix is singular or motion model not supported + TRUE - otherwise +#fd SIDE EFFECTS + None +#endfn +*/ + +int vp_invert_motion(const VP_MOTION* in,VP_MOTION* out) +{ + int refid; + + /* check for non-empty structures structure */ + if (((VP_MOTION *) NULL == in) || ((VP_MOTION *) NULL == out)) { + return FALSE; + } + + if (in->type>VP_MOTION_SEMI_PROJ_3D) { + return FALSE; + } + + if (inv4Mat(in,out)<0) + return FALSE; + + /*VP_NORMALIZE(*out);*/ + out->type = in->type; + refid=in->refid; + out->refid=in->insid; + out->insid=refid; + return TRUE; +} + +/* +* =================================================================== +#fn vp_cascade_motion +#ft Cascade two motion transforms +#fd DEFINITION + Bool + vp_cascade_motion(const VP_MOTION* InAB,const VP_MOTION* InBC,VP_MOTION* OutAC) +#fd PURPOSE + Given Motion Transforms A->B and B->C, this function will + generate a New Motion that describes the transformation + from A->C. + More specifically, OutAC = InBC * InAC. + This function works ok if InAB,InBC and OutAC are the same pointer. +#fd INPUTS + InAB - First Motion Transform + InBC - Second Motion Tranform +#fd OUTPUTS + OutAC - Cascaded Motion +#fd RETURNS + FALSE - motion model not supported + TRUE - otherwise +#fd SIDE EFFECTS + None +#endfn +*/ + +int vp_cascade_motion(const VP_MOTION* InA, const VP_MOTION* InB,VP_MOTION* Out) +{ + /* ==> This is a poor implementation of matrix multiplication. + Writing the formula out in closed form is unnecessarily complicated + and mistakes are easy to make. */ + VP_PAR mxx,mxy,mxz,mxw; + VP_PAR myx,myy,myz,myw; + VP_PAR mzx,mzy,mzz,mzw; + VP_PAR mwx,mwy,mwz,mww; + + /* check for non-empty structures structure */ + if (((VP_MOTION *) NULL == InA) || ((VP_MOTION *) NULL == InB) || + ((VP_MOTION *) NULL == Out)) { + return FALSE; + } + + if (InA->type>VP_MOTION_PROJ_3D) { + return FALSE; + } + + if (InB->type>VP_MOTION_PROJ_3D) { + return FALSE; + } + + mxx = MXX(*InB)*MXX(*InA)+MXY(*InB)*MYX(*InA)+MXZ(*InB)*MZX(*InA)+MXW(*InB)*MWX(*InA); + mxy = MXX(*InB)*MXY(*InA)+MXY(*InB)*MYY(*InA)+MXZ(*InB)*MZY(*InA)+MXW(*InB)*MWY(*InA); + mxz = MXX(*InB)*MXZ(*InA)+MXY(*InB)*MYZ(*InA)+MXZ(*InB)*MZZ(*InA)+MXW(*InB)*MWZ(*InA); + mxw = MXX(*InB)*MXW(*InA)+MXY(*InB)*MYW(*InA)+MXZ(*InB)*MZW(*InA)+MXW(*InB)*MWW(*InA); + myx = MYX(*InB)*MXX(*InA)+MYY(*InB)*MYX(*InA)+MYZ(*InB)*MZX(*InA)+MYW(*InB)*MWX(*InA); + myy = MYX(*InB)*MXY(*InA)+MYY(*InB)*MYY(*InA)+MYZ(*InB)*MZY(*InA)+MYW(*InB)*MWY(*InA); + myz = MYX(*InB)*MXZ(*InA)+MYY(*InB)*MYZ(*InA)+MYZ(*InB)*MZZ(*InA)+MYW(*InB)*MWZ(*InA); + myw = MYX(*InB)*MXW(*InA)+MYY(*InB)*MYW(*InA)+MYZ(*InB)*MZW(*InA)+MYW(*InB)*MWW(*InA); + mzx = MZX(*InB)*MXX(*InA)+MZY(*InB)*MYX(*InA)+MZZ(*InB)*MZX(*InA)+MZW(*InB)*MWX(*InA); + mzy = MZX(*InB)*MXY(*InA)+MZY(*InB)*MYY(*InA)+MZZ(*InB)*MZY(*InA)+MZW(*InB)*MWY(*InA); + mzz = MZX(*InB)*MXZ(*InA)+MZY(*InB)*MYZ(*InA)+MZZ(*InB)*MZZ(*InA)+MZW(*InB)*MWZ(*InA); + mzw = MZX(*InB)*MXW(*InA)+MZY(*InB)*MYW(*InA)+MZZ(*InB)*MZW(*InA)+MZW(*InB)*MWW(*InA); + mwx = MWX(*InB)*MXX(*InA)+MWY(*InB)*MYX(*InA)+MWZ(*InB)*MZX(*InA)+MWW(*InB)*MWX(*InA); + mwy = MWX(*InB)*MXY(*InA)+MWY(*InB)*MYY(*InA)+MWZ(*InB)*MZY(*InA)+MWW(*InB)*MWY(*InA); + mwz = MWX(*InB)*MXZ(*InA)+MWY(*InB)*MYZ(*InA)+MWZ(*InB)*MZZ(*InA)+MWW(*InB)*MWZ(*InA); + mww = MWX(*InB)*MXW(*InA)+MWY(*InB)*MYW(*InA)+MWZ(*InB)*MZW(*InA)+MWW(*InB)*MWW(*InA); + + MXX(*Out)=mxx; MXY(*Out)=mxy; MXZ(*Out)=mxz; MXW(*Out)=mxw; + MYX(*Out)=myx; MYY(*Out)=myy; MYZ(*Out)=myz; MYW(*Out)=myw; + MZX(*Out)=mzx; MZY(*Out)=mzy; MZZ(*Out)=mzz; MZW(*Out)=mzw; + MWX(*Out)=mwx; MWY(*Out)=mwy; MWZ(*Out)=mwz; MWW(*Out)=mww; + /* VP_NORMALIZE(*Out); */ + Out->type= (InA->type > InB->type) ? InA->type : InB->type; + Out->refid=InA->refid; + Out->insid=InB->insid; + + return TRUE; +} + +/* +* =================================================================== +#fn vp_copy_motion +#ft Copies the source motion to the destination motion. +#fd DEFINITION + void + vp_copy_motion (const VP_MOTION *src, VP_MOTION *dst) +#fd PURPOSE + Copies the source motion to the destination motion. + It is OK if src == dst. + NOTE THAT THE SOURCE IS THE FIRST ARGUMENT. + This is different from some of the other VP + copy functions. +#fd INPUTS + src is the source motion + dst is the destination motion +#fd RETURNS + void +#endfn +*/ +void vp_copy_motion (const VP_MOTION *src, VP_MOTION *dst) +{ + /* Use memmove rather than memcpy because it handles overlapping memory + OK. */ + memmove(dst, src, sizeof(VP_MOTION)); + return; +} /* vp_copy_motion() */ + +#define VP_SQR(x) ( (x)*(x) ) +double vp_motion_cornerdiff(const VP_MOTION *mot_a, const VP_MOTION *mot_b, + int xo, int yo, int w, int h) +{ + double ax1, ay1, ax2, ay2, ax3, ay3, ax4, ay4; + double bx1, by1, bx2, by2, bx3, by3, bx4, by4; + double err; + + /*lint -e639 -e632 -e633 */ + VP_WARP_POINT_2D(xo, yo, *mot_a, ax1, ay1); + VP_WARP_POINT_2D(xo+w-1, yo, *mot_a, ax2, ay2); + VP_WARP_POINT_2D(xo+w-1, yo+h-1, *mot_a, ax3, ay3); + VP_WARP_POINT_2D(xo, yo+h-1, *mot_a, ax4, ay4); + VP_WARP_POINT_2D(xo, yo, *mot_b, bx1, by1); + VP_WARP_POINT_2D(xo+w-1, yo, *mot_b, bx2, by2); + VP_WARP_POINT_2D(xo+w-1, yo+h-1, *mot_b, bx3, by3); + VP_WARP_POINT_2D(xo, yo+h-1, *mot_b, bx4, by4); + /*lint +e639 +e632 +e633 */ + + err = 0; + err += (VP_SQR(ax1 - bx1) + VP_SQR(ay1 - by1)); + err += (VP_SQR(ax2 - bx2) + VP_SQR(ay2 - by2)); + err += (VP_SQR(ax3 - bx3) + VP_SQR(ay3 - by3)); + err += (VP_SQR(ax4 - bx4) + VP_SQR(ay4 - by4)); + + return(sqrt(err)); +} + +int vp_zoom_motion2d(VP_MOTION* in, VP_MOTION* out, + int n, int w, int h, double zoom) +{ + int ii; + VP_PAR inv_zoom; + VP_PAR cx, cy; + VP_MOTION R2r,R2f; + VP_MOTION *res; + + /* check for non-empty structures structure */ + if (((VP_MOTION *) NULL == in)||(zoom <= 0.0)||(w <= 0)||(h <= 0)) { + return FALSE; + } + + /* ==> Not sure why the special case of out=NULL is necessary. Why couldn't + the caller just pass the same pointer for both in and out? */ + res = ((VP_MOTION *) NULL == out)?in:out; + + cx = (VP_PAR) (w/2.0); + cy = (VP_PAR) (h/2.0); + + VP_MOTION_ID(R2r); + inv_zoom = (VP_PAR)(1.0/zoom); + MXX(R2r) = inv_zoom; + MYY(R2r) = inv_zoom; + MXW(R2r)=cx*(((VP_PAR)1.0) - inv_zoom); + MYW(R2r)=cy*(((VP_PAR)1.0) - inv_zoom); + + VP_KEEP_AFFINE_2D(R2r); + + for(ii=0;ii<n;ii++) { + (void) vp_cascade_motion(&R2r,in+ii,&R2f); + res[ii]=R2f; + } + + return TRUE; +} /* vp_zoom_motion2d() */ + +/* =================================================================== */ +/* end vp_motionmodel.c */ |