diff options
Diffstat (limited to 'jni/feature_stab/db_vlvm')
28 files changed, 0 insertions, 12006 deletions
diff --git a/jni/feature_stab/db_vlvm/db_bundle.h b/jni/feature_stab/db_vlvm/db_bundle.h deleted file mode 100644 index e4fb8db2c..000000000 --- a/jni/feature_stab/db_vlvm/db_bundle.h +++ /dev/null @@ -1,68 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_bundle.h,v 1.2 2011/06/17 14:03:30 mbansal Exp $ */ - -#ifndef DB_BUNDLE_H -#define DB_BUNDLE_H - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ -/*! - * \defgroup LMBundle (LM) Bundle adjustment utilities (a.k.a. Levenberg-Marquardt algorithm) - */ -/*\{*/ - -#include "db_utilities.h" - -/*! -Solve for update dx such that diagmult(1+lambda,transpose(J)%J)%dx= -Jtf -using only upper half of JtJ, destroying lower half below diagonal in the process -dimension is n and d should point to n allocated doubles of scratch memory -*/ -inline void db_Compute_dx(double *dx,double **JtJ,double *min_Jtf,double lambda,double *d,int n) -{ - int i; - double opl; - - opl=1.0+lambda; - for(i=0;i<n;i++) d[i]=JtJ[i][i]*opl; - - db_CholeskyDecompSeparateDiagonal(JtJ,d,n); - db_CholeskyBacksub(dx,JtJ,d,n,min_Jtf); -} - -/*! -Solve for update dx such that diagmult(1+lambda,transpose(J)%J)%dx= -Jtf -using only upper half of JtJ, destroying lower half below diagonal in the process -*/ -inline void db_Compute_dx_3x3(double dx[3],double JtJ[9],const double min_Jtf[3],double lambda) -{ - double d[3],opl; - - opl=1.0+lambda; - d[0]=JtJ[0]*opl; - d[1]=JtJ[4]*opl; - d[2]=JtJ[8]*opl; - db_CholeskyDecomp3x3SeparateDiagonal(JtJ,d); - db_CholeskyBacksub3x3(dx,JtJ,d,min_Jtf); -} - -/*\}*/ - -#endif /* DB_BUNDLE_H */ diff --git a/jni/feature_stab/db_vlvm/db_feature_detection.cpp b/jni/feature_stab/db_vlvm/db_feature_detection.cpp deleted file mode 100644 index 28cb4a781..000000000 --- a/jni/feature_stab/db_vlvm/db_feature_detection.cpp +++ /dev/null @@ -1,1770 +0,0 @@ -/* - * 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. - */ - -/*$Id: db_feature_detection.cpp,v 1.4 2011/06/17 14:03:30 mbansal Exp $*/ - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ - -#include "db_utilities.h" -#include "db_feature_detection.h" -#ifdef _VERBOSE_ -#include <iostream> -#endif -#include <float.h> - -#define DB_SUB_PIXEL - -#define BORDER 10 // 5 - -float** db_AllocStrengthImage_f(float **im,int w,int h) -{ - int i,n,aw; - long c,size; - float **img,*aim,*p; - - /*Determine number of 124 element chunks needed*/ - n=(db_maxi(1,w-6)+123)/124; - /*Determine the total allocation width aw*/ - aw=n*124+8; - /*Allocate*/ - size=aw*h+16; - *im=new float [size]; - /*Clean up*/ - p=(*im); - for(c=0;c<size;c++) p[c]=0.0; - /*Get a 16 byte aligned pointer*/ - aim=db_AlignPointer_f(*im,16); - /*Allocate pointer table*/ - img=new float* [h]; - /*Initialize the pointer table*/ - for(i=0;i<h;i++) - { - img[i]=aim+aw*i+1; - } - - return(img); -} - -void db_FreeStrengthImage_f(float *im,float **img,int h) -{ - delete [] im; - delete [] img; -} - -/*Compute derivatives Ix,Iy for a subrow of img with upper left (i,j) and width chunk_width -Memory references occur one pixel outside the subrow*/ -inline void db_IxIyRow_f(float *Ix,float *Iy,const float * const *img,int i,int j,int chunk_width) -{ - int c; - - for(c=0;c<chunk_width;c++) - { - Ix[c]=img[i][j+c-1]-img[i][j+c+1]; - Iy[c]=img[i-1][j+c]-img[i+1][j+c]; - } -} - -/*Compute derivatives Ix,Iy for a subrow of img with upper left (i,j) and width 128 -Memory references occur one pixel outside the subrow*/ -inline void db_IxIyRow_u(int *dxx,const unsigned char * const *img,int i,int j,int nc) -{ -#ifdef DB_USE_MMX - const unsigned char *r1,*r2,*r3; - - r1=img[i-1]+j; r2=img[i]+j; r3=img[i+1]+j; - - _asm - { - mov esi,16 - mov eax,r1 - mov ebx,r2 - mov ecx,r3 - mov edx,dxx - - /*Get bitmask into mm7*/ - mov edi,7F7F7F7Fh - movd mm7,edi - punpckldq mm7,mm7 - -loopstart: - /***************dx part 1-12*********************************/ - movq mm0,[eax] /*1 Get upper*/ - pxor mm6,mm6 /*2 Set to zero*/ - movq mm1,[ecx] /*3 Get lower*/ - psrlq mm0,1 /*4 Shift*/ - psrlq mm1,1 /*5 Shift*/ - pand mm0,mm7 /*6 And*/ - movq mm2,[ebx-1] /*13 Get left*/ - pand mm1,mm7 /*7 And*/ - psubb mm0,mm1 /*8 Subtract*/ - pxor mm5,mm5 /*14 Set to zero*/ - movq mm1,mm0 /*9 Copy*/ - pcmpgtb mm6,mm0 /*10 Create unpack mask*/ - movq mm3,[ebx+1] /*15 Get right*/ - punpcklbw mm0,mm6 /*11 Unpack low*/ - punpckhbw mm1,mm6 /*12 Unpack high*/ - /***************dy part 13-24*********************************/ - movq mm4,mm0 /*25 Copy dx*/ - psrlq mm2,1 /*16 Shift*/ - pmullw mm0,mm0 /*26 Multiply dx*dx*/ - psrlq mm3,1 /*17 Shift*/ - pand mm2,mm7 /*18 And*/ - pand mm3,mm7 /*19 And*/ - /*Stall*/ - psubb mm2,mm3 /*20 Subtract*/ - /*Stall*/ - movq mm3,mm2 /*21 Copy*/ - pcmpgtb mm5,mm2 /*22 Create unpack mask*/ - punpcklbw mm2,mm5 /*23 Unpack low*/ - /*Stall*/ - punpckhbw mm3,mm5 /*24 Unpack high*/ - /***************dxx dxy dyy low part 25-49*********************************/ - pmullw mm4,mm2 /*27 Multiply dx*dy*/ - pmullw mm2,mm2 /*28 Multiply dy*dy*/ - pxor mm6,mm6 /*29 Set to zero*/ - movq mm5,mm0 /*30 Copy dx*dx*/ - pcmpgtw mm6,mm0 /*31 Create unpack mask for dx*dx*/ - punpcklwd mm0,mm6 /*32 Unpack dx*dx lows*/ - /*Stall*/ - punpckhwd mm5,mm6 /*33 Unpack dx*dx highs*/ - pxor mm6,mm6 /*36 Set to zero*/ - movq [edx],mm0 /*34 Store dx*dx lows*/ - movq mm0,mm4 /*37 Copy dx*dy*/ - movq [edx+8],mm5 /*35 Store dx*dx highs*/ - pcmpgtw mm6,mm4 /*38 Create unpack mask for dx*dy*/ - punpcklwd mm4,mm6 /*39 Unpack dx*dy lows*/ - /*Stall*/ - punpckhwd mm0,mm6 /*40 Unpack dx*dy highs*/ - pxor mm6,mm6 /*43 Set to zero*/ - movq [edx+512],mm4 /*41 Store dx*dy lows*/ - movq mm5,mm2 /*44 Copy dy*dy*/ - movq [edx+520],mm0 /*42 Store dx*dy highs*/ - pcmpgtw mm6,mm2 /*45 Create unpack mask for dy*dy*/ - punpcklwd mm2,mm6 /*46 Unpack dy*dy lows*/ - movq mm4,mm1 /*50 Copy dx*/ - punpckhwd mm5,mm6 /*47 Unpack dy*dy highs*/ - pmullw mm1,mm1 /*51 Multiply dx*dx*/ - movq [edx+1024],mm2 /*48 Store dy*dy lows*/ - pmullw mm4,mm3 /*52 Multiply dx*dy*/ - movq [edx+1032],mm5 /*49 Store dy*dy highs*/ - /***************dxx dxy dyy high part 50-79*********************************/ - pmullw mm3,mm3 /*53 Multiply dy*dy*/ - pxor mm6,mm6 /*54 Set to zero*/ - movq mm5,mm1 /*55 Copy dx*dx*/ - pcmpgtw mm6,mm1 /*56 Create unpack mask for dx*dx*/ - pxor mm2,mm2 /*61 Set to zero*/ - punpcklwd mm1,mm6 /*57 Unpack dx*dx lows*/ - movq mm0,mm4 /*62 Copy dx*dy*/ - punpckhwd mm5,mm6 /*58 Unpack dx*dx highs*/ - pcmpgtw mm2,mm4 /*63 Create unpack mask for dx*dy*/ - movq [edx+16],mm1 /*59 Store dx*dx lows*/ - punpcklwd mm4,mm2 /*64 Unpack dx*dy lows*/ - movq [edx+24],mm5 /*60 Store dx*dx highs*/ - punpckhwd mm0,mm2 /*65 Unpack dx*dy highs*/ - movq [edx+528],mm4 /*66 Store dx*dy lows*/ - pxor mm6,mm6 /*68 Set to zero*/ - movq [edx+536],mm0 /*67 Store dx*dy highs*/ - movq mm5,mm3 /*69 Copy dy*dy*/ - pcmpgtw mm6,mm3 /*70 Create unpack mask for dy*dy*/ - add eax,8 /*75*/ - punpcklwd mm3,mm6 /*71 Unpack dy*dy lows*/ - add ebx,8 /*76*/ - punpckhwd mm5,mm6 /*72 Unpack dy*dy highs*/ - add ecx,8 /*77*/ - movq [edx+1040],mm3 /*73 Store dy*dy lows*/ - /*Stall*/ - movq [edx+1048],mm5 /*74 Store dy*dy highs*/ - /*Stall*/ - add edx,32 /*78*/ - dec esi /*79*/ - jnz loopstart - - emms - } - -#else - int c; - int Ix,Iy; - - for(c=0;c<nc;c++) - { - Ix=(img[i][j+c-1]-img[i][j+c+1])>>1; - Iy=(img[i-1][j+c]-img[i+1][j+c])>>1; - dxx[c]=Ix*Ix; - dxx[c+128]=Ix*Iy; - dxx[c+256]=Iy*Iy; - } -#endif /*DB_USE_MMX*/ -} - -/*Filter vertically five rows of derivatives of length chunk_width into gxx,gxy,gyy*/ -inline void db_gxx_gxy_gyy_row_f(float *gxx,float *gxy,float *gyy,int chunk_width, - float *Ix0,float *Ix1,float *Ix2,float *Ix3,float *Ix4, - float *Iy0,float *Iy1,float *Iy2,float *Iy3,float *Iy4) -{ - int c; - float dx,dy; - float Ixx0,Ixy0,Iyy0,Ixx1,Ixy1,Iyy1,Ixx2,Ixy2,Iyy2,Ixx3,Ixy3,Iyy3,Ixx4,Ixy4,Iyy4; - - for(c=0;c<chunk_width;c++) - { - dx=Ix0[c]; - dy=Iy0[c]; - Ixx0=dx*dx; - Ixy0=dx*dy; - Iyy0=dy*dy; - - dx=Ix1[c]; - dy=Iy1[c]; - Ixx1=dx*dx; - Ixy1=dx*dy; - Iyy1=dy*dy; - - dx=Ix2[c]; - dy=Iy2[c]; - Ixx2=dx*dx; - Ixy2=dx*dy; - Iyy2=dy*dy; - - dx=Ix3[c]; - dy=Iy3[c]; - Ixx3=dx*dx; - Ixy3=dx*dy; - Iyy3=dy*dy; - - dx=Ix4[c]; - dy=Iy4[c]; - Ixx4=dx*dx; - Ixy4=dx*dy; - Iyy4=dy*dy; - - /*Filter vertically*/ - gxx[c]=Ixx0+Ixx1*4.0f+Ixx2*6.0f+Ixx3*4.0f+Ixx4; - gxy[c]=Ixy0+Ixy1*4.0f+Ixy2*6.0f+Ixy3*4.0f+Ixy4; - gyy[c]=Iyy0+Iyy1*4.0f+Iyy2*6.0f+Iyy3*4.0f+Iyy4; - } -} - -/*Filter vertically five rows of derivatives of length 128 into gxx,gxy,gyy*/ -inline void db_gxx_gxy_gyy_row_s(int *g,int *d0,int *d1,int *d2,int *d3,int *d4,int nc) -{ -#ifdef DB_USE_MMX - int c; - - _asm - { - mov c,64 - mov eax,d0 - mov ebx,d1 - mov ecx,d2 - mov edx,d3 - mov edi,d4 - mov esi,g - -loopstart: - /***************dxx part 1-14*********************************/ - movq mm0,[eax] /*1 Get dxx0*/ - /*Stall*/ - movq mm1,[ebx] /*2 Get dxx1*/ - /*Stall*/ - movq mm2,[ecx] /*5 Get dxx2*/ - pslld mm1,2 /*3 Shift dxx1*/ - movq mm3,[edx] /*10 Get dxx3*/ - paddd mm0,mm1 /*4 Accumulate dxx1*/ - movq mm4,[eax+512] /*15 Get dxy0*/ - pslld mm2,1 /*6 Shift dxx2 1*/ - paddd mm0,mm2 /*7 Accumulate dxx2 1*/ - pslld mm2,1 /*8 Shift dxx2 2*/ - movq mm5,[ebx+512] /*16 Get dxy1*/ - paddd mm0,mm2 /*9 Accumulate dxx2 2*/ - pslld mm3,2 /*11 Shift dxx3*/ - /*Stall*/ - paddd mm0,mm3 /*12 Accumulate dxx3*/ - pslld mm5,2 /*17 Shift dxy1*/ - paddd mm0,[edi] /*13 Accumulate dxx4*/ - paddd mm4,mm5 /*18 Accumulate dxy1*/ - movq mm6,[ecx+512] /*19 Get dxy2*/ - /*Stall*/ - movq [esi],mm0 /*14 Store dxx sums*/ - /***************dxy part 15-28*********************************/ - pslld mm6,1 /*20 Shift dxy2 1*/ - paddd mm4,mm6 /*21 Accumulate dxy2 1*/ - pslld mm6,1 /*22 Shift dxy2 2*/ - movq mm0,[eax+1024] /*29 Get dyy0*/ - paddd mm4,mm6 /*23 Accumulate dxy2 2*/ - movq mm7,[edx+512] /*24 Get dxy3*/ - pslld mm7,2 /*25 Shift dxy3*/ - movq mm1,[ebx+1024] /*30 Get dyy1*/ - paddd mm4,mm7 /*26 Accumulate dxy3*/ - paddd mm4,[edi+512] /*27 Accumulate dxy4*/ - pslld mm1,2 /*31 Shift dyy1*/ - movq mm2,[ecx+1024] /*33 Get dyy2*/ - paddd mm0,mm1 /*32 Accumulate dyy1*/ - movq [esi+512],mm4 /*28 Store dxy sums*/ - pslld mm2,1 /*34 Shift dyy2 1*/ - /***************dyy part 29-49*********************************/ - - - movq mm3,[edx+1024] /*38 Get dyy3*/ - paddd mm0,mm2 /*35 Accumulate dyy2 1*/ - paddd mm0,[edi+1024] /*41 Accumulate dyy4*/ - pslld mm2,1 /*36 Shift dyy2 2*/ - paddd mm0,mm2 /*37 Accumulate dyy2 2*/ - pslld mm3,2 /*39 Shift dyy3*/ - paddd mm0,mm3 /*40 Accumulate dyy3*/ - add eax,8 /*43*/ - add ebx,8 /*44*/ - add ecx,8 /*45*/ - movq [esi+1024],mm0 /*42 Store dyy sums*/ - /*Stall*/ - add edx,8 /*46*/ - add edi,8 /*47*/ - add esi,8 /*48*/ - dec c /*49*/ - jnz loopstart - - emms - } - -#else - int c,dd; - - for(c=0;c<nc;c++) - { - /*Filter vertically*/ - dd=d2[c]; - g[c]=d0[c]+(d1[c]<<2)+(dd<<2)+(dd<<1)+(d3[c]<<2)+d4[c]; - - dd=d2[c+128]; - g[c+128]=d0[c+128]+(d1[c+128]<<2)+(dd<<2)+(dd<<1)+(d3[c+128]<<2)+d4[c+128]; - - dd=d2[c+256]; - g[c+256]=d0[c+256]+(d1[c+256]<<2)+(dd<<2)+(dd<<1)+(d3[c+256]<<2)+d4[c+256]; - } -#endif /*DB_USE_MMX*/ -} - -/*Filter horizontally the three rows gxx,gxy,gyy into the strength subrow starting at i,j -and with width chunk_width. gxx,gxy and gyy are assumed to be four pixels wider than chunk_width -and starting at (i,j-2)*/ -inline void db_HarrisStrength_row_f(float **s,float *gxx,float *gxy,float *gyy,int i,int j,int chunk_width) -{ - float Gxx,Gxy,Gyy,det,trc; - int c; - - for(c=0;c<chunk_width;c++) - { - Gxx=gxx[c]+gxx[c+1]*4.0f+gxx[c+2]*6.0f+gxx[c+3]*4.0f+gxx[c+4]; - Gxy=gxy[c]+gxy[c+1]*4.0f+gxy[c+2]*6.0f+gxy[c+3]*4.0f+gxy[c+4]; - Gyy=gyy[c]+gyy[c+1]*4.0f+gyy[c+2]*6.0f+gyy[c+3]*4.0f+gyy[c+4]; - - det=Gxx*Gyy-Gxy*Gxy; - trc=Gxx+Gyy; - s[i][j+c]=det-0.06f*trc*trc; - } -} - -/*Filter g of length 128 in place with 14641. Output is shifted two steps -and of length 124*/ -inline void db_Filter14641_128_i(int *g,int nc) -{ -#ifdef DB_USE_MMX - int mask; - - mask=0xFFFFFFFF; - _asm - { - mov esi,31 - mov eax,g - - /*Get bitmask 00000000FFFFFFFF into mm7*/ - movd mm7,mask - - /*Warming iteration one 1-16********************/ - movq mm6,[eax] /*1 Load new data*/ - paddd mm0,mm6 /*2 Add 1* behind two steps*/ - movq mm2,mm6 /*3 Start with 1* in front two steps*/ - pslld mm6,1 /*4*/ - paddd mm1,mm6 /*5 Add 2* same place*/ - pslld mm6,1 /*6*/ - paddd mm1,mm6 /*7 Add 4* same place*/ - pshufw mm6,mm6,4Eh /*8 Swap the two double-words using bitmask 01001110=4Eh*/ - paddd mm1,mm6 /*9 Add 4* swapped*/ - movq mm5,mm6 /*10 Copy*/ - pand mm6,mm7 /*11 Get low double-word only*/ - paddd mm2,mm6 /*12 Add 4* in front one step*/ - pxor mm6,mm5 /*13 Get high double-word only*/ - paddd mm0,mm6 /*14 Add 4* behind one step*/ - movq mm0,mm1 /*15 Shift along*/ - movq mm1,mm2 /*16 Shift along*/ - /*Warming iteration two 17-32********************/ - movq mm4,[eax+8] /*17 Load new data*/ - paddd mm0,mm4 /*18 Add 1* behind two steps*/ - movq mm2,mm4 /*19 Start with 1* in front two steps*/ - pslld mm4,1 /*20*/ - paddd mm1,mm4 /*21 Add 2* same place*/ - pslld mm4,1 /*22*/ - paddd mm1,mm4 /*23 Add 4* same place*/ - pshufw mm4,mm4,4Eh /*24 Swap the two double-words using bitmask 01001110=4Eh*/ - paddd mm1,mm4 /*25 Add 4* swapped*/ - movq mm3,mm4 /*26 Copy*/ - pand mm4,mm7 /*27 Get low double-word only*/ - paddd mm2,mm4 /*28 Add 4* in front one step*/ - pxor mm4,mm3 /*29 Get high double-word only*/ - paddd mm0,mm4 /*30 Add 4* behind one step*/ - movq mm0,mm1 /*31 Shift along*/ - movq mm1,mm2 /*32 Shift along*/ - - /*Loop********************/ -loopstart: - /*First part of loop 33-47********/ - movq mm6,[eax+16] /*33 Load new data*/ - /*Stall*/ - paddd mm0,mm6 /*34 Add 1* behind two steps*/ - movq mm2,mm6 /*35 Start with 1* in front two steps*/ - movq mm4,[eax+24] /*48 Load new data*/ - pslld mm6,1 /*36*/ - paddd mm1,mm6 /*37 Add 2* same place*/ - pslld mm6,1 /*38*/ - paddd mm1,mm6 /*39 Add 4* same place*/ - pshufw mm6,mm6,4Eh /*40 Swap the two double-words using bitmask 01001110=4Eh*/ - paddd mm1,mm4 /*49 Add 1* behind two steps*/ - movq mm5,mm6 /*41 Copy*/ - paddd mm1,mm6 /*42 Add 4* swapped*/ - pand mm6,mm7 /*43 Get low double-word only*/ - paddd mm2,mm6 /*44 Add 4* in front one step*/ - pxor mm6,mm5 /*45 Get high double-word only*/ - paddd mm0,mm6 /*46 Add 4* behind one step*/ - movq mm6,mm4 /*50a Copy*/ - pslld mm4,1 /*51*/ - /*Stall*/ - movq [eax],mm0 /*47 Store result two steps behind*/ - /*Second part of loop 48-66********/ - movq mm0,mm6 /*50b Start with 1* in front two steps*/ - paddd mm2,mm4 /*52 Add 2* same place*/ - pslld mm4,1 /*53*/ - paddd mm2,mm4 /*54 Add 4* same place*/ - pshufw mm4,mm4,4Eh /*55 Swap the two double-words using bitmask 01001110=4Eh*/ - paddd mm2,mm4 /*56 Add 4* swapped*/ - movq mm3,mm4 /*57 Copy*/ - pand mm4,mm7 /*58 Get low double-word only*/ - /*Stall*/ - paddd mm0,mm4 /*59 Add 4* in front one step*/ - pxor mm4,mm3 /*60 Get high double-word only*/ - paddd mm1,mm4 /*61 Add 4* behind one step*/ - add eax,16 /*65*/ - dec esi /*66*/ - /*Stall*/ - movq [eax-8],mm1 /*62 Store result two steps behind*/ - movq mm1,mm0 /*63 Shift along*/ - movq mm0,mm2 /*64 Shift along*/ - jnz loopstart - - emms - } - -#else - int c; - - for(c=0;c<nc-4;c++) - { - g[c]=g[c]+(g[c+1]<<2)+(g[c+2]<<2)+(g[c+2]<<1)+(g[c+3]<<2)+g[c+4]; - } -#endif /*DB_USE_MMX*/ -} - -/*Filter horizontally the three rows gxx,gxy,gyy of length 128 into the strength subrow s -of length 124. gxx,gxy and gyy are assumed to be starting at (i,j-2) if s[i][j] is sought. -s should be 16 byte aligned*/ -inline void db_HarrisStrength_row_s(float *s,int *gxx,int *gxy,int *gyy,int nc) -{ - float k; - - k=0.06f; - - db_Filter14641_128_i(gxx,nc); - db_Filter14641_128_i(gxy,nc); - db_Filter14641_128_i(gyy,nc); - -#ifdef DB_USE_SIMD - - - _asm - { - mov esi,15 - mov eax,gxx - mov ebx,gxy - mov ecx,gyy - mov edx,s - - /*broadcast k to all positions of xmm7*/ - movss xmm7,k - shufps xmm7,xmm7,0 - - /*****Warm up 1-10**************************************/ - cvtpi2ps xmm0,[eax+8] /*1 Convert two integers into floating point of low double-word*/ - /*Stall*/ - cvtpi2ps xmm1,[ebx+8] /*4 Convert two integers into floating point of low double-word*/ - movlhps xmm0,xmm0 /*2 Move them to the high double-word*/ - cvtpi2ps xmm2,[ecx+8] /*7 Convert two integers into floating point of low double-word*/ - movlhps xmm1,xmm1 /*5 Move them to the high double-word*/ - cvtpi2ps xmm0,[eax] /*3 Convert two integers into floating point of low double-word*/ - movlhps xmm2,xmm2 /*8 Move them to the high double-word*/ - cvtpi2ps xmm1,[ebx] /*6 Convert two integers into floating point of low double-word*/ - movaps xmm3,xmm0 /*10 Copy Cxx*/ - cvtpi2ps xmm2,[ecx] /*9 Convert two integers into floating point of low double-word*/ - /*Stall*/ -loopstart: - /*****First part of loop 11-18***********************/ - mulps xmm0,xmm2 /*11 Multiply to get Gxx*Gyy*/ - addps xmm2,xmm3 /*12 Add to get Gxx+Gyy*/ - cvtpi2ps xmm4,[eax+24] /*19 Convert two integers into floating point of low double-word*/ - mulps xmm1,xmm1 /*13 Multiply to get Gxy*Gxy*/ - mulps xmm2,xmm2 /*14 Multiply to get (Gxx+Gyy)*(Gxx+Gyy)*/ - movlhps xmm4,xmm4 /*20 Move them to the high double-word*/ - cvtpi2ps xmm4,[eax+16] /*21 Convert two integers into floating point of low double-word*/ - /*Stall*/ - subps xmm0,xmm1 /*15 Subtract to get Gxx*Gyy-Gxy*Gxy*/ - mulps xmm2,xmm7 /*16 Multiply to get k*(Gxx+Gyy)*(Gxx+Gyy)*/ - cvtpi2ps xmm5,[ebx+24] /*22 Convert two integers into floating point of low double-word*/ - /*Stall*/ - movlhps xmm5,xmm5 /*23 Move them to the high double-word*/ - /*Stall*/ - cvtpi2ps xmm5,[ebx+16] /*24 Convert two integers into floating point of low double-word*/ - subps xmm0,xmm2 /*17 Subtract to get Gxx*Gyy-Gxy*Gxy-k*(Gxx+Gyy)*(Gxx+Gyy)*/ - cvtpi2ps xmm6,[ecx+24] /*25 Convert two integers into floating point of low double-word*/ - /*Stall*/ - movaps [edx],xmm0 /*18 Store*/ - /*****Second part of loop 26-40***********************/ - movlhps xmm6,xmm6 /*26 Move them to the high double-word*/ - cvtpi2ps xmm6,[ecx+16] /*27 Convert two integers into floating point of low double-word*/ - movaps xmm3,xmm4 /*28 Copy Cxx*/ - mulps xmm4,xmm6 /*29 Multiply to get Gxx*Gyy*/ - addps xmm6,xmm3 /*30 Add to get Gxx+Gyy*/ - cvtpi2ps xmm0,[eax+40] /*(1 Next) Convert two integers into floating point of low double-word*/ - mulps xmm5,xmm5 /*31 Multiply to get Gxy*Gxy*/ - cvtpi2ps xmm1,[ebx+40] /*(4 Next) Convert two integers into floating point of low double-word*/ - mulps xmm6,xmm6 /*32 Multiply to get (Gxx+Gyy)*(Gxx+Gyy)*/ - cvtpi2ps xmm2,[ecx+40] /*(7 Next) Convert two integers into floating point of low double-word*/ - movlhps xmm0,xmm0 /*(2 Next) Move them to the high double-word*/ - subps xmm4,xmm5 /*33 Subtract to get Gxx*Gyy-Gxy*Gxy*/ - movlhps xmm1,xmm1 /*(5 Next) Move them to the high double-word*/ - cvtpi2ps xmm0,[eax+32] /*(3 Next)Convert two integers into floating point of low double-word*/ - mulps xmm6,xmm7 /*34 Multiply to get k*(Gxx+Gyy)*(Gxx+Gyy)*/ - cvtpi2ps xmm1,[ebx+32] /*(6 Next) Convert two integers into floating point of low double-word*/ - movlhps xmm2,xmm2 /*(8 Next) Move them to the high double-word*/ - movaps xmm3,xmm0 /*(10 Next) Copy Cxx*/ - add eax,32 /*37*/ - subps xmm4,xmm6 /*35 Subtract to get Gxx*Gyy-Gxy*Gxy-k*(Gxx+Gyy)*(Gxx+Gyy)*/ - add ebx,32 /*38*/ - cvtpi2ps xmm2,[ecx+32] /*(9 Next) Convert two integers into floating point of low double-word*/ - /*Stall*/ - movaps [edx+16],xmm4 /*36 Store*/ - /*Stall*/ - add ecx,32 /*39*/ - add edx,32 /*40*/ - dec esi /*41*/ - jnz loopstart - - /****Cool down***************/ - mulps xmm0,xmm2 /*Multiply to get Gxx*Gyy*/ - addps xmm2,xmm3 /*Add to get Gxx+Gyy*/ - mulps xmm1,xmm1 /*Multiply to get Gxy*Gxy*/ - mulps xmm2,xmm2 /*Multiply to get (Gxx+Gyy)*(Gxx+Gyy)*/ - subps xmm0,xmm1 /*Subtract to get Gxx*Gyy-Gxy*Gxy*/ - mulps xmm2,xmm7 /*Multiply to get k*(Gxx+Gyy)*(Gxx+Gyy)*/ - subps xmm0,xmm2 /*Subtract to get Gxx*Gyy-Gxy*Gxy-k*(Gxx+Gyy)*(Gxx+Gyy)*/ - movaps [edx],xmm0 /*Store*/ - } - -#else - float Gxx,Gxy,Gyy,det,trc; - int c; - - //for(c=0;c<124;c++) - for(c=0;c<nc-4;c++) - { - Gxx=(float)gxx[c]; - Gxy=(float)gxy[c]; - Gyy=(float)gyy[c]; - - det=Gxx*Gyy-Gxy*Gxy; - trc=Gxx+Gyy; - s[c]=det-k*trc*trc; - } -#endif /*DB_USE_SIMD*/ -} - -/*Compute the Harris corner strength of the chunk [left,top,right,bottom] of img and -store it into the corresponding region of s. left and top have to be at least 3 and -right and bottom have to be at most width-4,height-4*/ -inline void db_HarrisStrengthChunk_f(float **s,const float * const *img,int left,int top,int right,int bottom, - /*temp should point to at least - 13*(right-left+5) of allocated memory*/ - float *temp) -{ - float *Ix[5],*Iy[5]; - float *gxx,*gxy,*gyy; - int i,chunk_width,chunk_width_p4; - - chunk_width=right-left+1; - chunk_width_p4=chunk_width+4; - gxx=temp; - gxy=gxx+chunk_width_p4; - gyy=gxy+chunk_width_p4; - for(i=0;i<5;i++) - { - Ix[i]=gyy+chunk_width_p4+(2*i*chunk_width_p4); - Iy[i]=Ix[i]+chunk_width_p4; - } - - /*Fill four rows of the wrap-around derivative buffers*/ - for(i=top-2;i<top+2;i++) db_IxIyRow_f(Ix[i%5],Iy[i%5],img,i,left-2,chunk_width_p4); - - /*For each output row*/ - for(i=top;i<=bottom;i++) - { - /*Step the derivative buffers*/ - db_IxIyRow_f(Ix[(i+2)%5],Iy[(i+2)%5],img,(i+2),left-2,chunk_width_p4); - - /*Filter Ix2,IxIy,Iy2 vertically into gxx,gxy,gyy*/ - db_gxx_gxy_gyy_row_f(gxx,gxy,gyy,chunk_width_p4, - Ix[(i-2)%5],Ix[(i-1)%5],Ix[i%5],Ix[(i+1)%5],Ix[(i+2)%5], - Iy[(i-2)%5],Iy[(i-1)%5],Iy[i%5],Iy[(i+1)%5],Iy[(i+2)%5]); - - /*Filter gxx,gxy,gyy horizontally and compute corner response s*/ - db_HarrisStrength_row_f(s,gxx,gxy,gyy,i,left,chunk_width); - } -} - -/*Compute the Harris corner strength of the chunk [left,top,left+123,bottom] of img and -store it into the corresponding region of s. left and top have to be at least 3 and -right and bottom have to be at most width-4,height-4. The left of the region in s should -be 16 byte aligned*/ -inline void db_HarrisStrengthChunk_u(float **s,const unsigned char * const *img,int left,int top,int bottom, - /*temp should point to at least - 18*128 of allocated memory*/ - int *temp, int nc) -{ - int *Ixx[5],*Ixy[5],*Iyy[5]; - int *gxx,*gxy,*gyy; - int i; - - gxx=temp; - gxy=gxx+128; - gyy=gxy+128; - for(i=0;i<5;i++) - { - Ixx[i]=gyy+(3*i+1)*128; - Ixy[i]=gyy+(3*i+2)*128; - Iyy[i]=gyy+(3*i+3)*128; - } - - /*Fill four rows of the wrap-around derivative buffers*/ - for(i=top-2;i<top+2;i++) db_IxIyRow_u(Ixx[i%5],img,i,left-2,nc); - - /*For each output row*/ - for(i=top;i<=bottom;i++) - { - /*Step the derivative buffers*/ - db_IxIyRow_u(Ixx[(i+2)%5],img,(i+2),left-2,nc); - - /*Filter Ix2,IxIy,Iy2 vertically into gxx,gxy,gyy*/ - db_gxx_gxy_gyy_row_s(gxx,Ixx[(i-2)%5],Ixx[(i-1)%5],Ixx[i%5],Ixx[(i+1)%5],Ixx[(i+2)%5],nc); - - /*Filter gxx,gxy,gyy horizontally and compute corner response s*/ - db_HarrisStrength_row_s(s[i]+left,gxx,gxy,gyy,nc); - } - -} - -/*Compute Harris corner strength of img. Strength is returned for the region -with (3,3) as upper left and (w-4,h-4) as lower right, positioned in the -same place in s. In other words,image should be at least 7 pixels wide and 7 pixels high -for a meaningful result*/ -void db_HarrisStrength_f(float **s,const float * const *img,int w,int h, - /*temp should point to at least - 13*(chunk_width+4) of allocated memory*/ - float *temp, - int chunk_width) -{ - int x,next_x,last,right; - - last=w-4; - for(x=3;x<=last;x=next_x) - { - next_x=x+chunk_width; - right=next_x-1; - if(right>last) right=last; - /*Compute the Harris strength of a chunk*/ - db_HarrisStrengthChunk_f(s,img,x,3,right,h-4,temp); - } -} - -/*Compute Harris corner strength of img. Strength is returned for the region -with (3,3) as upper left and (w-4,h-4) as lower right, positioned in the -same place in s. In other words,image should be at least 7 pixels wide and 7 pixels high -for a meaningful result.Moreover, the image should be overallocated by 256 bytes. -s[i][3] should by 16 byte aligned for any i*/ -void db_HarrisStrength_u(float **s, const unsigned char * const *img,int w,int h, - /*temp should point to at least - 18*128 of allocated memory*/ - int *temp) -{ - int x,next_x,last; - int nc; - - last=w-4; - for(x=3;x<=last;x=next_x) - { - next_x=x+124; - - // mayban: to revert to the original full chunks state, change the line below to: nc = 128; - nc = db_mini(128,last-x+1); - //nc = 128; - - /*Compute the Harris strength of a chunk*/ - db_HarrisStrengthChunk_u(s,img,x,3,h-4,temp,nc); - } -} - -inline float db_Max_128Aligned16_f(float *v) -{ -#ifdef DB_USE_SIMD - float back; - - _asm - { - mov eax,v - - /*Chunk1*/ - movaps xmm0,[eax] - movaps xmm1,[eax+16] - movaps xmm2,[eax+32] - movaps xmm3,[eax+48] - movaps xmm4,[eax+64] - movaps xmm5,[eax+80] - movaps xmm6,[eax+96] - movaps xmm7,[eax+112] - - /*Chunk2*/ - maxps xmm0,[eax+128] - maxps xmm1,[eax+144] - maxps xmm2,[eax+160] - maxps xmm3,[eax+176] - maxps xmm4,[eax+192] - maxps xmm5,[eax+208] - maxps xmm6,[eax+224] - maxps xmm7,[eax+240] - - /*Chunk3*/ - maxps xmm0,[eax+256] - maxps xmm1,[eax+272] - maxps xmm2,[eax+288] - maxps xmm3,[eax+304] - maxps xmm4,[eax+320] - maxps xmm5,[eax+336] - maxps xmm6,[eax+352] - maxps xmm7,[eax+368] - - /*Chunk4*/ - maxps xmm0,[eax+384] - maxps xmm1,[eax+400] - maxps xmm2,[eax+416] - maxps xmm3,[eax+432] - maxps xmm4,[eax+448] - maxps xmm5,[eax+464] - maxps xmm6,[eax+480] - maxps xmm7,[eax+496] - - /*Collect*/ - maxps xmm0,xmm1 - maxps xmm2,xmm3 - maxps xmm4,xmm5 - maxps xmm6,xmm7 - maxps xmm0,xmm2 - maxps xmm4,xmm6 - maxps xmm0,xmm4 - movhlps xmm1,xmm0 - maxps xmm0,xmm1 - shufps xmm1,xmm0,1 - maxps xmm0,xmm1 - movss back,xmm0 - } - - return(back); -#else - float val,max_val; - float *p,*stop_p; - max_val=v[0]; - for(p=v+1,stop_p=v+128;p!=stop_p;) - { - val= *p++; - if(val>max_val) max_val=val; - } - return(max_val); -#endif /*DB_USE_SIMD*/ -} - -inline float db_Max_64Aligned16_f(float *v) -{ -#ifdef DB_USE_SIMD - float back; - - _asm - { - mov eax,v - - /*Chunk1*/ - movaps xmm0,[eax] - movaps xmm1,[eax+16] - movaps xmm2,[eax+32] - movaps xmm3,[eax+48] - movaps xmm4,[eax+64] - movaps xmm5,[eax+80] - movaps xmm6,[eax+96] - movaps xmm7,[eax+112] - - /*Chunk2*/ - maxps xmm0,[eax+128] - maxps xmm1,[eax+144] - maxps xmm2,[eax+160] - maxps xmm3,[eax+176] - maxps xmm4,[eax+192] - maxps xmm5,[eax+208] - maxps xmm6,[eax+224] - maxps xmm7,[eax+240] - - /*Collect*/ - maxps xmm0,xmm1 - maxps xmm2,xmm3 - maxps xmm4,xmm5 - maxps xmm6,xmm7 - maxps xmm0,xmm2 - maxps xmm4,xmm6 - maxps xmm0,xmm4 - movhlps xmm1,xmm0 - maxps xmm0,xmm1 - shufps xmm1,xmm0,1 - maxps xmm0,xmm1 - movss back,xmm0 - } - - return(back); -#else - float val,max_val; - float *p,*stop_p; - max_val=v[0]; - for(p=v+1,stop_p=v+64;p!=stop_p;) - { - val= *p++; - if(val>max_val) max_val=val; - } - return(max_val); -#endif /*DB_USE_SIMD*/ -} - -inline float db_Max_32Aligned16_f(float *v) -{ -#ifdef DB_USE_SIMD - float back; - - _asm - { - mov eax,v - - /*Chunk1*/ - movaps xmm0,[eax] - movaps xmm1,[eax+16] - movaps xmm2,[eax+32] - movaps xmm3,[eax+48] - movaps xmm4,[eax+64] - movaps xmm5,[eax+80] - movaps xmm6,[eax+96] - movaps xmm7,[eax+112] - - /*Collect*/ - maxps xmm0,xmm1 - maxps xmm2,xmm3 - maxps xmm4,xmm5 - maxps xmm6,xmm7 - maxps xmm0,xmm2 - maxps xmm4,xmm6 - maxps xmm0,xmm4 - movhlps xmm1,xmm0 - maxps xmm0,xmm1 - shufps xmm1,xmm0,1 - maxps xmm0,xmm1 - movss back,xmm0 - } - - return(back); -#else - float val,max_val; - float *p,*stop_p; - max_val=v[0]; - for(p=v+1,stop_p=v+32;p!=stop_p;) - { - val= *p++; - if(val>max_val) max_val=val; - } - return(max_val); -#endif /*DB_USE_SIMD*/ -} - -inline float db_Max_16Aligned16_f(float *v) -{ -#ifdef DB_USE_SIMD - float back; - - _asm - { - mov eax,v - - /*Chunk1*/ - movaps xmm0,[eax] - movaps xmm1,[eax+16] - movaps xmm2,[eax+32] - movaps xmm3,[eax+48] - - /*Collect*/ - maxps xmm0,xmm1 - maxps xmm2,xmm3 - maxps xmm0,xmm2 - movhlps xmm1,xmm0 - maxps xmm0,xmm1 - shufps xmm1,xmm0,1 - maxps xmm0,xmm1 - movss back,xmm0 - } - - return(back); -#else - float val,max_val; - float *p,*stop_p; - max_val=v[0]; - for(p=v+1,stop_p=v+16;p!=stop_p;) - { - val= *p++; - if(val>max_val) max_val=val; - } - return(max_val); -#endif /*DB_USE_SIMD*/ -} - -inline float db_Max_8Aligned16_f(float *v) -{ -#ifdef DB_USE_SIMD - float back; - - _asm - { - mov eax,v - - /*Chunk1*/ - movaps xmm0,[eax] - movaps xmm1,[eax+16] - - /*Collect*/ - maxps xmm0,xmm1 - movhlps xmm1,xmm0 - maxps xmm0,xmm1 - shufps xmm1,xmm0,1 - maxps xmm0,xmm1 - movss back,xmm0 - } - - return(back); -#else - float val,max_val; - float *p,*stop_p; - max_val=v[0]; - for(p=v+1,stop_p=v+8;p!=stop_p;) - { - val= *p++; - if(val>max_val) max_val=val; - } - return(max_val); -#endif /*DB_USE_SIMD*/ -} - -inline float db_Max_Aligned16_f(float *v,int size) -{ - float val,max_val; - float *stop_v; - - max_val=v[0]; - for(;size>=128;size-=128) - { - val=db_Max_128Aligned16_f(v); - v+=128; - if(val>max_val) max_val=val; - } - if(size&64) - { - val=db_Max_64Aligned16_f(v); - v+=64; - if(val>max_val) max_val=val; - } - if(size&32) - { - val=db_Max_32Aligned16_f(v); - v+=32; - if(val>max_val) max_val=val; - } - if(size&16) - { - val=db_Max_16Aligned16_f(v); - v+=16; - if(val>max_val) max_val=val; - } - if(size&8) - { - val=db_Max_8Aligned16_f(v); - v+=8; - if(val>max_val) max_val=val; - } - if(size&7) - { - for(stop_v=v+(size&7);v!=stop_v;) - { - val= *v++; - if(val>max_val) max_val=val; - } - } - - return(max_val); -} - -/*Find maximum value of img in the region starting at (left,top) -and with width w and height h. img[left] should be 16 byte aligned*/ -float db_MaxImage_Aligned16_f(float **img,int left,int top,int w,int h) -{ - float val,max_val; - int i,stop_i; - - if(w && h) - { - stop_i=top+h; - max_val=img[top][left]; - - for(i=top;i<stop_i;i++) - { - val=db_Max_Aligned16_f(img[i]+left,w); - if(val>max_val) max_val=val; - } - return(max_val); - } - return(0.0); -} - -inline void db_MaxVector_128_Aligned16_f(float *m,float *v1,float *v2) -{ -#ifdef DB_USE_SIMD - _asm - { - mov eax,v1 - mov ebx,v2 - mov ecx,m - - /*Chunk1*/ - movaps xmm0,[eax] - movaps xmm1,[eax+16] - movaps xmm2,[eax+32] - movaps xmm3,[eax+48] - movaps xmm4,[eax+64] - movaps xmm5,[eax+80] - movaps xmm6,[eax+96] - movaps xmm7,[eax+112] - maxps xmm0,[ebx] - maxps xmm1,[ebx+16] - maxps xmm2,[ebx+32] - maxps xmm3,[ebx+48] - maxps xmm4,[ebx+64] - maxps xmm5,[ebx+80] - maxps xmm6,[ebx+96] - maxps xmm7,[ebx+112] - movaps [ecx],xmm0 - movaps [ecx+16],xmm1 - movaps [ecx+32],xmm2 - movaps [ecx+48],xmm3 - movaps [ecx+64],xmm4 - movaps [ecx+80],xmm5 - movaps [ecx+96],xmm6 - movaps [ecx+112],xmm7 - - /*Chunk2*/ - movaps xmm0,[eax+128] - movaps xmm1,[eax+144] - movaps xmm2,[eax+160] - movaps xmm3,[eax+176] - movaps xmm4,[eax+192] - movaps xmm5,[eax+208] - movaps xmm6,[eax+224] - movaps xmm7,[eax+240] - maxps xmm0,[ebx+128] - maxps xmm1,[ebx+144] - maxps xmm2,[ebx+160] - maxps xmm3,[ebx+176] - maxps xmm4,[ebx+192] - maxps xmm5,[ebx+208] - maxps xmm6,[ebx+224] - maxps xmm7,[ebx+240] - movaps [ecx+128],xmm0 - movaps [ecx+144],xmm1 - movaps [ecx+160],xmm2 - movaps [ecx+176],xmm3 - movaps [ecx+192],xmm4 - movaps [ecx+208],xmm5 - movaps [ecx+224],xmm6 - movaps [ecx+240],xmm7 - - /*Chunk3*/ - movaps xmm0,[eax+256] - movaps xmm1,[eax+272] - movaps xmm2,[eax+288] - movaps xmm3,[eax+304] - movaps xmm4,[eax+320] - movaps xmm5,[eax+336] - movaps xmm6,[eax+352] - movaps xmm7,[eax+368] - maxps xmm0,[ebx+256] - maxps xmm1,[ebx+272] - maxps xmm2,[ebx+288] - maxps xmm3,[ebx+304] - maxps xmm4,[ebx+320] - maxps xmm5,[ebx+336] - maxps xmm6,[ebx+352] - maxps xmm7,[ebx+368] - movaps [ecx+256],xmm0 - movaps [ecx+272],xmm1 - movaps [ecx+288],xmm2 - movaps [ecx+304],xmm3 - movaps [ecx+320],xmm4 - movaps [ecx+336],xmm5 - movaps [ecx+352],xmm6 - movaps [ecx+368],xmm7 - - /*Chunk4*/ - movaps xmm0,[eax+384] - movaps xmm1,[eax+400] - movaps xmm2,[eax+416] - movaps xmm3,[eax+432] - movaps xmm4,[eax+448] - movaps xmm5,[eax+464] - movaps xmm6,[eax+480] - movaps xmm7,[eax+496] - maxps xmm0,[ebx+384] - maxps xmm1,[ebx+400] - maxps xmm2,[ebx+416] - maxps xmm3,[ebx+432] - maxps xmm4,[ebx+448] - maxps xmm5,[ebx+464] - maxps xmm6,[ebx+480] - maxps xmm7,[ebx+496] - movaps [ecx+384],xmm0 - movaps [ecx+400],xmm1 - movaps [ecx+416],xmm2 - movaps [ecx+432],xmm3 - movaps [ecx+448],xmm4 - movaps [ecx+464],xmm5 - movaps [ecx+480],xmm6 - movaps [ecx+496],xmm7 - } -#else - int i; - float a,b; - for(i=0;i<128;i++) - { - a=v1[i]; - b=v2[i]; - if(a>=b) m[i]=a; - else m[i]=b; - } -#endif /*DB_USE_SIMD*/ -} - -inline void db_MaxVector_128_SecondSourceDestAligned16_f(float *m,float *v1,float *v2) -{ -#ifdef DB_USE_SIMD - _asm - { - mov eax,v1 - mov ebx,v2 - mov ecx,m - - /*Chunk1*/ - movups xmm0,[eax] - movups xmm1,[eax+16] - movups xmm2,[eax+32] - movups xmm3,[eax+48] - movups xmm4,[eax+64] - movups xmm5,[eax+80] - movups xmm6,[eax+96] - movups xmm7,[eax+112] - maxps xmm0,[ebx] - maxps xmm1,[ebx+16] - maxps xmm2,[ebx+32] - maxps xmm3,[ebx+48] - maxps xmm4,[ebx+64] - maxps xmm5,[ebx+80] - maxps xmm6,[ebx+96] - maxps xmm7,[ebx+112] - movaps [ecx],xmm0 - movaps [ecx+16],xmm1 - movaps [ecx+32],xmm2 - movaps [ecx+48],xmm3 - movaps [ecx+64],xmm4 - movaps [ecx+80],xmm5 - movaps [ecx+96],xmm6 - movaps [ecx+112],xmm7 - - /*Chunk2*/ - movups xmm0,[eax+128] - movups xmm1,[eax+144] - movups xmm2,[eax+160] - movups xmm3,[eax+176] - movups xmm4,[eax+192] - movups xmm5,[eax+208] - movups xmm6,[eax+224] - movups xmm7,[eax+240] - maxps xmm0,[ebx+128] - maxps xmm1,[ebx+144] - maxps xmm2,[ebx+160] - maxps xmm3,[ebx+176] - maxps xmm4,[ebx+192] - maxps xmm5,[ebx+208] - maxps xmm6,[ebx+224] - maxps xmm7,[ebx+240] - movaps [ecx+128],xmm0 - movaps [ecx+144],xmm1 - movaps [ecx+160],xmm2 - movaps [ecx+176],xmm3 - movaps [ecx+192],xmm4 - movaps [ecx+208],xmm5 - movaps [ecx+224],xmm6 - movaps [ecx+240],xmm7 - - /*Chunk3*/ - movups xmm0,[eax+256] - movups xmm1,[eax+272] - movups xmm2,[eax+288] - movups xmm3,[eax+304] - movups xmm4,[eax+320] - movups xmm5,[eax+336] - movups xmm6,[eax+352] - movups xmm7,[eax+368] - maxps xmm0,[ebx+256] - maxps xmm1,[ebx+272] - maxps xmm2,[ebx+288] - maxps xmm3,[ebx+304] - maxps xmm4,[ebx+320] - maxps xmm5,[ebx+336] - maxps xmm6,[ebx+352] - maxps xmm7,[ebx+368] - movaps [ecx+256],xmm0 - movaps [ecx+272],xmm1 - movaps [ecx+288],xmm2 - movaps [ecx+304],xmm3 - movaps [ecx+320],xmm4 - movaps [ecx+336],xmm5 - movaps [ecx+352],xmm6 - movaps [ecx+368],xmm7 - - /*Chunk4*/ - movups xmm0,[eax+384] - movups xmm1,[eax+400] - movups xmm2,[eax+416] - movups xmm3,[eax+432] - movups xmm4,[eax+448] - movups xmm5,[eax+464] - movups xmm6,[eax+480] - movups xmm7,[eax+496] - maxps xmm0,[ebx+384] - maxps xmm1,[ebx+400] - maxps xmm2,[ebx+416] - maxps xmm3,[ebx+432] - maxps xmm4,[ebx+448] - maxps xmm5,[ebx+464] - maxps xmm6,[ebx+480] - maxps xmm7,[ebx+496] - movaps [ecx+384],xmm0 - movaps [ecx+400],xmm1 - movaps [ecx+416],xmm2 - movaps [ecx+432],xmm3 - movaps [ecx+448],xmm4 - movaps [ecx+464],xmm5 - movaps [ecx+480],xmm6 - movaps [ecx+496],xmm7 - } -#else - int i; - float a,b; - for(i=0;i<128;i++) - { - a=v1[i]; - b=v2[i]; - if(a>=b) m[i]=a; - else m[i]=b; - } -#endif /*DB_USE_SIMD*/ -} - -/*Compute Max-suppression-filtered image for a chunk of sf starting at (left,top), of width 124 and -stopping at bottom. The output is shifted two steps left and overwrites 128 elements for each row. -The input s should be of width at least 128, and exist for 2 pixels outside the specified region. -s[i][left-2] and sf[i][left-2] should be 16 byte aligned. Top must be at least 3*/ -inline void db_MaxSuppressFilterChunk_5x5_Aligned16_f(float **sf,float **s,int left,int top,int bottom, - /*temp should point to at least - 6*132 floats of 16-byte-aligned allocated memory*/ - float *temp) -{ -#ifdef DB_USE_SIMD - int i,lm2; - float *two[4]; - float *four,*five; - - lm2=left-2; - - /*Set pointers to pre-allocated memory*/ - four=temp; - five=four+132; - for(i=0;i<4;i++) - { - two[i]=five+(i+1)*132; - } - - /*Set rests of four and five to zero to avoid - floating point exceptions*/ - for(i=129;i<132;i++) - { - four[i]=0.0; - five[i]=0.0; - } - - /*Fill three rows of the wrap-around max buffers*/ - for(i=top-3;i<top;i++) db_MaxVector_128_Aligned16_f(two[i&3],s[i+1]+lm2,s[i+2]+lm2); - - /*For each output row*/ - for(;i<=bottom;i++) - { - /*Compute max of the lowest pair of rows in the five row window*/ - db_MaxVector_128_Aligned16_f(two[i&3],s[i+1]+lm2,s[i+2]+lm2); - /*Compute max of the lowest and highest pair of rows in the five row window*/ - db_MaxVector_128_Aligned16_f(four,two[i&3],two[(i-3)&3]); - /*Compute max of all rows*/ - db_MaxVector_128_Aligned16_f(five,four,two[(i-1)&3]); - /*Compute max of 2x5 chunks*/ - db_MaxVector_128_SecondSourceDestAligned16_f(five,five+1,five); - /*Compute max of pairs of 2x5 chunks*/ - db_MaxVector_128_SecondSourceDestAligned16_f(five,five+3,five); - /*Compute max of pairs of 5x5 except middle*/ - db_MaxVector_128_SecondSourceDestAligned16_f(sf[i]+lm2,four+2,five); - } - -#else - int i,j,right; - float sv; - - right=left+128; - for(i=top;i<=bottom;i++) for(j=left;j<right;j++) - { - sv=s[i][j]; - - if( sv>s[i-2][j-2] && sv>s[i-2][j-1] && sv>s[i-2][j] && sv>s[i-2][j+1] && sv>s[i-2][j+2] && - sv>s[i-1][j-2] && sv>s[i-1][j-1] && sv>s[i-1][j] && sv>s[i-1][j+1] && sv>s[i-1][j+2] && - sv>s[ i][j-2] && sv>s[ i][j-1] && sv>s[ i][j+1] && sv>s[ i][j+2] && - sv>s[i+1][j-2] && sv>s[i+1][j-1] && sv>s[i+1][j] && sv>s[i+1][j+1] && sv>s[i+1][j+2] && - sv>s[i+2][j-2] && sv>s[i+2][j-1] && sv>s[i+2][j] && sv>s[i+2][j+1] && sv>s[i+2][j+2]) - { - sf[i][j-2]=0.0; - } - else sf[i][j-2]=sv; - } -#endif /*DB_USE_SIMD*/ -} - -/*Compute Max-suppression-filtered image for a chunk of sf starting at (left,top) and -stopping at bottom. The output is shifted two steps left. The input s should exist for 2 pixels -outside the specified region. s[i][left-2] and sf[i][left-2] should be 16 byte aligned. -Top must be at least 3. Reading and writing from and to the input and output images is done -as if the region had a width equal to a multiple of 124. If this is not the case, the images -should be over-allocated and the input cleared for a sufficient region*/ -void db_MaxSuppressFilter_5x5_Aligned16_f(float **sf,float **s,int left,int top,int right,int bottom, - /*temp should point to at least - 6*132 floats of 16-byte-aligned allocated memory*/ - float *temp) -{ - int x,next_x; - - for(x=left;x<=right;x=next_x) - { - next_x=x+124; - db_MaxSuppressFilterChunk_5x5_Aligned16_f(sf,s,x,top,bottom,temp); - } -} - -/*Extract corners from the chunk (left,top) to (right,bottom). Store in x_temp,y_temp and s_temp -which should point to space of at least as many positions as there are pixels in the chunk*/ -inline int db_CornersFromChunk(float **strength,int left,int top,int right,int bottom,float threshold,double *x_temp,double *y_temp,double *s_temp) -{ - int i,j,nr; - float s; - - nr=0; - for(i=top;i<=bottom;i++) for(j=left;j<=right;j++) - { - s=strength[i][j]; - - if(s>=threshold && - s>strength[i-2][j-2] && s>strength[i-2][j-1] && s>strength[i-2][j] && s>strength[i-2][j+1] && s>strength[i-2][j+2] && - s>strength[i-1][j-2] && s>strength[i-1][j-1] && s>strength[i-1][j] && s>strength[i-1][j+1] && s>strength[i-1][j+2] && - s>strength[ i][j-2] && s>strength[ i][j-1] && s>strength[ i][j+1] && s>strength[ i][j+2] && - s>strength[i+1][j-2] && s>strength[i+1][j-1] && s>strength[i+1][j] && s>strength[i+1][j+1] && s>strength[i+1][j+2] && - s>strength[i+2][j-2] && s>strength[i+2][j-1] && s>strength[i+2][j] && s>strength[i+2][j+1] && s>strength[i+2][j+2]) - { - x_temp[nr]=(double) j; - y_temp[nr]=(double) i; - s_temp[nr]=(double) s; - nr++; - } - } - return(nr); -} - - -//Sub-pixel accuracy using 2D quadratic interpolation.(YCJ) -inline void db_SubPixel(float **strength, const double xd, const double yd, double &xs, double &ys) -{ - int x = (int) xd; - int y = (int) yd; - - float fxx = strength[y][x-1] - strength[y][x] - strength[y][x] + strength[y][x+1]; - float fyy = strength[y-1][x] - strength[y][x] - strength[y][x] + strength[y+1][x]; - float fxy = (strength[y-1][x-1] - strength[y-1][x+1] - strength[y+1][x-1] + strength[y+1][x+1])/(float)4.0; - - float denom = (fxx * fyy - fxy * fxy) * (float) 2.0; - - xs = xd; - ys = yd; - - if ( db_absf(denom) <= FLT_EPSILON ) - { - return; - } - else - { - float fx = strength[y][x+1] - strength[y][x-1]; - float fy = strength[y+1][x] - strength[y-1][x]; - - float dx = (fyy * fx - fxy * fy) / denom; - float dy = (fxx * fy - fxy * fx) / denom; - - if ( db_absf(dx) > 1.0 || db_absf(dy) > 1.0 ) - { - return; - } - else - { - xs -= dx; - ys -= dy; - } - } - - return; -} - -/*Extract corners from the image part from (left,top) to (right,bottom). -Store in x and y, extracting at most satnr corners in each block of size (bw,bh). -The pointer temp_d should point to at least 5*bw*bh positions. -area_factor holds how many corners max to extract per 10000 pixels*/ -void db_ExtractCornersSaturated(float **strength,int left,int top,int right,int bottom, - int bw,int bh,unsigned long area_factor, - float threshold,double *temp_d, - double *x_coord,double *y_coord,int *nr_corners) -{ - double *x_temp,*y_temp,*s_temp,*select_temp; - double loc_thresh; - unsigned long bwbh,area,saturation; - int x,next_x,last_x; - int y,next_y,last_y; - int nr,nr_points,i,stop; - - bwbh=bw*bh; - x_temp=temp_d; - y_temp=x_temp+bwbh; - s_temp=y_temp+bwbh; - select_temp=s_temp+bwbh; - -#ifdef DB_SUB_PIXEL - // subpixel processing may sometimes push the corner ourside the real border - // increasing border size: - left++; - top++; - bottom--; - right--; -#endif /*DB_SUB_PIXEL*/ - - nr_points=0; - for(y=top;y<=bottom;y=next_y) - { - next_y=y+bh; - last_y=next_y-1; - if(last_y>bottom) last_y=bottom; - for(x=left;x<=right;x=next_x) - { - next_x=x+bw; - last_x=next_x-1; - if(last_x>right) last_x=right; - - area=(last_x-x+1)*(last_y-y+1); - saturation=(area*area_factor)/10000; - nr=db_CornersFromChunk(strength,x,y,last_x,last_y,threshold,x_temp,y_temp,s_temp); - if(nr) - { - if(((unsigned long)nr)>saturation) loc_thresh=db_LeanQuickSelect(s_temp,nr,nr-saturation,select_temp); - else loc_thresh=threshold; - - stop=nr_points+saturation; - for(i=0;(i<nr)&&(nr_points<stop);i++) - { - if(s_temp[i]>=loc_thresh) - { - #ifdef DB_SUB_PIXEL - db_SubPixel(strength, x_temp[i], y_temp[i], x_coord[nr_points], y_coord[nr_points]); - #else - x_coord[nr_points]=x_temp[i]; - y_coord[nr_points]=y_temp[i]; - #endif - - nr_points++; - } - } - } - } - } - *nr_corners=nr_points; -} - -db_CornerDetector_f::db_CornerDetector_f() -{ - m_w=0; m_h=0; -} - -db_CornerDetector_f::~db_CornerDetector_f() -{ - Clean(); -} - -void db_CornerDetector_f::Clean() -{ - if(m_w!=0) - { - delete [] m_temp_f; - delete [] m_temp_d; - db_FreeStrengthImage_f(m_strength_mem,m_strength,m_h); - } - m_w=0; m_h=0; -} - -unsigned long db_CornerDetector_f::Init(int im_width,int im_height,int target_nr_corners, - int nr_horizontal_blocks,int nr_vertical_blocks, - double absolute_threshold,double relative_threshold) -{ - int chunkwidth=208; - int block_width,block_height; - unsigned long area_factor; - int active_width,active_height; - - active_width=db_maxi(1,im_width-10); - active_height=db_maxi(1,im_height-10); - block_width=db_maxi(1,active_width/nr_horizontal_blocks); - block_height=db_maxi(1,active_height/nr_vertical_blocks); - - area_factor=db_minl(1000,db_maxl(1,(long)(10000.0*((double)target_nr_corners)/ - (((double)active_width)*((double)active_height))))); - - return(Start(im_width,im_height,block_width,block_height,area_factor, - absolute_threshold,relative_threshold,chunkwidth)); -} - -unsigned long db_CornerDetector_f::Start(int im_width,int im_height, - int block_width,int block_height,unsigned long area_factor, - double absolute_threshold,double relative_threshold,int chunkwidth) -{ - Clean(); - - m_w=im_width; - m_h=im_height; - m_cw=chunkwidth; - m_bw=block_width; - m_bh=block_height; - m_area_factor=area_factor; - m_r_thresh=relative_threshold; - m_a_thresh=absolute_threshold; - m_max_nr=db_maxl(1,1+(m_w*m_h*m_area_factor)/10000); - - m_temp_f=new float[13*(m_cw+4)]; - m_temp_d=new double[5*m_bw*m_bh]; - m_strength=db_AllocStrengthImage_f(&m_strength_mem,m_w,m_h); - - return(m_max_nr); -} - -void db_CornerDetector_f::DetectCorners(const float * const *img,double *x_coord,double *y_coord,int *nr_corners) const -{ - float max_val,threshold; - - db_HarrisStrength_f(m_strength,img,m_w,m_h,m_temp_f,m_cw); - - if(m_r_thresh) - { - max_val=db_MaxImage_Aligned16_f(m_strength,3,3,m_w-6,m_h-6); - threshold= (float) db_maxd(m_a_thresh,max_val*m_r_thresh); - } - else threshold= (float) m_a_thresh; - - db_ExtractCornersSaturated(m_strength,BORDER,BORDER,m_w-BORDER-1,m_h-BORDER-1,m_bw,m_bh,m_area_factor,threshold, - m_temp_d,x_coord,y_coord,nr_corners); -} - -db_CornerDetector_u::db_CornerDetector_u() -{ - m_w=0; m_h=0; -} - -db_CornerDetector_u::~db_CornerDetector_u() -{ - Clean(); -} - -db_CornerDetector_u::db_CornerDetector_u(const db_CornerDetector_u& cd) -{ - Start(cd.m_w, cd.m_h, cd.m_bw, cd.m_bh, cd.m_area_factor, - cd.m_a_thresh, cd.m_r_thresh); -} - -db_CornerDetector_u& db_CornerDetector_u::operator=(const db_CornerDetector_u& cd) -{ - if ( this == &cd ) return *this; - - Clean(); - - Start(cd.m_w, cd.m_h, cd.m_bw, cd.m_bh, cd.m_area_factor, - cd.m_a_thresh, cd.m_r_thresh); - - return *this; -} - -void db_CornerDetector_u::Clean() -{ - if(m_w!=0) - { - delete [] m_temp_i; - delete [] m_temp_d; - db_FreeStrengthImage_f(m_strength_mem,m_strength,m_h); - } - m_w=0; m_h=0; -} - -unsigned long db_CornerDetector_u::Init(int im_width,int im_height,int target_nr_corners, - int nr_horizontal_blocks,int nr_vertical_blocks, - double absolute_threshold,double relative_threshold) -{ - int block_width,block_height; - unsigned long area_factor; - int active_width,active_height; - - active_width=db_maxi(1,im_width-10); - active_height=db_maxi(1,im_height-10); - block_width=db_maxi(1,active_width/nr_horizontal_blocks); - block_height=db_maxi(1,active_height/nr_vertical_blocks); - - area_factor=db_minl(1000,db_maxl(1,(long)(10000.0*((double)target_nr_corners)/ - (((double)active_width)*((double)active_height))))); - - return(Start(im_width,im_height,block_width,block_height,area_factor, - 16.0*absolute_threshold,relative_threshold)); -} - -unsigned long db_CornerDetector_u::Start(int im_width,int im_height, - int block_width,int block_height,unsigned long area_factor, - double absolute_threshold,double relative_threshold) -{ - Clean(); - - m_w=im_width; - m_h=im_height; - m_bw=block_width; - m_bh=block_height; - m_area_factor=area_factor; - m_r_thresh=relative_threshold; - m_a_thresh=absolute_threshold; - m_max_nr=db_maxl(1,1+(m_w*m_h*m_area_factor)/10000); - - m_temp_i=new int[18*128]; - m_temp_d=new double[5*m_bw*m_bh]; - m_strength=db_AllocStrengthImage_f(&m_strength_mem,m_w,m_h); - - return(m_max_nr); -} - -void db_CornerDetector_u::DetectCorners(const unsigned char * const *img,double *x_coord,double *y_coord,int *nr_corners, - const unsigned char * const *msk, unsigned char fgnd) const -{ - float max_val,threshold; - - db_HarrisStrength_u(m_strength,img,m_w,m_h,m_temp_i); - - - if(m_r_thresh) - { - max_val=db_MaxImage_Aligned16_f(m_strength,3,3,m_w-6,m_h-6); - threshold= (float) db_maxd(m_a_thresh,max_val*m_r_thresh); - } - else threshold= (float) m_a_thresh; - - db_ExtractCornersSaturated(m_strength,BORDER,BORDER,m_w-BORDER-1,m_h-BORDER-1,m_bw,m_bh,m_area_factor,threshold, - m_temp_d,x_coord,y_coord,nr_corners); - - - if ( msk ) - { - int nr_corners_mask=0; - - for ( int i = 0; i < *nr_corners; ++i) - { - int cor_x = db_roundi(*(x_coord+i)); - int cor_y = db_roundi(*(y_coord+i)); - if ( msk[cor_y][cor_x] == fgnd ) - { - x_coord[nr_corners_mask] = x_coord[i]; - y_coord[nr_corners_mask] = y_coord[i]; - nr_corners_mask++; - } - } - *nr_corners = nr_corners_mask; - } -} - -void db_CornerDetector_u::ExtractCorners(float ** strength, double *x_coord, double *y_coord, int *nr_corners) { - if ( m_w!=0 ) - db_ExtractCornersSaturated(strength,BORDER,BORDER,m_w-BORDER-1,m_h-BORDER-1,m_bw,m_bh,m_area_factor,float(m_a_thresh), - m_temp_d,x_coord,y_coord,nr_corners); -} - diff --git a/jni/feature_stab/db_vlvm/db_feature_detection.h b/jni/feature_stab/db_vlvm/db_feature_detection.h deleted file mode 100644 index 68ffcc9ad..000000000 --- a/jni/feature_stab/db_vlvm/db_feature_detection.h +++ /dev/null @@ -1,179 +0,0 @@ -/* - * 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. - */ - -/*$Id: db_feature_detection.h,v 1.3 2011/06/17 14:03:30 mbansal Exp $*/ - -#ifndef DB_FEATURE_DETECTION_H -#define DB_FEATURE_DETECTION_H - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ -/*! - * \defgroup FeatureDetection Feature Detection - */ -#include "db_utilities.h" -#include "db_utilities_constants.h" -#include <stdlib.h> //for NULL - -/*! - * \class db_CornerDetector_f - * \ingroup FeatureDetection - * \brief Harris corner detector for float images. - * - * This class performs Harris corner extraction on *float* images managed - * with functions in \ref LMImageBasicUtilities. - */ -class DB_API db_CornerDetector_f -{ -public: - db_CornerDetector_f(); - ~db_CornerDetector_f(); - - /*! - * Set parameters and pre-allocate memory. Return an upper bound - * on the number of corners detected in one frame. - * \param im_width width - * \param im_height height - * \param target_nr_corners - * \param nr_horizontal_blocks - * \param nr_vertical_blocks - * \param absolute_threshold - * \param relative_threshold - */ - unsigned long Init(int im_width,int im_height, - int target_nr_corners=DB_DEFAULT_TARGET_NR_CORNERS, - int nr_horizontal_blocks=DB_DEFAULT_NR_FEATURE_BLOCKS, - int nr_vertical_blocks=DB_DEFAULT_NR_FEATURE_BLOCKS, - double absolute_threshold=DB_DEFAULT_ABS_CORNER_THRESHOLD, - double relative_threshold=DB_DEFAULT_REL_CORNER_THRESHOLD); - - /*! - * Detect the corners. - * x_coord and y_coord should be pre-allocated arrays of length returned by Init(). - * \param img row array pointer - * \param x_coord corner locations - * \param y_coord corner locations - * \param nr_corners actual number of corners computed - */ - void DetectCorners(const float * const *img,double *x_coord,double *y_coord,int *nr_corners) const; - void SetAbsoluteThreshold(double a_thresh) { m_a_thresh = a_thresh; }; - void SetRelativeThreshold(double r_thresh) { m_r_thresh = r_thresh; }; -protected: - void Clean(); - unsigned long Start(int im_width,int im_height, - int block_width,int block_height,unsigned long area_factor, - double absolute_threshold,double relative_threshold,int chunkwidth); - - int m_w,m_h,m_cw,m_bw,m_bh; - /*Area factor holds the maximum number of corners to detect - per 10000 pixels*/ - unsigned long m_area_factor,m_max_nr; - double m_a_thresh,m_r_thresh; - float *m_temp_f; - double *m_temp_d; - float **m_strength,*m_strength_mem; -}; -/*! - * \class db_CornerDetector_u - * \ingroup FeatureDetection - * \brief Harris corner detector for byte images. - * - * This class performs Harris corner extraction on *byte* images managed - * with functions in \ref LMImageBasicUtilities. - */ -class DB_API db_CornerDetector_u -{ -public: - db_CornerDetector_u(); - virtual ~db_CornerDetector_u(); - - /*! - Copy ctor duplicates settings. - Memory is not copied. - */ - db_CornerDetector_u(const db_CornerDetector_u& cd); - /*! - Assignment optor duplicates settings. - Memory not copied. - */ - db_CornerDetector_u& operator=(const db_CornerDetector_u& cd); - - /*! - * Set parameters and pre-allocate memory. Return an upper bound - * on the number of corners detected in one frame - */ - virtual unsigned long Init(int im_width,int im_height, - int target_nr_corners=DB_DEFAULT_TARGET_NR_CORNERS, - int nr_horizontal_blocks=DB_DEFAULT_NR_FEATURE_BLOCKS, - int nr_vertical_blocks=DB_DEFAULT_NR_FEATURE_BLOCKS, - double absolute_threshold=DB_DEFAULT_ABS_CORNER_THRESHOLD, - double relative_threshold=DB_DEFAULT_REL_CORNER_THRESHOLD); - - /*! - * Detect the corners. - * Observe that the image should be overallocated by at least 256 bytes - * at the end. - * x_coord and y_coord should be pre-allocated arrays of length returned by Init(). - * Specifying image mask will restrict corner output to foreground regions. - * Foreground value can be specified using fgnd. By default any >0 mask value - * is considered to be foreground - * \param img row array pointer - * \param x_coord corner locations - * \param y_coord corner locations - * \param nr_corners actual number of corners computed - * \param msk row array pointer to mask image - * \param fgnd foreground value in the mask - */ - virtual void DetectCorners(const unsigned char * const *img,double *x_coord,double *y_coord,int *nr_corners, - const unsigned char * const * msk=NULL, unsigned char fgnd=255) const; - - /*! - Set absolute feature threshold - */ - virtual void SetAbsoluteThreshold(double a_thresh) { m_a_thresh = a_thresh; }; - /*! - Set relative feature threshold - */ - virtual void SetRelativeThreshold(double r_thresh) { m_r_thresh = r_thresh; }; - - /*! - Extract corners from a pre-computed strength image. - \param strength Harris strength image - \param x_coord corner locations - \param y_coord corner locations - \param nr_corners actual number of corners computed - */ - virtual void ExtractCorners(float ** strength, double *x_coord, double *y_coord, int *nr_corners); -protected: - virtual void Clean(); - /*The absolute threshold to this function should be 16.0 times - normal*/ - unsigned long Start(int im_width,int im_height, - int block_width,int block_height,unsigned long area_factor, - double absolute_threshold,double relative_threshold); - - int m_w,m_h,m_bw,m_bh; - /*Area factor holds the maximum number of corners to detect - per 10000 pixels*/ - unsigned long m_area_factor,m_max_nr; - double m_a_thresh,m_r_thresh; - int *m_temp_i; - double *m_temp_d; - float **m_strength,*m_strength_mem; -}; - -#endif /*DB_FEATURE_DETECTION_H*/ diff --git a/jni/feature_stab/db_vlvm/db_feature_matching.cpp b/jni/feature_stab/db_vlvm/db_feature_matching.cpp deleted file mode 100644 index d278d0cf6..000000000 --- a/jni/feature_stab/db_vlvm/db_feature_matching.cpp +++ /dev/null @@ -1,3410 +0,0 @@ -/* - * 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. - */ - -/*$Id: db_feature_matching.cpp,v 1.4 2011/06/17 14:03:30 mbansal Exp $*/ - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ - -#include "db_utilities.h" -#include "db_feature_matching.h" -#ifdef _VERBOSE_ -#include <iostream> -#endif - - -int AffineWarpPoint_NN_LUT_x[11][11]; -int AffineWarpPoint_NN_LUT_y[11][11]; - -float AffineWarpPoint_BL_LUT_x[11][11]; -float AffineWarpPoint_BL_LUT_y[11][11]; - - -inline float db_SignedSquareNormCorr7x7_u(unsigned char **f_img,unsigned char **g_img,int x_f,int y_f,int x_g,int y_g) -{ - unsigned char *pf,*pg; - float f,g,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den; - int xm_f,xm_g; - - xm_f=x_f-3; - xm_g=x_g-3; - fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0; - - pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - fg_corr=49.0f*fgsum-fsum*gsum; - den=(49.0f*f2sum-fsum*fsum)*(49.0f*g2sum-gsum*gsum); - if(den!=0.0) - { - if(fg_corr>=0.0) return(fg_corr*fg_corr/den); - return(-fg_corr*fg_corr/den); - } - return(0.0); -} - -inline float db_SignedSquareNormCorr9x9_u(unsigned char **f_img,unsigned char **g_img,int x_f,int y_f,int x_g,int y_g) -{ - unsigned char *pf,*pg; - float f,g,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den; - int xm_f,xm_g; - - xm_f=x_f-4; - xm_g=x_g-4; - fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0; - - pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - fg_corr=81.0f*fgsum-fsum*gsum; - den=(81.0f*f2sum-fsum*fsum)*(81.0f*g2sum-gsum*gsum); - if(den!=0.0) - { - if(fg_corr>=0.0) return(fg_corr*fg_corr/den); - return(-fg_corr*fg_corr/den); - } - return(0.0); -} - -inline float db_SignedSquareNormCorr11x11_u(unsigned char **f_img,unsigned char **g_img,int x_f,int y_f,int x_g,int y_g) -{ - unsigned char *pf,*pg; - float f,g,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den; - int xm_f,xm_g; - - xm_f=x_f-5; - xm_g=x_g-5; - fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0; - - pf=f_img[y_f-5]+xm_f; pg=g_img[y_g-5]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+5]+xm_f; pg=g_img[y_g+5]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - fg_corr=121.0f*fgsum-fsum*gsum; - den=(121.0f*f2sum-fsum*fsum)*(121.0f*g2sum-gsum*gsum); - if(den!=0.0) - { - if(fg_corr>=0.0) return(fg_corr*fg_corr/den); - return(-fg_corr*fg_corr/den); - } - return(0.0); -} - -inline void db_SignedSquareNormCorr11x11_Pre_u(unsigned char **f_img,int x_f,int y_f,float *sum,float *recip) -{ - unsigned char *pf; - float den; - int f,f2sum,fsum; - int xm_f; - - xm_f=x_f-5; - - pf=f_img[y_f-5]+xm_f; - f= *pf++; f2sum=f*f; fsum=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f-4]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f-3]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f-2]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f-1]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f+1]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f+2]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f+3]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f+4]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f+5]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - *sum= (float) fsum; - den=(121.0f*f2sum-fsum*fsum); - *recip=(float)(((den!=0.0)?1.0/den:0.0)); -} - -inline void db_SignedSquareNormCorr5x5_PreAlign_u(short *patch,const unsigned char * const *f_img,int x_f,int y_f,float *sum,float *recip) -{ - float den; - int f2sum,fsum; - int xm_f=x_f-2; - -#ifndef DB_USE_SSE2 - const unsigned char *pf; - short f; - - pf=f_img[y_f-2]+xm_f; - f= *pf++; f2sum=f*f; fsum=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f-1]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f+1]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f+2]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - //int xwi; - //int ywi; - //f2sum=0; - //fsum=0; - //for (int r=-5;r<=5;r++){ - // ywi=y_f+r; - // for (int c=-5;c<=5;c++){ - // xwi=x_f+c; - // f=f_img[ywi][xwi]; - // f2sum+=f*f; - // fsum+=f; - // (*patch++)=f; - // } - //} - (*patch++)=0; (*patch++)=0; (*patch++)=0; (*patch++)=0; (*patch++)=0; - (*patch++)=0; (*patch++)=0; -#endif /* DB_USE_SSE2 */ - - *sum= (float) fsum; - den=(25.0f*f2sum-fsum*fsum); - *recip= (float)((den!=0.0)?1.0/den:0.0); -} - -inline void db_SignedSquareNormCorr21x21_PreAlign_u(short *patch,const unsigned char * const *f_img,int x_f,int y_f,float *sum,float *recip) -{ - float den; - int f2sum,fsum; - int xm_f=x_f-10; - short f; - - int xwi; - int ywi; - f2sum=0; - fsum=0; - for (int r=-10;r<=10;r++){ - ywi=y_f+r; - for (int c=-10;c<=10;c++){ - xwi=x_f+c; - f=f_img[ywi][xwi]; - f2sum+=f*f; - fsum+=f; - (*patch++)=f; - } - } - - for(int i=442; i<512; i++) - (*patch++)=0; - - *sum= (float) fsum; - den=(441.0f*f2sum-fsum*fsum); - *recip= (float)((den!=0.0)?1.0/den:0.0); - - -} - -/* Lay out the image in the patch, computing norm and -*/ -inline void db_SignedSquareNormCorr11x11_PreAlign_u(short *patch,const unsigned char * const *f_img,int x_f,int y_f,float *sum,float *recip) -{ - float den; - int f2sum,fsum; - int xm_f=x_f-5; - -#ifndef DB_USE_SSE2 - const unsigned char *pf; - short f; - - pf=f_img[y_f-5]+xm_f; - f= *pf++; f2sum=f*f; fsum=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f-4]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f-3]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f-2]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f-1]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f+1]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f+2]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f+3]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f+4]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f+5]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - //int xwi; - //int ywi; - //f2sum=0; - //fsum=0; - //for (int r=-5;r<=5;r++){ - // ywi=y_f+r; - // for (int c=-5;c<=5;c++){ - // xwi=x_f+c; - // f=f_img[ywi][xwi]; - // f2sum+=f*f; - // fsum+=f; - // (*patch++)=f; - // } - //} - - (*patch++)=0; (*patch++)=0; (*patch++)=0; (*patch++)=0; (*patch++)=0; - (*patch++)=0; (*patch++)=0; -#else - const unsigned char *pf0 =f_img[y_f-5]+xm_f; - const unsigned char *pf1 =f_img[y_f-4]+xm_f; - const unsigned char *pf2 =f_img[y_f-3]+xm_f; - const unsigned char *pf3 =f_img[y_f-2]+xm_f; - const unsigned char *pf4 =f_img[y_f-1]+xm_f; - const unsigned char *pf5 =f_img[y_f ]+xm_f; - const unsigned char *pf6 =f_img[y_f+1]+xm_f; - const unsigned char *pf7 =f_img[y_f+2]+xm_f; - const unsigned char *pf8 =f_img[y_f+3]+xm_f; - const unsigned char *pf9 =f_img[y_f+4]+xm_f; - const unsigned char *pf10=f_img[y_f+5]+xm_f; - - /* pixel mask */ - const unsigned char pm[16] = { - 0xFF,0xFF, - 0xFF,0xFF, - 0xFF,0xFF, - 0,0,0,0,0, - 0,0,0,0,0}; - const unsigned char * pm_p = pm; - - _asm - { - mov ecx,patch /* load patch pointer */ - mov ebx, pm_p /* load pixel mask pointer */ - movdqu xmm1,[ebx] /* load pixel mask */ - - pxor xmm5,xmm5 /* set xmm5 to 0 accumulator for sum squares */ - pxor xmm4,xmm4 /* set xmm4 to 0 accumulator for sum */ - pxor xmm0,xmm0 /* set xmm0 to 0 */ - - /* row 0 */ - mov eax,pf0 /* load image pointer */ - movdqu xmm7,[eax] /* load 16 pixels */ - movdqa xmm6,xmm7 - - punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/ - punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/ - - pand xmm6,xmm1 /* mask out pixels 12-16 */ - - movdqa [ecx+0*22],xmm7 /* move short values to patch */ - movdqa [ecx+0*22+16],xmm6 /* move short values to patch */ - - paddusw xmm4,xmm7 /* accumulate sums */ - pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */ - paddd xmm5,xmm7 /* accumulate sum squares */ - - paddw xmm4,xmm6 /* accumulate sums */ - pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */ - paddd xmm5,xmm6 /* accumulate sum squares */ - - /* row 1 */ - mov eax,pf1 /* load image pointer */ - movdqu xmm7,[eax] /* load 16 pixels */ - movdqa xmm6,xmm7 - - punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/ - punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/ - - pand xmm6,xmm1 /* mask out pixels 12-16 */ - - movdqu [ecx+1*22],xmm7 /* move short values to patch */ - movdqu [ecx+1*22+16],xmm6 /* move short values to patch */ - - paddusw xmm4,xmm7 /* accumulate sums */ - pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */ - paddd xmm5,xmm7 /* accumulate sum squares */ - - paddw xmm4,xmm6 /* accumulate sums */ - pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */ - paddd xmm5,xmm6 /* accumulate sum squares */ - - /* row 2 */ - mov eax,pf2 /* load image pointer */ - movdqu xmm7,[eax] /* load 16 pixels */ - movdqa xmm6,xmm7 - - punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/ - punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/ - - pand xmm6,xmm1 /* mask out pixels 12-16 */ - - movdqu [ecx+2*22],xmm7 /* move short values to patch */ - movdqu [ecx+2*22+16],xmm6 /* move short values to patch */ - - paddusw xmm4,xmm7 /* accumulate sums */ - pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */ - paddd xmm5,xmm7 /* accumulate sum squares */ - - paddw xmm4,xmm6 /* accumulate sums */ - pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */ - paddd xmm5,xmm6 /* accumulate sum squares */ - - /* row 3 */ - mov eax,pf3 /* load image pointer */ - movdqu xmm7,[eax] /* load 16 pixels */ - movdqa xmm6,xmm7 - - punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/ - punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/ - - pand xmm6,xmm1 /* mask out pixels 12-16 */ - - movdqu [ecx+3*22],xmm7 /* move short values to patch */ - movdqu [ecx+3*22+16],xmm6 /* move short values to patch */ - - paddusw xmm4,xmm7 /* accumulate sums */ - pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */ - paddd xmm5,xmm7 /* accumulate sum squares */ - - paddw xmm4,xmm6 /* accumulate sums */ - pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */ - paddd xmm5,xmm6 /* accumulate sum squares */ - - /* row 4 */ - mov eax,pf4 /* load image pointer */ - movdqu xmm7,[eax] /* load 16 pixels */ - movdqa xmm6,xmm7 - - punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/ - punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/ - - pand xmm6,xmm1 /* mask out pixels 12-16 */ - - movdqu [ecx+4*22],xmm7 /* move short values to patch */ - movdqu [ecx+4*22+16],xmm6 /* move short values to patch */ - - paddusw xmm4,xmm7 /* accumulate sums */ - pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */ - paddd xmm5,xmm7 /* accumulate sum squares */ - - paddw xmm4,xmm6 /* accumulate sums */ - pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */ - paddd xmm5,xmm6 /* accumulate sum squares */ - - /* row 5 */ - mov eax,pf5 /* load image pointer */ - movdqu xmm7,[eax] /* load 16 pixels */ - movdqa xmm6,xmm7 - - punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/ - punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/ - - pand xmm6,xmm1 /* mask out pixels 12-16 */ - - movdqu [ecx+5*22],xmm7 /* move short values to patch */ - movdqu [ecx+5*22+16],xmm6 /* move short values to patch */ - - paddusw xmm4,xmm7 /* accumulate sums */ - pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */ - paddd xmm5,xmm7 /* accumulate sum squares */ - - paddw xmm4,xmm6 /* accumulate sums */ - pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */ - paddd xmm5,xmm6 /* accumulate sum squares */ - - /* row 6 */ - mov eax,pf6 /* load image pointer */ - movdqu xmm7,[eax] /* load 16 pixels */ - movdqa xmm6,xmm7 - - punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/ - punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/ - - pand xmm6,xmm1 /* mask out pixels 12-16 */ - - movdqu [ecx+6*22],xmm7 /* move short values to patch */ - movdqu [ecx+6*22+16],xmm6 /* move short values to patch */ - - paddusw xmm4,xmm7 /* accumulate sums */ - pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */ - paddd xmm5,xmm7 /* accumulate sum squares */ - - paddw xmm4,xmm6 /* accumulate sums */ - pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */ - paddd xmm5,xmm6 /* accumulate sum squares */ - - /* row 7 */ - mov eax,pf7 /* load image pointer */ - movdqu xmm7,[eax] /* load 16 pixels */ - movdqa xmm6,xmm7 - - punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/ - punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/ - - pand xmm6,xmm1 /* mask out pixels 12-16 */ - - movdqu [ecx+7*22],xmm7 /* move short values to patch */ - movdqu [ecx+7*22+16],xmm6 /* move short values to patch */ - - paddusw xmm4,xmm7 /* accumulate sums */ - pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */ - paddd xmm5,xmm7 /* accumulate sum squares */ - - paddw xmm4,xmm6 /* accumulate sums */ - pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */ - paddd xmm5,xmm6 /* accumulate sum squares */ - - /* row 8 */ - mov eax,pf8 /* load image pointer */ - movdqu xmm7,[eax] /* load 16 pixels */ - movdqa xmm6,xmm7 - - punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/ - punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/ - - pand xmm6,xmm1 /* mask out pixels 12-16 */ - - movdqa [ecx+8*22],xmm7 /* move short values to patch */ - movdqa [ecx+8*22+16],xmm6 /* move short values to patch */ - - paddusw xmm4,xmm7 /* accumulate sums */ - pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */ - paddd xmm5,xmm7 /* accumulate sum squares */ - - paddw xmm4,xmm6 /* accumulate sums */ - pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */ - paddd xmm5,xmm6 /* accumulate sum squares */ - - /* row 9 */ - mov eax,pf9 /* load image pointer */ - movdqu xmm7,[eax] /* load 16 pixels */ - movdqa xmm6,xmm7 - - punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/ - punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/ - - pand xmm6,xmm1 /* mask out pixels 12-16 */ - - movdqu [ecx+9*22],xmm7 /* move short values to patch */ - movdqu [ecx+9*22+16],xmm6 /* move short values to patch */ - - paddusw xmm4,xmm7 /* accumulate sums */ - pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */ - paddd xmm5,xmm7 /* accumulate sum squares */ - - paddw xmm4,xmm6 /* accumulate sums */ - pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */ - paddd xmm5,xmm6 /* accumulate sum squares */ - - /* row 10 */ - mov eax,pf10 /* load image pointer */ - movdqu xmm7,[eax] /* load 16 pixels */ - movdqa xmm6,xmm7 - - punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/ - punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/ - - pand xmm6,xmm1 /* mask out pixels 12-16 */ - - movdqu [ecx+10*22],xmm7 /* move short values to patch */ - movdqu [ecx+10*22+16],xmm6 /* move short values to patch */ - - paddusw xmm4,xmm7 /* accumulate sums */ - pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */ - paddd xmm5,xmm7 /* accumulate sum squares */ - - paddw xmm4,xmm6 /* accumulate sums */ - pmaddwd xmm6,xmm6 /* multiply 16 bit ints and add into 32 bit ints */ - paddd xmm5,xmm6 /* accumulate sum squares */ - - /* add up the sum squares */ - movhlps xmm0,xmm5 /* high half to low half */ - paddd xmm5,xmm0 /* add high to low */ - pshuflw xmm0,xmm5, 0xE /* reshuffle */ - paddd xmm5,xmm0 /* add remaining */ - movd f2sum,xmm5 - - /* add up the sum */ - movhlps xmm0,xmm4 - paddw xmm4,xmm0 /* halves added */ - pshuflw xmm0,xmm4,0xE - paddw xmm4,xmm0 /* quarters added */ - pshuflw xmm0,xmm4,0x1 - paddw xmm4,xmm0 /* eighth added */ - movd fsum, xmm4 - - emms - } - - fsum = fsum & 0xFFFF; - - patch[126] = 0; - patch[127] = 0; -#endif /* DB_USE_SSE2 */ - - *sum= (float) fsum; - den=(121.0f*f2sum-fsum*fsum); - *recip= (float)((den!=0.0)?1.0/den:0.0); -} - -void AffineWarpPointOffset(float &r_w,float &c_w,double Hinv[9],int r,int c) -{ - r_w=(float)(Hinv[3]*c+Hinv[4]*r); - c_w=(float)(Hinv[0]*c+Hinv[1]*r); -} - - - -/*! -Prewarp the patches with given affine transform. For a given homogeneous point "x", "H*x" is -the warped point and for any displacement "d" in the warped image resulting in point "y", the -corresponding point in the original image is given by "Hinv*y", which can be simplified for affine H. -If "affine" is 1, then nearest neighbor method is used, else if it is 2, then -bilinear method is used. - */ -inline void db_SignedSquareNormCorr11x11_PreAlign_AffinePatchWarp_u(short *patch,const unsigned char * const *f_img, - int xi,int yi,float *sum,float *recip, - const double Hinv[9],int affine) -{ - float den; - short f; - int f2sum,fsum; - - f2sum=0; - fsum=0; - - if (affine==1) - { - for (int r=0;r<11;r++){ - for (int c=0;c<11;c++){ - f=f_img[yi+AffineWarpPoint_NN_LUT_y[r][c]][xi+AffineWarpPoint_NN_LUT_x[r][c]]; - f2sum+=f*f; - fsum+=f; - (*patch++)=f; - } - } - } - else if (affine==2) - { - for (int r=0;r<11;r++){ - for (int c=0;c<11;c++){ - f=db_BilinearInterpolation(yi+AffineWarpPoint_BL_LUT_y[r][c] - ,xi+AffineWarpPoint_BL_LUT_x[r][c],f_img); - f2sum+=f*f; - fsum+=f; - (*patch++)=f; - } - } - } - - - - (*patch++)=0; (*patch++)=0; (*patch++)=0; (*patch++)=0; (*patch++)=0; - (*patch++)=0; (*patch++)=0; - - *sum= (float) fsum; - den=(121.0f*f2sum-fsum*fsum); - *recip= (float)((den!=0.0)?1.0/den:0.0); -} - - -inline float db_SignedSquareNormCorr11x11_Post_u(unsigned char **f_img,unsigned char **g_img,int x_f,int y_f,int x_g,int y_g, - float fsum_gsum,float f_recip_g_recip) -{ - unsigned char *pf,*pg; - int fgsum; - float fg_corr; - int xm_f,xm_g; - - xm_f=x_f-5; - xm_g=x_g-5; - - pf=f_img[y_f-5]+xm_f; pg=g_img[y_g-5]+xm_g; - fgsum=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f+5]+xm_f; pg=g_img[y_g+5]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - fg_corr=121.0f*fgsum-fsum_gsum; - if(fg_corr>=0.0) return(fg_corr*fg_corr*f_recip_g_recip); - return(-fg_corr*fg_corr*f_recip_g_recip); -} - -float db_SignedSquareNormCorr21x21Aligned_Post_s(const short *f_patch,const short *g_patch,float fsum_gsum,float f_recip_g_recip) -{ - float fgsum,fg_corr; - - fgsum= (float) db_ScalarProduct512_s(f_patch,g_patch); - - fg_corr=441.0f*fgsum-fsum_gsum; - if(fg_corr>=0.0) return(fg_corr*fg_corr*f_recip_g_recip); - return(-fg_corr*fg_corr*f_recip_g_recip); -} - - -float db_SignedSquareNormCorr11x11Aligned_Post_s(const short *f_patch,const short *g_patch,float fsum_gsum,float f_recip_g_recip) -{ - float fgsum,fg_corr; - - fgsum= (float) db_ScalarProduct128_s(f_patch,g_patch); - - fg_corr=121.0f*fgsum-fsum_gsum; - if(fg_corr>=0.0) return(fg_corr*fg_corr*f_recip_g_recip); - return(-fg_corr*fg_corr*f_recip_g_recip); -} - -float db_SignedSquareNormCorr5x5Aligned_Post_s(const short *f_patch,const short *g_patch,float fsum_gsum,float f_recip_g_recip) -{ - float fgsum,fg_corr; - - fgsum= (float) db_ScalarProduct32_s(f_patch,g_patch); - - fg_corr=25.0f*fgsum-fsum_gsum; - if(fg_corr>=0.0) return(fg_corr*fg_corr*f_recip_g_recip); - return(-fg_corr*fg_corr*f_recip_g_recip); -} - - -inline float db_SignedSquareNormCorr15x15_u(unsigned char **f_img,unsigned char **g_img,int x_f,int y_f,int x_g,int y_g) -{ - unsigned char *pf,*pg; - float f,g,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den; - int xm_f,xm_g; - - xm_f=x_f-7; - xm_g=x_g-7; - fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0; - - pf=f_img[y_f-7]+xm_f; pg=g_img[y_g-7]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-6]+xm_f; pg=g_img[y_g-6]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-5]+xm_f; pg=g_img[y_g-5]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+5]+xm_f; pg=g_img[y_g+5]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+6]+xm_f; pg=g_img[y_g+6]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+7]+xm_f; pg=g_img[y_g+7]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - fg_corr=225.0f*fgsum-fsum*gsum; - den=(225.0f*f2sum-fsum*fsum)*(225.0f*g2sum-gsum*gsum); - if(den!=0.0) - { - if(fg_corr>=0.0) return(fg_corr*fg_corr/den); - return(-fg_corr*fg_corr/den); - } - return(0.0); -} - -inline float db_SignedSquareNormCorr7x7_f(float **f_img,float **g_img,int x_f,int y_f,int x_g,int y_g) -{ - float f,g,*pf,*pg,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den; - int xm_f,xm_g; - - xm_f=x_f-3; - xm_g=x_g-3; - fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0; - - pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - fg_corr=49.0f*fgsum-fsum*gsum; - den=(49.0f*f2sum-fsum*fsum)*(49.0f*g2sum-gsum*gsum); - if(den!=0.0) - { - if(fg_corr>=0.0) return(fg_corr*fg_corr/den); - return(-fg_corr*fg_corr/den); - } - return(0.0); -} - -inline float db_SignedSquareNormCorr9x9_f(float **f_img,float **g_img,int x_f,int y_f,int x_g,int y_g) -{ - float f,g,*pf,*pg,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den; - int xm_f,xm_g; - - xm_f=x_f-4; - xm_g=x_g-4; - fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0; - - pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - fg_corr=81.0f*fgsum-fsum*gsum; - den=(81.0f*f2sum-fsum*fsum)*(81.0f*g2sum-gsum*gsum); - if(den!=0.0) - { - if(fg_corr>=0.0) return(fg_corr*fg_corr/den); - return(-fg_corr*fg_corr/den); - } - return(0.0); -} - -inline float db_SignedSquareNormCorr11x11_f(float **f_img,float **g_img,int x_f,int y_f,int x_g,int y_g) -{ - float *pf,*pg; - float f,g,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den; - int xm_f,xm_g; - - xm_f=x_f-5; - xm_g=x_g-5; - fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0; - - pf=f_img[y_f-5]+xm_f; pg=g_img[y_g-5]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+5]+xm_f; pg=g_img[y_g+5]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - fg_corr=121.0f*fgsum-fsum*gsum; - den=(121.0f*f2sum-fsum*fsum)*(121.0f*g2sum-gsum*gsum); - if(den!=0.0) - { - if(fg_corr>=0.0) return(fg_corr*fg_corr/den); - return(-fg_corr*fg_corr/den); - } - return(0.0); -} - -inline void db_SignedSquareNormCorr11x11_Pre_f(float **f_img,int x_f,int y_f,float *sum,float *recip) -{ - float *pf,den; - float f,f2sum,fsum; - int xm_f; - - xm_f=x_f-5; - - pf=f_img[y_f-5]+xm_f; - f= *pf++; f2sum=f*f; fsum=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f-4]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f-3]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f-2]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f-1]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f+1]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f+2]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f+3]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f+4]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - pf=f_img[y_f+5]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf++; f2sum+=f*f; fsum+=f; - f= *pf; f2sum+=f*f; fsum+=f; - - *sum=fsum; - den=(121.0f*f2sum-fsum*fsum); - *recip= (float) ((den!=0.0)?1.0/den:0.0); -} - -inline void db_SignedSquareNormCorr11x11_PreAlign_f(float *patch,const float * const *f_img,int x_f,int y_f,float *sum,float *recip) -{ - const float *pf; - float den,f,f2sum,fsum; - int xm_f; - - xm_f=x_f-5; - - pf=f_img[y_f-5]+xm_f; - f= *pf++; f2sum=f*f; fsum=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f-4]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f-3]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f-2]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f-1]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f+1]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f+2]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f+3]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f+4]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - pf=f_img[y_f+5]+xm_f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f; - f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f; - - (*patch++)=0.0; (*patch++)=0.0; (*patch++)=0.0; (*patch++)=0.0; (*patch++)=0.0; - (*patch++)=0.0; (*patch++)=0.0; - - *sum=fsum; - den=(121.0f*f2sum-fsum*fsum); - *recip= (float) ((den!=0.0)?1.0/den:0.0); -} - -inline float db_SignedSquareNormCorr11x11_Post_f(float **f_img,float **g_img,int x_f,int y_f,int x_g,int y_g, - float fsum_gsum,float f_recip_g_recip) -{ - float *pf,*pg; - float fgsum,fg_corr; - int xm_f,xm_g; - - xm_f=x_f-5; - xm_g=x_g-5; - - pf=f_img[y_f-5]+xm_f; pg=g_img[y_g-5]+xm_g; - fgsum=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - pf=f_img[y_f+5]+xm_f; pg=g_img[y_g+5]+xm_g; - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); - fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg); - - fg_corr=121.0f*fgsum-fsum_gsum; - if(fg_corr>=0.0) return(fg_corr*fg_corr*f_recip_g_recip); - return(-fg_corr*fg_corr*f_recip_g_recip); -} - -inline float db_SignedSquareNormCorr11x11Aligned_Post_f(const float *f_patch,const float *g_patch,float fsum_gsum,float f_recip_g_recip) -{ - float fgsum,fg_corr; - - fgsum=db_ScalarProduct128Aligned16_f(f_patch,g_patch); - - fg_corr=121.0f*fgsum-fsum_gsum; - if(fg_corr>=0.0) return(fg_corr*fg_corr*f_recip_g_recip); - return(-fg_corr*fg_corr*f_recip_g_recip); -} - -inline float db_SignedSquareNormCorr15x15_f(float **f_img,float **g_img,int x_f,int y_f,int x_g,int y_g) -{ - float *pf,*pg; - float f,g,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den; - int xm_f,xm_g; - - xm_f=x_f-7; - xm_g=x_g-7; - fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0; - - pf=f_img[y_f-7]+xm_f; pg=g_img[y_g-7]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-6]+xm_f; pg=g_img[y_g-6]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-5]+xm_f; pg=g_img[y_g-5]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+5]+xm_f; pg=g_img[y_g+5]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+6]+xm_f; pg=g_img[y_g+6]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - pf=f_img[y_f+7]+xm_f; pg=g_img[y_g+7]+xm_g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g; - - fg_corr=225.0f*fgsum-fsum*gsum; - den=(225.0f*f2sum-fsum*fsum)*(225.0f*g2sum-gsum*gsum); - if(den!=0.0) - { - if(fg_corr>=0.0) return(fg_corr*fg_corr/den); - return(-fg_corr*fg_corr/den); - } - return(0.0); -} - -db_Bucket_f** db_AllocBuckets_f(int nr_h,int nr_v,int bd) -{ - int i,j; - db_Bucket_f **bp,*b; - - b=new db_Bucket_f [(nr_h+2)*(nr_v+2)]; - bp=new db_Bucket_f* [(nr_v+2)]; - bp=bp+1; - for(i= -1;i<=nr_v;i++) - { - bp[i]=b+1+(nr_h+2)*(i+1); - for(j= -1;j<=nr_h;j++) - { - bp[i][j].ptr=new db_PointInfo_f [bd]; - } - } - - return(bp); -} - -db_Bucket_u** db_AllocBuckets_u(int nr_h,int nr_v,int bd) -{ - int i,j; - db_Bucket_u **bp,*b; - - b=new db_Bucket_u [(nr_h+2)*(nr_v+2)]; - bp=new db_Bucket_u* [(nr_v+2)]; - bp=bp+1; - for(i= -1;i<=nr_v;i++) - { - bp[i]=b+1+(nr_h+2)*(i+1); - for(j= -1;j<=nr_h;j++) - { - bp[i][j].ptr=new db_PointInfo_u [bd]; - } - } - - return(bp); -} - -void db_FreeBuckets_f(db_Bucket_f **bp,int nr_h,int nr_v) -{ - int i,j; - - for(i= -1;i<=nr_v;i++) for(j= -1;j<=nr_h;j++) - { - delete [] bp[i][j].ptr; - } - delete [] (bp[-1]-1); - delete [] (bp-1); -} - -void db_FreeBuckets_u(db_Bucket_u **bp,int nr_h,int nr_v) -{ - int i,j; - - for(i= -1;i<=nr_v;i++) for(j= -1;j<=nr_h;j++) - { - delete [] bp[i][j].ptr; - } - delete [] (bp[-1]-1); - delete [] (bp-1); -} - -void db_EmptyBuckets_f(db_Bucket_f **bp,int nr_h,int nr_v) -{ - int i,j; - for(i= -1;i<=nr_v;i++) for(j= -1;j<=nr_h;j++) bp[i][j].nr=0; -} - -void db_EmptyBuckets_u(db_Bucket_u **bp,int nr_h,int nr_v) -{ - int i,j; - for(i= -1;i<=nr_v;i++) for(j= -1;j<=nr_h;j++) bp[i][j].nr=0; -} - -float* db_FillBuckets_f(float *patch_space,const float * const *f_img,db_Bucket_f **bp,int bw,int bh,int nr_h,int nr_v,int bd,const double *x,const double *y,int nr_corners) -{ - int i,xi,yi,xpos,ypos,nr; - db_Bucket_f *br; - db_PointInfo_f *pir; - - db_EmptyBuckets_f(bp,nr_h,nr_v); - for(i=0;i<nr_corners;i++) - { - xi=(int) x[i]; - yi=(int) y[i]; - xpos=xi/bw; - ypos=yi/bh; - if(xpos>=0 && xpos<nr_h && ypos>=0 && ypos<nr_v) - { - br=&bp[ypos][xpos]; - nr=br->nr; - if(nr<bd) - { - pir=&(br->ptr[nr]); - pir->x=xi; - pir->y=yi; - pir->id=i; - pir->pir=0; - pir->patch=patch_space; - br->nr=nr+1; - - db_SignedSquareNormCorr11x11_PreAlign_f(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip)); - patch_space+=128; - } - } - } - return(patch_space); -} - -short* db_FillBuckets_u(short *patch_space,const unsigned char * const *f_img,db_Bucket_u **bp,int bw,int bh,int nr_h,int nr_v,int bd,const double *x,const double *y,int nr_corners,int use_smaller_matching_window, int use_21) -{ - int i,xi,yi,xpos,ypos,nr; - db_Bucket_u *br; - db_PointInfo_u *pir; - - db_EmptyBuckets_u(bp,nr_h,nr_v); - for(i=0;i<nr_corners;i++) - { - xi=(int)db_roundi(x[i]); - yi=(int)db_roundi(y[i]); - xpos=xi/bw; - ypos=yi/bh; - if(xpos>=0 && xpos<nr_h && ypos>=0 && ypos<nr_v) - { - br=&bp[ypos][xpos]; - nr=br->nr; - if(nr<bd) - { - pir=&(br->ptr[nr]); - pir->x=xi; - pir->y=yi; - pir->id=i; - pir->pir=0; - pir->patch=patch_space; - br->nr=nr+1; - - if(use_21) - { - db_SignedSquareNormCorr21x21_PreAlign_u(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip)); - patch_space+=512; - } - else - { - if(!use_smaller_matching_window) - { - db_SignedSquareNormCorr11x11_PreAlign_u(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip)); - patch_space+=128; - } - else - { - db_SignedSquareNormCorr5x5_PreAlign_u(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip)); - patch_space+=32; - } - } - } - } - } - return(patch_space); -} - - - -float* db_FillBucketsPrewarped_f(float *patch_space,const float *const *f_img,db_Bucket_f **bp,int bw,int bh,int nr_h,int nr_v,int bd,const double *x,const double *y,int nr_corners,const double H[9]) -{ - int i,xi,yi,xpos,ypos,nr,wxi,wyi; - db_Bucket_f *br; - db_PointInfo_f *pir; - double xd[2],wx[2]; - - db_EmptyBuckets_f(bp,nr_h,nr_v); - for(i=0;i<nr_corners;i++) - { - xd[0]=x[i]; - xd[1]=y[i]; - xi=(int) xd[0]; - yi=(int) xd[1]; - db_ImageHomographyInhomogenous(wx,H,xd); - wxi=(int) wx[0]; - wyi=(int) wx[1]; - - xpos=((wxi+bw)/bw)-1; - ypos=((wyi+bh)/bh)-1; - if(xpos>= -1 && xpos<=nr_h && ypos>= -1 && ypos<=nr_v) - { - br=&bp[ypos][xpos]; - nr=br->nr; - if(nr<bd) - { - pir=&(br->ptr[nr]); - pir->x=wxi; - pir->y=wyi; - pir->id=i; - pir->pir=0; - pir->patch=patch_space; - br->nr=nr+1; - - db_SignedSquareNormCorr11x11_PreAlign_f(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip)); - patch_space+=128; - } - } - } - return(patch_space); -} - -short* db_FillBucketsPrewarped_u(short *patch_space,const unsigned char * const *f_img,db_Bucket_u **bp, - int bw,int bh,int nr_h,int nr_v,int bd,const double *x,const double *y, - int nr_corners,const double H[9]) -{ - int i,xi,yi,xpos,ypos,nr,wxi,wyi; - db_Bucket_u *br; - db_PointInfo_u *pir; - double xd[2],wx[2]; - - db_EmptyBuckets_u(bp,nr_h,nr_v); - for(i=0;i<nr_corners;i++) - { - xd[0]=x[i]; - xd[1]=y[i]; - xi=(int) db_roundi(xd[0]); - yi=(int) db_roundi(xd[1]); - db_ImageHomographyInhomogenous(wx,H,xd); - wxi=(int) wx[0]; - wyi=(int) wx[1]; - - xpos=((wxi+bw)/bw)-1; - ypos=((wyi+bh)/bh)-1; - if(xpos>= -1 && xpos<=nr_h && ypos>= -1 && ypos<=nr_v) - { - br=&bp[ypos][xpos]; - nr=br->nr; - if(nr<bd) - { - pir=&(br->ptr[nr]); - pir->x=wxi; - pir->y=wyi; - pir->id=i; - pir->pir=0; - pir->patch=patch_space; - br->nr=nr+1; - - db_SignedSquareNormCorr11x11_PreAlign_u(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip)); - patch_space+=128; - } - } - } - return(patch_space); -} - - - -short* db_FillBucketsPrewarpedAffine_u(short *patch_space,const unsigned char * const *f_img,db_Bucket_u **bp, - int bw,int bh,int nr_h,int nr_v,int bd,const double *x,const double *y, - int nr_corners,const double H[9],const double Hinv[9],const int warpboundsp[4], - int affine) -{ - int i,xi,yi,xpos,ypos,nr,wxi,wyi; - db_Bucket_u *br; - db_PointInfo_u *pir; - double xd[2],wx[2]; - - db_EmptyBuckets_u(bp,nr_h,nr_v); - for(i=0;i<nr_corners;i++) - { - xd[0]=x[i]; - xd[1]=y[i]; - xi=(int) db_roundi(xd[0]); - yi=(int) db_roundi(xd[1]); - db_ImageHomographyInhomogenous(wx,H,xd); - wxi=(int) wx[0]; - wyi=(int) wx[1]; - - xpos=((wxi+bw)/bw)-1; - ypos=((wyi+bh)/bh)-1; - - - if (xpos>= -1 && xpos<=nr_h && ypos>= -1 && ypos<=nr_v) - { - if( xi>warpboundsp[0] && xi<warpboundsp[1] && yi>warpboundsp[2] && yi<warpboundsp[3]) - { - - br=&bp[ypos][xpos]; - nr=br->nr; - if(nr<bd) - { - pir=&(br->ptr[nr]); - pir->x=wxi; - pir->y=wyi; - pir->id=i; - pir->pir=0; - pir->patch=patch_space; - br->nr=nr+1; - - db_SignedSquareNormCorr11x11_PreAlign_AffinePatchWarp_u(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip),Hinv,affine); - patch_space+=128; - } - } - } - } - return(patch_space); -} - - - -inline void db_MatchPointPair_f(db_PointInfo_f *pir_l,db_PointInfo_f *pir_r, - unsigned long kA,unsigned long kB) -{ - int x_l,y_l,x_r,y_r,xm,ym; - double score; - - x_l=pir_l->x; - y_l=pir_l->y; - x_r=pir_r->x; - y_r=pir_r->y; - xm=x_l-x_r; - ym=y_l-y_r; - /*Check if disparity is within the maximum disparity - with the formula xm^2*256+ym^2*kA<kB - where kA=256*w^2/h^2 - and kB=256*max_disp^2*w^2*/ - if(((xm*xm)<<8)+ym*ym*kA<kB) - { - /*Correlate*/ - score=db_SignedSquareNormCorr11x11Aligned_Post_f(pir_l->patch,pir_r->patch, - (pir_l->sum)*(pir_r->sum), - (pir_l->recip)*(pir_r->recip)); - - if((!(pir_l->pir)) || (score>pir_l->s)) - { - /*Update left corner*/ - pir_l->s=score; - pir_l->pir=pir_r; - } - if((!(pir_r->pir)) || (score>pir_r->s)) - { - /*Update right corner*/ - pir_r->s=score; - pir_r->pir=pir_l; - } - } -} - -inline void db_MatchPointPair_u(db_PointInfo_u *pir_l,db_PointInfo_u *pir_r, - unsigned long kA,unsigned long kB, unsigned int rect_window,bool use_smaller_matching_window, int use_21) -{ - int xm,ym; - double score; - bool compute_score; - - - if( rect_window ) - compute_score = ((unsigned)db_absi(pir_l->x - pir_r->x)<kA && (unsigned)db_absi(pir_l->y - pir_r->y)<kB); - else - { /*Check if disparity is within the maximum disparity - with the formula xm^2*256+ym^2*kA<kB - where kA=256*w^2/h^2 - and kB=256*max_disp^2*w^2*/ - xm= pir_l->x - pir_r->x; - ym= pir_l->y - pir_r->y; - compute_score = ((xm*xm)<<8)+ym*ym*kA < kB; - } - - if ( compute_score ) - { - if(use_21) - { - score=db_SignedSquareNormCorr21x21Aligned_Post_s(pir_l->patch,pir_r->patch, - (pir_l->sum)*(pir_r->sum), - (pir_l->recip)*(pir_r->recip)); - } - else - { - /*Correlate*/ - if(!use_smaller_matching_window) - { - score=db_SignedSquareNormCorr11x11Aligned_Post_s(pir_l->patch,pir_r->patch, - (pir_l->sum)*(pir_r->sum), - (pir_l->recip)*(pir_r->recip)); - } - else - { - score=db_SignedSquareNormCorr5x5Aligned_Post_s(pir_l->patch,pir_r->patch, - (pir_l->sum)*(pir_r->sum), - (pir_l->recip)*(pir_r->recip)); - } - } - - if((!(pir_l->pir)) || (score>pir_l->s)) - { - /*Update left corner*/ - pir_l->s=score; - pir_l->pir=pir_r; - } - if((!(pir_r->pir)) || (score>pir_r->s)) - { - /*Update right corner*/ - pir_r->s=score; - pir_r->pir=pir_l; - } - } -} - -inline void db_MatchPointAgainstBucket_f(db_PointInfo_f *pir_l,db_Bucket_f *b_r, - unsigned long kA,unsigned long kB) -{ - int p_r,nr; - db_PointInfo_f *pir_r; - - nr=b_r->nr; - pir_r=b_r->ptr; - for(p_r=0;p_r<nr;p_r++) db_MatchPointPair_f(pir_l,pir_r+p_r,kA,kB); -} - -inline void db_MatchPointAgainstBucket_u(db_PointInfo_u *pir_l,db_Bucket_u *b_r, - unsigned long kA,unsigned long kB,int rect_window, bool use_smaller_matching_window, int use_21) -{ - int p_r,nr; - db_PointInfo_u *pir_r; - - nr=b_r->nr; - pir_r=b_r->ptr; - - for(p_r=0;p_r<nr;p_r++) db_MatchPointPair_u(pir_l,pir_r+p_r,kA,kB, rect_window, use_smaller_matching_window, use_21); - -} - -void db_MatchBuckets_f(db_Bucket_f **bp_l,db_Bucket_f **bp_r,int nr_h,int nr_v, - unsigned long kA,unsigned long kB) -{ - int i,j,k,a,b,br_nr; - db_Bucket_f *br; - db_PointInfo_f *pir_l; - - /*For all buckets*/ - for(i=0;i<nr_v;i++) for(j=0;j<nr_h;j++) - { - br=&bp_l[i][j]; - br_nr=br->nr; - /*For all points in bucket*/ - for(k=0;k<br_nr;k++) - { - pir_l=br->ptr+k; - for(a=i-1;a<=i+1;a++) - { - for(b=j-1;b<=j+1;b++) - { - db_MatchPointAgainstBucket_f(pir_l,&bp_r[a][b],kA,kB); - } - } - } - } -} - -void db_MatchBuckets_u(db_Bucket_u **bp_l,db_Bucket_u **bp_r,int nr_h,int nr_v, - unsigned long kA,unsigned long kB,int rect_window,bool use_smaller_matching_window, int use_21) -{ - int i,j,k,a,b,br_nr; - db_Bucket_u *br; - db_PointInfo_u *pir_l; - - /*For all buckets*/ - for(i=0;i<nr_v;i++) for(j=0;j<nr_h;j++) - { - br=&bp_l[i][j]; - br_nr=br->nr; - /*For all points in bucket*/ - for(k=0;k<br_nr;k++) - { - pir_l=br->ptr+k; - for(a=i-1;a<=i+1;a++) - { - for(b=j-1;b<=j+1;b++) - { - db_MatchPointAgainstBucket_u(pir_l,&bp_r[a][b],kA,kB,rect_window,use_smaller_matching_window, use_21); - } - } - } - } -} - -void db_CollectMatches_f(db_Bucket_f **bp_l,int nr_h,int nr_v,unsigned long target,int *id_l,int *id_r,int *nr_matches) -{ - int i,j,k,br_nr; - unsigned long count; - db_Bucket_f *br; - db_PointInfo_f *pir,*pir2; - - count=0; - /*For all buckets*/ - for(i=0;i<nr_v;i++) for(j=0;j<nr_h;j++) - { - br=&bp_l[i][j]; - br_nr=br->nr; - /*For all points in bucket*/ - for(k=0;k<br_nr;k++) - { - pir=br->ptr+k; - pir2=pir->pir; - if(pir2) - { - /*This point has a best match*/ - if((pir2->pir)==pir) - { - /*We have a mutually consistent match*/ - if(count<target) - { - id_l[count]=pir->id; - id_r[count]=pir2->id; - count++; - } - } - } - } - } - *nr_matches=count; -} - -void db_CollectMatches_u(db_Bucket_u **bp_l,int nr_h,int nr_v,unsigned long target,int *id_l,int *id_r,int *nr_matches) -{ - int i,j,k,br_nr; - unsigned long count; - db_Bucket_u *br; - db_PointInfo_u *pir,*pir2; - - count=0; - /*For all buckets*/ - for(i=0;i<nr_v;i++) for(j=0;j<nr_h;j++) - { - br=&bp_l[i][j]; - br_nr=br->nr; - /*For all points in bucket*/ - for(k=0;k<br_nr;k++) - { - pir=br->ptr+k; - pir2=pir->pir; - if(pir2) - { - /*This point has a best match*/ - if((pir2->pir)==pir) - { - /*We have a mutually consistent match*/ - if(count<target) - { - id_l[count]=pir->id; - id_r[count]=pir2->id; - count++; - } - } - } - } - } - *nr_matches=count; -} - -db_Matcher_f::db_Matcher_f() -{ - m_w=0; m_h=0; -} - -db_Matcher_f::~db_Matcher_f() -{ - Clean(); -} - -void db_Matcher_f::Clean() -{ - if(m_w) - { - /*Free buckets*/ - db_FreeBuckets_f(m_bp_l,m_nr_h,m_nr_v); - db_FreeBuckets_f(m_bp_r,m_nr_h,m_nr_v); - /*Free space for patch layouts*/ - delete [] m_patch_space; - } - m_w=0; m_h=0; -} - -unsigned long db_Matcher_f::Init(int im_width,int im_height,double max_disparity,int target_nr_corners) -{ - Clean(); - m_w=im_width; - m_h=im_height; - m_bw=db_maxi(1,(int) (max_disparity*((double)im_width))); - m_bh=db_maxi(1,(int) (max_disparity*((double)im_height))); - m_nr_h=1+(im_width-1)/m_bw; - m_nr_v=1+(im_height-1)/m_bh; - m_bd=db_maxi(1,(int)(((double)target_nr_corners)* - max_disparity*max_disparity)); - m_target=target_nr_corners; - m_kA=(long)(256.0*((double)(m_w*m_w))/((double)(m_h*m_h))); - m_kB=(long)(256.0*max_disparity*max_disparity*((double)(m_w*m_w))); - - /*Alloc bucket structure*/ - m_bp_l=db_AllocBuckets_f(m_nr_h,m_nr_v,m_bd); - m_bp_r=db_AllocBuckets_f(m_nr_h,m_nr_v,m_bd); - - /*Alloc 16byte-aligned space for patch layouts*/ - m_patch_space=new float [2*(m_nr_h+2)*(m_nr_v+2)*m_bd*128+16]; - m_aligned_patch_space=db_AlignPointer_f(m_patch_space,16); - - return(m_target); -} - -void db_Matcher_f::Match(const float * const *l_img,const float * const *r_img, - const double *x_l,const double *y_l,int nr_l,const double *x_r,const double *y_r,int nr_r, - int *id_l,int *id_r,int *nr_matches,const double H[9]) -{ - float *ps; - - /*Insert the corners into bucket structure*/ - ps=db_FillBuckets_f(m_aligned_patch_space,l_img,m_bp_l,m_bw,m_bh,m_nr_h,m_nr_v,m_bd,x_l,y_l,nr_l); - if(H==0) db_FillBuckets_f(ps,r_img,m_bp_r,m_bw,m_bh,m_nr_h,m_nr_v,m_bd,x_r,y_r,nr_r); - else db_FillBucketsPrewarped_f(ps,r_img,m_bp_r,m_bw,m_bh,m_nr_h,m_nr_v,m_bd,x_r,y_r,nr_r,H); - - /*Compute all the necessary match scores*/ - db_MatchBuckets_f(m_bp_l,m_bp_r,m_nr_h,m_nr_v,m_kA,m_kB); - - /*Collect the correspondences*/ - db_CollectMatches_f(m_bp_l,m_nr_h,m_nr_v,m_target,id_l,id_r,nr_matches); -} - -db_Matcher_u::db_Matcher_u() -{ - m_w=0; m_h=0; - m_rect_window = 0; - m_bw=m_bh=m_nr_h=m_nr_v=m_bd=m_target=0; - m_bp_l=m_bp_r=0; - m_patch_space=m_aligned_patch_space=0; -} - -db_Matcher_u::db_Matcher_u(const db_Matcher_u& cm) -{ - Init(cm.m_w, cm.m_h, cm.m_max_disparity, cm.m_target, cm.m_max_disparity_v); -} - -db_Matcher_u& db_Matcher_u::operator= (const db_Matcher_u& cm) -{ - if ( this == &cm ) return *this; - Init(cm.m_w, cm.m_h, cm.m_max_disparity, cm.m_target, cm.m_max_disparity_v); - return *this; -} - - -db_Matcher_u::~db_Matcher_u() -{ - Clean(); -} - -void db_Matcher_u::Clean() -{ - if(m_w) - { - /*Free buckets*/ - db_FreeBuckets_u(m_bp_l,m_nr_h,m_nr_v); - db_FreeBuckets_u(m_bp_r,m_nr_h,m_nr_v); - /*Free space for patch layouts*/ - delete [] m_patch_space; - } - m_w=0; m_h=0; -} - - -unsigned long db_Matcher_u::Init(int im_width,int im_height,double max_disparity,int target_nr_corners, - double max_disparity_v, bool use_smaller_matching_window, int use_21) -{ - Clean(); - m_w=im_width; - m_h=im_height; - m_max_disparity=max_disparity; - m_max_disparity_v=max_disparity_v; - - if ( max_disparity_v != DB_DEFAULT_NO_DISPARITY ) - { - m_rect_window = 1; - - m_bw=db_maxi(1,(int)(max_disparity*((double)im_width))); - m_bh=db_maxi(1,(int)(max_disparity_v*((double)im_height))); - - m_bd=db_maxi(1,(int)(((double)target_nr_corners)*max_disparity*max_disparity_v)); - - m_kA=(int)(max_disparity*m_w); - m_kB=(int)(max_disparity_v*m_h); - - } else - { - m_bw=(int)db_maxi(1,(int)(max_disparity*((double)im_width))); - m_bh=(int)db_maxi(1,(int)(max_disparity*((double)im_height))); - - m_bd=db_maxi(1,(int)(((double)target_nr_corners)*max_disparity*max_disparity)); - - m_kA=(long)(256.0*((double)(m_w*m_w))/((double)(m_h*m_h))); - m_kB=(long)(256.0*max_disparity*max_disparity*((double)(m_w*m_w))); - } - - m_nr_h=1+(im_width-1)/m_bw; - m_nr_v=1+(im_height-1)/m_bh; - - m_target=target_nr_corners; - - /*Alloc bucket structure*/ - m_bp_l=db_AllocBuckets_u(m_nr_h,m_nr_v,m_bd); - m_bp_r=db_AllocBuckets_u(m_nr_h,m_nr_v,m_bd); - - m_use_smaller_matching_window = use_smaller_matching_window; - m_use_21 = use_21; - - if(m_use_21) - { - /*Alloc 64byte-aligned space for patch layouts*/ - m_patch_space=new short [2*(m_nr_h+2)*(m_nr_v+2)*m_bd*512+64]; - m_aligned_patch_space=db_AlignPointer_s(m_patch_space,64); - } - else - { - if(!m_use_smaller_matching_window) - { - /*Alloc 16byte-aligned space for patch layouts*/ - m_patch_space=new short [2*(m_nr_h+2)*(m_nr_v+2)*m_bd*128+16]; - m_aligned_patch_space=db_AlignPointer_s(m_patch_space,16); - } - else - { - /*Alloc 4byte-aligned space for patch layouts*/ - m_patch_space=new short [2*(m_nr_h+2)*(m_nr_v+2)*m_bd*32+4]; - m_aligned_patch_space=db_AlignPointer_s(m_patch_space,4); - } - } - - return(m_target); -} - -void db_Matcher_u::Match(const unsigned char * const *l_img,const unsigned char * const *r_img, - const double *x_l,const double *y_l,int nr_l,const double *x_r,const double *y_r,int nr_r, - int *id_l,int *id_r,int *nr_matches,const double H[9],int affine) -{ - short *ps; - - /*Insert the corners into bucket structure*/ - ps=db_FillBuckets_u(m_aligned_patch_space,l_img,m_bp_l,m_bw,m_bh,m_nr_h,m_nr_v,m_bd,x_l,y_l,nr_l,m_use_smaller_matching_window,m_use_21); - if(H==0) - db_FillBuckets_u(ps,r_img,m_bp_r,m_bw,m_bh,m_nr_h,m_nr_v,m_bd,x_r,y_r,nr_r,m_use_smaller_matching_window,m_use_21); - else - { - if (affine) - { - double Hinv[9]; - db_InvertAffineTransform(Hinv,H); - float r_w, c_w; - float stretch_x[2]; - float stretch_y[2]; - AffineWarpPointOffset(r_w,c_w,Hinv, 5,5); - stretch_x[0]=db_absf(c_w);stretch_y[0]=db_absf(r_w); - AffineWarpPointOffset(r_w,c_w,Hinv, 5,-5); - stretch_x[1]=db_absf(c_w);stretch_y[1]=db_absf(r_w); - int max_stretxh_x=(int) (db_maxd(stretch_x[0],stretch_x[1])); - int max_stretxh_y=(int) (db_maxd(stretch_y[0],stretch_y[1])); - int warpbounds[4]={max_stretxh_x,m_w-1-max_stretxh_x,max_stretxh_y,m_h-1-max_stretxh_y}; - - for (int r=-5;r<=5;r++){ - for (int c=-5;c<=5;c++){ - AffineWarpPointOffset(r_w,c_w,Hinv,r,c); - AffineWarpPoint_BL_LUT_y[r+5][c+5]=r_w; - AffineWarpPoint_BL_LUT_x[r+5][c+5]=c_w; - - AffineWarpPoint_NN_LUT_y[r+5][c+5]=db_roundi(r_w); - AffineWarpPoint_NN_LUT_x[r+5][c+5]=db_roundi(c_w); - - } - } - - db_FillBucketsPrewarpedAffine_u(ps,r_img,m_bp_r,m_bw,m_bh,m_nr_h,m_nr_v,m_bd, - x_r,y_r,nr_r,H,Hinv,warpbounds,affine); - } - else - db_FillBucketsPrewarped_u(ps,r_img,m_bp_r,m_bw,m_bh,m_nr_h,m_nr_v,m_bd,x_r,y_r,nr_r,H); - } - - - /*Compute all the necessary match scores*/ - db_MatchBuckets_u(m_bp_l,m_bp_r,m_nr_h,m_nr_v,m_kA,m_kB, m_rect_window,m_use_smaller_matching_window,m_use_21); - - /*Collect the correspondences*/ - db_CollectMatches_u(m_bp_l,m_nr_h,m_nr_v,m_target,id_l,id_r,nr_matches); -} - -int db_Matcher_u::IsAllocated() -{ - return (int)(m_w != 0); -} diff --git a/jni/feature_stab/db_vlvm/db_feature_matching.h b/jni/feature_stab/db_vlvm/db_feature_matching.h deleted file mode 100644 index 6c056b9a3..000000000 --- a/jni/feature_stab/db_vlvm/db_feature_matching.h +++ /dev/null @@ -1,260 +0,0 @@ -/* - * 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. - */ - -/*$Id: db_feature_matching.h,v 1.3 2011/06/17 14:03:30 mbansal Exp $*/ - -#ifndef DB_FEATURE_MATCHING_H -#define DB_FEATURE_MATCHING_H - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ -/*! - * \defgroup FeatureMatching Feature Matching - */ -#include "db_utilities.h" -#include "db_utilities_constants.h" - -DB_API void db_SignedSquareNormCorr21x21_PreAlign_u(short *patch,const unsigned char * const *f_img,int x_f,int y_f,float *sum,float *recip); -DB_API void db_SignedSquareNormCorr11x11_PreAlign_u(short *patch,const unsigned char * const *f_img,int x_f,int y_f,float *sum,float *recip); -float db_SignedSquareNormCorr21x21Aligned_Post_s(const short *f_patch,const short *g_patch,float fsum_gsum,float f_recip_g_recip); -float db_SignedSquareNormCorr11x11Aligned_Post_s(const short *f_patch,const short *g_patch,float fsum_gsum,float f_recip_g_recip); - -class db_PointInfo_f -{ -public: - /*Coordinates of point*/ - int x; - int y; - /*Id nr of point*/ - int id; - /*Best match score*/ - double s; - /*Best match candidate*/ - db_PointInfo_f *pir; - /*Precomputed coefficients - of image patch*/ - float sum; - float recip; - /*Pointer to patch layout*/ - const float *patch; -}; - -class db_Bucket_f -{ -public: - db_PointInfo_f *ptr; - int nr; -}; - -class db_PointInfo_u -{ -public: - /*Coordinates of point*/ - int x; - int y; - /*Id nr of point*/ - int id; - /*Best match score*/ - double s; - /*Best match candidate*/ - db_PointInfo_u *pir; - /*Precomputed coefficients - of image patch*/ - float sum; - float recip; - /*Pointer to patch layout*/ - const short *patch; -}; - -class db_Bucket_u -{ -public: - db_PointInfo_u *ptr; - int nr; -}; -/*! - * \class db_Matcher_f - * \ingroup FeatureMatching - * \brief Feature matcher for float images. - * - * Normalized correlation feature matcher for <b>float</b> images. - * Correlation window size is constant and set to 11x11. - * See \ref FeatureDetection to detect Harris corners. - * Images are managed with functions in \ref LMImageBasicUtilities. - */ -class DB_API db_Matcher_f -{ -public: - db_Matcher_f(); - ~db_Matcher_f(); - - /*! - * Set parameters and pre-allocate memory. Return an upper bound - * on the number of matches. - * \param im_width width - * \param im_height height - * \param max_disparity maximum distance (as fraction of image size) between matches - * \param target_nr_corners maximum number of matches - * \return maximum number of matches - */ - unsigned long Init(int im_width,int im_height, - double max_disparity=DB_DEFAULT_MAX_DISPARITY, - int target_nr_corners=DB_DEFAULT_TARGET_NR_CORNERS); - - /*! - * Match two sets of features. - * If the prewarp H is not NULL it will be applied to the features - * in the right image before matching. - * Parameters id_l and id_r must point to arrays of size target_nr_corners - * (returned by Init()). - * The results of matching are in id_l and id_r. - * Interpretaqtion of results: if id_l[i] = m and id_r[i] = n, - * feature at (x_l[m],y_l[m]) matched to (x_r[n],y_r[n]). - * \param l_img left image - * \param r_img right image - * \param x_l left x coordinates of features - * \param y_l left y coordinates of features - * \param nr_l number of features in left image - * \param x_r right x coordinates of features - * \param y_r right y coordinates of features - * \param nr_r number of features in right image - * \param id_l indices of left features that matched - * \param id_r indices of right features that matched - * \param nr_matches number of features actually matched - * \param H image homography (prewarp) to be applied to right image features - */ - void Match(const float * const *l_img,const float * const *r_img, - const double *x_l,const double *y_l,int nr_l,const double *x_r,const double *y_r,int nr_r, - int *id_l,int *id_r,int *nr_matches,const double H[9]=0); - -protected: - void Clean(); - - int m_w,m_h,m_bw,m_bh,m_nr_h,m_nr_v,m_bd,m_target; - unsigned long m_kA,m_kB; - db_Bucket_f **m_bp_l; - db_Bucket_f **m_bp_r; - float *m_patch_space,*m_aligned_patch_space; -}; -/*! - * \class db_Matcher_u - * \ingroup FeatureMatching - * \brief Feature matcher for byte images. - * - * Normalized correlation feature matcher for <b>byte</b> images. - * Correlation window size is constant and set to 11x11. - * See \ref FeatureDetection to detect Harris corners. - * Images are managed with functions in \ref LMImageBasicUtilities. - * - * If the prewarp matrix H is supplied, the feature coordinates are warped by H before being placed in - * appropriate buckets. If H is an affine transform and the "affine" parameter is set to 1 or 2, - * then the correlation patches themselves are warped before being placed in the patch space. - */ -class DB_API db_Matcher_u -{ -public: - db_Matcher_u(); - - int GetPatchSize(){return 11;}; - - virtual ~db_Matcher_u(); - - /*! - Copy ctor duplicates settings. - Memory not copied. - */ - db_Matcher_u(const db_Matcher_u& cm); - - /*! - Assignment optor duplicates settings - Memory not copied. - */ - db_Matcher_u& operator= (const db_Matcher_u& cm); - - /*! - * Set parameters and pre-allocate memory. Return an upper bound - * on the number of matches. - * If max_disparity_v is DB_DEFAULT_NO_DISPARITY, look for matches - * in a ellipse around a feature of radius max_disparity*im_width by max_disparity*im_height. - * If max_disparity_v is specified, use a rectangle max_disparity*im_width by max_disparity_v*im_height. - * \param im_width width - * \param im_height height - * \param max_disparity maximum distance (as fraction of image size) between matches - * \param target_nr_corners maximum number of matches - * \param max_disparity_v maximum vertical disparity (distance between matches) - * \param use_smaller_matching_window if set to true, uses a correlation window of 5x5 instead of the default 11x11 - * \return maximum number of matches - */ - virtual unsigned long Init(int im_width,int im_height, - double max_disparity=DB_DEFAULT_MAX_DISPARITY, - int target_nr_corners=DB_DEFAULT_TARGET_NR_CORNERS, - double max_disparity_v=DB_DEFAULT_NO_DISPARITY, - bool use_smaller_matching_window=false, int use_21=0); - - /*! - * Match two sets of features. - * If the prewarp H is not NULL it will be applied to the features - * in the right image before matching. - * Parameters id_l and id_r must point to arrays of size target_nr_corners - * (returned by Init()). - * The results of matching are in id_l and id_r. - * Interpretaqtion of results: if id_l[i] = m and id_r[i] = n, - * feature at (x_l[m],y_l[m]) matched to (x_r[n],y_r[n]). - * \param l_img left image - * \param r_img right image - * \param x_l left x coordinates of features - * \param y_l left y coordinates of features - * \param nr_l number of features in left image - * \param x_r right x coordinates of features - * \param y_r right y coordinates of features - * \param nr_r number of features in right image - * \param id_l indices of left features that matched - * \param id_r indices of right features that matched - * \param nr_matches number of features actually matched - * \param H image homography (prewarp) to be applied to right image features - * \param affine prewarp the 11x11 patches by given affine transform. 0 means no warping, - 1 means nearest neighbor, 2 means bilinear warping. - */ - virtual void Match(const unsigned char * const *l_img,const unsigned char * const *r_img, - const double *x_l,const double *y_l,int nr_l,const double *x_r,const double *y_r,int nr_r, - int *id_l,int *id_r,int *nr_matches,const double H[9]=0,int affine=0); - - /*! - * Checks if Init() was called. - * \return 1 if Init() was called, 0 otherwise. - */ - int IsAllocated(); - -protected: - virtual void Clean(); - - - int m_w,m_h,m_bw,m_bh,m_nr_h,m_nr_v,m_bd,m_target; - unsigned long m_kA,m_kB; - db_Bucket_u **m_bp_l; - db_Bucket_u **m_bp_r; - short *m_patch_space,*m_aligned_patch_space; - - double m_max_disparity, m_max_disparity_v; - int m_rect_window; - bool m_use_smaller_matching_window; - int m_use_21; -}; - - - -#endif /*DB_FEATURE_MATCHING_H*/ diff --git a/jni/feature_stab/db_vlvm/db_framestitching.cpp b/jni/feature_stab/db_vlvm/db_framestitching.cpp deleted file mode 100644 index b574f7a04..000000000 --- a/jni/feature_stab/db_vlvm/db_framestitching.cpp +++ /dev/null @@ -1,169 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_framestitching.cpp,v 1.2 2011/06/17 14:03:30 mbansal Exp $ */ - -#include "db_utilities.h" -#include "db_framestitching.h" - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ - -inline void db_RotationFromMOuterProductSum(double R[9],double *score,double M[9]) -{ - double N[16],q[4],lambda[4],lambda_max; - double y[4]; - int nr_roots; - - N[0]= M[0]+M[4]+M[8]; - N[5]= M[0]-M[4]-M[8]; - N[10]= -M[0]+M[4]-M[8]; - N[15]= -M[0]-M[4]+M[8]; - N[1] =N[4] =M[5]-M[7]; - N[2] =N[8] =M[6]-M[2]; - N[3] =N[12]=M[1]-M[3]; - N[6] =N[9] =M[1]+M[3]; - N[7] =N[13]=M[6]+M[2]; - N[11]=N[14]=M[5]+M[7]; - - /*get the quaternion representing the rotation - by finding the eigenvector corresponding to the most - positive eigenvalue. Force eigenvalue solutions, since the matrix - is symmetric and solutions might otherwise be lost - when the data is planar*/ - db_RealEigenvalues4x4(lambda,&nr_roots,N,1); - if(nr_roots) - { - lambda_max=lambda[0]; - if(nr_roots>=2) - { - if(lambda[1]>lambda_max) lambda_max=lambda[1]; - if(nr_roots>=3) - { - if(lambda[2]>lambda_max) lambda_max=lambda[2]; - { - if(nr_roots>=4) if(lambda[3]>lambda_max) lambda_max=lambda[3]; - } - } - } - } - else lambda_max=1.0; - db_EigenVector4x4(q,lambda_max,N); - - /*Compute the rotation matrix*/ - db_QuaternionToRotation(R,q); - - if(score) - { - /*Compute score=transpose(q)*N*q */ - db_Multiply4x4_4x1(y,N,q); - *score=db_ScalarProduct4(q,y); - } -} - -void db_StitchSimilarity3DRaw(double *scale,double R[9],double t[3], - double **Xp,double **X,int nr_points,int orientation_preserving, - int allow_scaling,int allow_rotation,int allow_translation) -{ - int i; - double c[3],cp[3],r[3],rp[3],M[9],s,sp,sc; - double Rr[9],score_p,score_r; - double *temp,*temp_p; - - if(allow_translation) - { - db_PointCentroid3D(c,X,nr_points); - db_PointCentroid3D(cp,Xp,nr_points); - } - else - { - db_Zero3(c); - db_Zero3(cp); - } - - db_Zero9(M); - s=sp=0; - for(i=0;i<nr_points;i++) - { - temp= *X++; - temp_p= *Xp++; - r[0]=(*temp++)-c[0]; - r[1]=(*temp++)-c[1]; - r[2]=(*temp++)-c[2]; - rp[0]=(*temp_p++)-cp[0]; - rp[1]=(*temp_p++)-cp[1]; - rp[2]=(*temp_p++)-cp[2]; - - M[0]+=r[0]*rp[0]; - M[1]+=r[0]*rp[1]; - M[2]+=r[0]*rp[2]; - M[3]+=r[1]*rp[0]; - M[4]+=r[1]*rp[1]; - M[5]+=r[1]*rp[2]; - M[6]+=r[2]*rp[0]; - M[7]+=r[2]*rp[1]; - M[8]+=r[2]*rp[2]; - - s+=db_sqr(r[0])+db_sqr(r[1])+db_sqr(r[2]); - sp+=db_sqr(rp[0])+db_sqr(rp[1])+db_sqr(rp[2]); - } - - /*Compute scale*/ - if(allow_scaling) sc=sqrt(db_SafeDivision(sp,s)); - else sc=1.0; - *scale=sc; - - /*Compute rotation*/ - if(allow_rotation) - { - if(orientation_preserving) - { - db_RotationFromMOuterProductSum(R,0,M); - } - else - { - /*Try preserving*/ - db_RotationFromMOuterProductSum(R,&score_p,M); - /*Try reversing*/ - M[6]= -M[6]; - M[7]= -M[7]; - M[8]= -M[8]; - db_RotationFromMOuterProductSum(Rr,&score_r,M); - if(score_r>score_p) - { - /*Reverse is better*/ - R[0]=Rr[0]; R[1]=Rr[1]; R[2]= -Rr[2]; - R[3]=Rr[3]; R[4]=Rr[4]; R[5]= -Rr[5]; - R[6]=Rr[6]; R[7]=Rr[7]; R[8]= -Rr[8]; - } - } - } - else db_Identity3x3(R); - - /*Compute translation*/ - if(allow_translation) - { - t[0]=cp[0]-sc*(R[0]*c[0]+R[1]*c[1]+R[2]*c[2]); - t[1]=cp[1]-sc*(R[3]*c[0]+R[4]*c[1]+R[5]*c[2]); - t[2]=cp[2]-sc*(R[6]*c[0]+R[7]*c[1]+R[8]*c[2]); - } - else db_Zero3(t); -} - - diff --git a/jni/feature_stab/db_vlvm/db_framestitching.h b/jni/feature_stab/db_vlvm/db_framestitching.h deleted file mode 100644 index 5fef5f37e..000000000 --- a/jni/feature_stab/db_vlvm/db_framestitching.h +++ /dev/null @@ -1,94 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_framestitching.h,v 1.2 2011/06/17 14:03:31 mbansal Exp $ */ - -#ifndef DB_FRAMESTITCHING_H -#define DB_FRAMESTITCHING_H -/*! - * \defgroup FrameStitching Frame Stitching (2D and 3D homography estimation) - */ -/*\{*/ - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ -/*! - * \defgroup LMFrameStitching (LM) Frame Stitching (2D and 3D homography estimation) - */ -/*\{*/ - -/*! -Find scale, rotation and translation of the similarity that -takes the nr_points inhomogenous 3D points X to Xp -(left to right according to Horn), i.e. for the homogenous equivalents -Xp and X we would have -\code - Xp~ - [sR t]*X - [0 1] -\endcode -If orientation_preserving is true, R is restricted such that det(R)>0. -allow_scaling, allow_rotation and allow_translation allow s,R and t -to differ from 1,Identity and 0 - -Full similarity takes the following on 550MHz: -\code -4.5 microseconds with 3 points -4.7 microseconds with 4 points -5.0 microseconds with 5 points -5.2 microseconds with 6 points -5.8 microseconds with 10 points -20 microseconds with 100 points -205 microseconds with 1000 points -2.9 milliseconds with 10000 points -50 milliseconds with 100000 points -0.5 seconds with 1000000 points -\endcode -Without orientation_preserving: -\code -4 points is minimal for (s,R,t) (R,t) -3 points is minimal for (s,R) (R) -2 points is minimal for (s,t) -1 point is minimal for (s) (t) -\endcode -With orientation_preserving: -\code -3 points is minimal for (s,R,t) (R,t) -2 points is minimal for (s,R) (s,t) (R) -1 point is minimal for (s) (t) -\endcode - -\param scale scale -\param R rotation -\param t translation -\param Xp inhomogenouse 3D points in first coordinate system -\param X inhomogenouse 3D points in second coordinate system -\param nr_points number of points -\param orientation_preserving if true, R is restricted such that det(R)>0. -\param allow_scaling estimate scale -\param allow_rotation estimate rotation -\param allow_translation estimate translation -*/ -DB_API void db_StitchSimilarity3DRaw(double *scale,double R[9],double t[3], - double **Xp,double **X,int nr_points,int orientation_preserving=1, - int allow_scaling=1,int allow_rotation=1,int allow_translation=1); - - -/*\}*/ - -#endif /* DB_FRAMESTITCHING_H */ diff --git a/jni/feature_stab/db_vlvm/db_image_homography.cpp b/jni/feature_stab/db_vlvm/db_image_homography.cpp deleted file mode 100644 index aaad7f85e..000000000 --- a/jni/feature_stab/db_vlvm/db_image_homography.cpp +++ /dev/null @@ -1,332 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_image_homography.cpp,v 1.2 2011/06/17 14:03:31 mbansal Exp $ */ - -#include "db_utilities.h" -#include "db_image_homography.h" -#include "db_framestitching.h" -#include "db_metrics.h" - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ - -/*Compute the linear constraint on H obtained by requiring that the -ratio between coordinate i_num and i_den of xp is equal to the ratio -between coordinate i_num and i_den of Hx. i_zero should be set to -the coordinate not equal to i_num or i_den. No normalization is used*/ -inline void db_SProjImagePointPointConstraint(double c[9],int i_num,int i_den,int i_zero, - double xp[3],double x[3]) -{ - db_MultiplyScalarCopy3(c+3*i_den,x, xp[i_num]); - db_MultiplyScalarCopy3(c+3*i_num,x, -xp[i_den]); - db_Zero3(c+3*i_zero); -} - -/*Compute two constraints on H generated by the correspondence (Xp,X), -assuming that Xp ~= H*X. No normalization is used*/ -inline void db_SProjImagePointPointConstraints(double c1[9],double c2[9],double xp[3],double x[3]) -{ - int ma_ind; - - /*Find index of coordinate of Xp with largest absolute value*/ - ma_ind=db_MaxAbsIndex3(xp); - - /*Generate 2 constraints, - each constraint is generated by considering the ratio between a - coordinate and the largest absolute value coordinate*/ - switch(ma_ind) - { - case 0: - db_SProjImagePointPointConstraint(c1,1,0,2,xp,x); - db_SProjImagePointPointConstraint(c2,2,0,1,xp,x); - break; - case 1: - db_SProjImagePointPointConstraint(c1,0,1,2,xp,x); - db_SProjImagePointPointConstraint(c2,2,1,0,xp,x); - break; - default: - db_SProjImagePointPointConstraint(c1,0,2,1,xp,x); - db_SProjImagePointPointConstraint(c2,1,2,0,xp,x); - } -} - -inline void db_SAffineImagePointPointConstraints(double c1[7],double c2[7],double xp[3],double x[3]) -{ - double ct1[9],ct2[9]; - - db_SProjImagePointPointConstraints(ct1,ct2,xp,x); - db_Copy6(c1,ct1); c1[6]=ct1[8]; - db_Copy6(c2,ct2); c2[6]=ct2[8]; -} - -void db_StitchProjective2D_4Points(double H[9], - double x1[3],double x2[3],double x3[3],double x4[3], - double xp1[3],double xp2[3],double xp3[3],double xp4[3]) -{ - double c[72]; - - /*Collect the constraints*/ - db_SProjImagePointPointConstraints(c ,c+9 ,xp1,x1); - db_SProjImagePointPointConstraints(c+18,c+27,xp2,x2); - db_SProjImagePointPointConstraints(c+36,c+45,xp3,x3); - db_SProjImagePointPointConstraints(c+54,c+63,xp4,x4); - /*Solve for the nullvector*/ - db_NullVector8x9Destructive(H,c); -} - -void db_StitchAffine2D_3Points(double H[9], - double x1[3],double x2[3],double x3[3], - double xp1[3],double xp2[3],double xp3[3]) -{ - double c[42]; - - /*Collect the constraints*/ - db_SAffineImagePointPointConstraints(c ,c+7 ,xp1,x1); - db_SAffineImagePointPointConstraints(c+14,c+21,xp2,x2); - db_SAffineImagePointPointConstraints(c+28,c+35,xp3,x3); - /*Solve for the nullvector*/ - db_NullVector6x7Destructive(H,c); - db_MultiplyScalar6(H,db_SafeReciprocal(H[6])); - H[6]=H[7]=0; H[8]=1.0; -} - -/*Compute up to three solutions for the focal length given two point correspondences -generated by a rotation with a common unknown focal length. No specific normalization -of the input points is required. If signed_disambiguation is true, the points are -required to be in front of the camera*/ -inline void db_CommonFocalLengthFromRotation_2Point(double fsol[3],int *nr_sols,double x1[3],double x2[3],double xp1[3],double xp2[3],int signed_disambiguation=1) -{ - double m,ax,ay,apx,apy,bx,by,bpx,bpy; - double p1[2],p2[2],p3[2],p4[2],p5[2],p6[2]; - double p7[3],p8[4],p9[5],p10[3],p11[4]; - double roots[3]; - int nr_roots,i,j; - - /*Solve for focal length using the equation - <a,b>^2*<ap,ap><bp,bp>=<ap,bp>^2*<a,a><b,b> - where a and ap are the homogenous vectors in the first image - after focal length scaling and b,bp are the vectors in the - second image*/ - - /*Normalize homogenous coordinates so that last coordinate is one*/ - m=db_SafeReciprocal(x1[2]); - ax=x1[0]*m; - ay=x1[1]*m; - m=db_SafeReciprocal(xp1[2]); - apx=xp1[0]*m; - apy=xp1[1]*m; - m=db_SafeReciprocal(x2[2]); - bx=x2[0]*m; - by=x2[1]*m; - m=db_SafeReciprocal(xp2[2]); - bpx=xp2[0]*m; - bpy=xp2[1]*m; - - /*Compute cubic in l=1/(f^2) - by dividing out the root l=0 from the equation - (l(ax*bx+ay*by)+1)^2*(l(apx^2+apy^2)+1)*(l(bpx^2+bpy^2)+1)= - (l(apx*bpx+apy*bpy)+1)^2*(l(ax^2+ay^2)+1)*(l(bx^2+by^2)+1)*/ - p1[1]=ax*bx+ay*by; - p2[1]=db_sqr(apx)+db_sqr(apy); - p3[1]=db_sqr(bpx)+db_sqr(bpy); - p4[1]=apx*bpx+apy*bpy; - p5[1]=db_sqr(ax)+db_sqr(ay); - p6[1]=db_sqr(bx)+db_sqr(by); - p1[0]=p2[0]=p3[0]=p4[0]=p5[0]=p6[0]=1; - - db_MultiplyPoly1_1(p7,p1,p1); - db_MultiplyPoly1_2(p8,p2,p7); - db_MultiplyPoly1_3(p9,p3,p8); - - db_MultiplyPoly1_1(p10,p4,p4); - db_MultiplyPoly1_2(p11,p5,p10); - db_SubtractPolyProduct1_3(p9,p6,p11); - /*Cubic starts at p9[1]*/ - db_SolveCubic(roots,&nr_roots,p9[4],p9[3],p9[2],p9[1]); - - for(j=0,i=0;i<nr_roots;i++) - { - if(roots[i]>0) - { - if((!signed_disambiguation) || (db_PolyEval1(p1,roots[i])*db_PolyEval1(p4,roots[i])>0)) - { - fsol[j++]=db_SafeSqrtReciprocal(roots[i]); - } - } - } - *nr_sols=j; -} - -int db_StitchRotationCommonFocalLength_3Points(double H[9],double x1[3],double x2[3],double x3[3],double xp1[3],double xp2[3],double xp3[3],double *f,int signed_disambiguation) -{ - double fsol[3]; - int nr_sols,i,best_sol,done; - double cost,best_cost; - double m,hyp[27],x1_temp[3],x2_temp[3],xp1_temp[3],xp2_temp[3]; - double *hyp_point,ft; - double y[2]; - - db_CommonFocalLengthFromRotation_2Point(fsol,&nr_sols,x1,x2,xp1,xp2,signed_disambiguation); - if(nr_sols) - { - db_DeHomogenizeImagePoint(y,xp3); - done=0; - for(i=0;i<nr_sols;i++) - { - ft=fsol[i]; - m=db_SafeReciprocal(ft); - x1_temp[0]=x1[0]*m; - x1_temp[1]=x1[1]*m; - x1_temp[2]=x1[2]; - x2_temp[0]=x2[0]*m; - x2_temp[1]=x2[1]*m; - x2_temp[2]=x2[2]; - xp1_temp[0]=xp1[0]*m; - xp1_temp[1]=xp1[1]*m; - xp1_temp[2]=xp1[2]; - xp2_temp[0]=xp2[0]*m; - xp2_temp[1]=xp2[1]*m; - xp2_temp[2]=xp2[2]; - - hyp_point=hyp+9*i; - db_StitchCameraRotation_2Points(hyp_point,x1_temp,x2_temp,xp1_temp,xp2_temp); - hyp_point[2]*=ft; - hyp_point[5]*=ft; - hyp_point[6]*=m; - hyp_point[7]*=m; - cost=db_SquaredReprojectionErrorHomography(y,hyp_point,x3); - - if(!done || cost<best_cost) - { - done=1; - best_cost=cost; - best_sol=i; - } - } - - if(f) *f=fsol[best_sol]; - db_Copy9(H,hyp+9*best_sol); - return(1); - } - else - { - db_Identity3x3(H); - if(f) *f=1.0; - return(0); - } -} - -void db_StitchSimilarity2DRaw(double *scale,double R[4],double t[2], - double **Xp,double **X,int nr_points,int orientation_preserving, - int allow_scaling,int allow_rotation,int allow_translation) -{ - int i; - double c[2],cp[2],r[2],rp[2],M[4],s,sp,sc; - double *temp,*temp_p; - double Aacc,Bacc,Aacc2,Bacc2,divisor,divisor2,m,Am,Bm; - - if(allow_translation) - { - db_PointCentroid2D(c,X,nr_points); - db_PointCentroid2D(cp,Xp,nr_points); - } - else - { - db_Zero2(c); - db_Zero2(cp); - } - - db_Zero4(M); - s=sp=0; - for(i=0;i<nr_points;i++) - { - temp= *X++; - temp_p= *Xp++; - r[0]=(*temp++)-c[0]; - r[1]=(*temp++)-c[1]; - rp[0]=(*temp_p++)-cp[0]; - rp[1]=(*temp_p++)-cp[1]; - - M[0]+=r[0]*rp[0]; - M[1]+=r[0]*rp[1]; - M[2]+=r[1]*rp[0]; - M[3]+=r[1]*rp[1]; - - s+=db_sqr(r[0])+db_sqr(r[1]); - sp+=db_sqr(rp[0])+db_sqr(rp[1]); - } - - /*Compute scale*/ - if(allow_scaling) sc=sqrt(db_SafeDivision(sp,s)); - else sc=1.0; - *scale=sc; - - /*Compute rotation*/ - if(allow_rotation) - { - /*orientation preserving*/ - Aacc=M[0]+M[3]; - Bacc=M[2]-M[1]; - /*orientation reversing*/ - Aacc2=M[0]-M[3]; - Bacc2=M[2]+M[1]; - if(Aacc!=0.0 || Bacc!=0.0) - { - divisor=sqrt(Aacc*Aacc+Bacc*Bacc); - m=db_SafeReciprocal(divisor); - Am=Aacc*m; - Bm=Bacc*m; - R[0]= Am; - R[1]= Bm; - R[2]= -Bm; - R[3]= Am; - } - else - { - db_Identity2x2(R); - divisor=0.0; - } - if(!orientation_preserving && (Aacc2!=0.0 || Bacc2!=0.0)) - { - divisor2=sqrt(Aacc2*Aacc2+Bacc2*Bacc2); - if(divisor2>divisor) - { - m=db_SafeReciprocal(divisor2); - Am=Aacc2*m; - Bm=Bacc2*m; - R[0]= Am; - R[1]= Bm; - R[2]= Bm; - R[3]= -Am; - } - } - } - else db_Identity2x2(R); - - /*Compute translation*/ - if(allow_translation) - { - t[0]=cp[0]-sc*(R[0]*c[0]+R[1]*c[1]); - t[1]=cp[1]-sc*(R[2]*c[0]+R[3]*c[1]); - } - else db_Zero2(t); -} - - diff --git a/jni/feature_stab/db_vlvm/db_image_homography.h b/jni/feature_stab/db_vlvm/db_image_homography.h deleted file mode 100644 index 165447dd7..000000000 --- a/jni/feature_stab/db_vlvm/db_image_homography.h +++ /dev/null @@ -1,183 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_image_homography.h,v 1.2 2011/06/17 14:03:31 mbansal Exp $ */ - -#ifndef DB_IMAGE_HOMOGRAPHY -#define DB_IMAGE_HOMOGRAPHY - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ - -#include "db_framestitching.h" -/*! - * \defgroup LMImageHomography (LM) Image Homography Estimation (feature based) - */ -/*\{*/ -/*! -Solve for projective H such that xp~Hx. Prior normalization is not necessary, -although desirable for numerical conditioning -\param H image projective (out) -\param x1 image 1 point 1 -\param x2 image 1 point 2 -\param x3 image 1 point 3 -\param x4 image 1 point 4 -\param xp1 image 2 point 1 -\param xp2 image 2 point 2 -\param xp3 image 2 point 3 -\param xp4 image 2 point 4 -*/ -DB_API void db_StitchProjective2D_4Points(double H[9], - double x1[3],double x2[3],double x3[3],double x4[3], - double xp1[3],double xp2[3],double xp3[3],double xp4[3]); - -/*! -Solve for affine H such that xp~Hx. Prior normalization is not necessary, -although desirable for numerical conditioning -\param H image projective (out) -\param x1 image 1 point 1 -\param x2 image 1 point 2 -\param x3 image 1 point 3 -\param xp1 image 2 point 1 -\param xp2 image 2 point 2 -\param xp3 image 2 point 3 -*/ -DB_API void db_StitchAffine2D_3Points(double H[9], - double x1[3],double x2[3],double x3[3], - double xp1[3],double xp2[3],double xp3[3]); - -/*! -Solve for rotation R such that xp~Rx. -Image points have to be of unit norm for the least squares to be meaningful. -\param R image rotation (out) -\param x1 image 1 point 1 -\param x2 image 1 point 2 -\param xp1 image 2 point 1 -\param xp2 image 2 point 2 -*/ -inline void db_StitchCameraRotation_2Points(double R[9], - /*Image points have to be of unit norm - for the least squares to be meaningful*/ - double x1[3],double x2[3], - double xp1[3],double xp2[3]) -{ - double* x[2]; - double* xp[2]; - double scale,t[3]; - - x[0]=x1; - x[1]=x2; - xp[0]=xp1; - xp[1]=xp2; - db_StitchSimilarity3DRaw(&scale,R,t,xp,x,2,1,0,1,0); -} - -/*! -Solve for a homography H generated by a rotation R with a common unknown focal length f, i.e. -H=diag(f,f,1)*R*diag(1/f,1/f,1) such that xp~Hx. -If signed_disambiguation is true, the points are -required to be in front of the camera. No specific normalization of the homogenous points -is required, although it could be desirable to keep x1,x2,xp1 and xp2 of reasonable magnitude. -If a solution is obtained the function returns 1, otherwise 0. If the focal length is desired -a valid pointer should be passed in f -*/ -DB_API int db_StitchRotationCommonFocalLength_3Points(double H[9],double x1[3],double x2[3],double x3[3], - double xp1[3],double xp2[3],double xp3[3],double *f=0,int signed_disambiguation=1); - -/*! -Find scale, rotation and translation of the similarity that -takes the nr_points inhomogenous 2D points X to Xp, -i.e. for the homogenous equivalents -Xp and X we would have -\code -Xp~ -[sR t]*X -[0 1] -\endcode -If orientation_preserving is true, R is restricted such that det(R)>0. -allow_scaling, allow_rotation and allow_translation allow s,R and t -to differ from 1,Identity and 0 - -Full similarity takes the following on 550MHz: -\code -0.9 microseconds with 2 points -1.0 microseconds with 3 points -1.1 microseconds with 4 points -1.3 microseconds with 5 points -1.4 microseconds with 6 points -1.7 microseconds with 10 points -9 microseconds with 100 points -130 microseconds with 1000 points -1.3 milliseconds with 10000 points -35 milliseconds with 100000 points -350 milliseconds with 1000000 points -\endcode - -Without orientation_preserving: -\code -3 points is minimal for (s,R,t) (R,t) -2 points is minimal for (s,t) (s,R) (R) -1 point is minimal for (s) (t) -\endcode - -With orientation_preserving: -\code -2 points is minimal for (s,R,t) (R,t) (s,t) -1 point is minimal for (s,R) (R) (s) (t) -\endcode -\param scale (out) -\param R 2D rotation (out) -\param t 2D translation (out) -\param Xp (nr_points x 2) pointer to array of image points -\param X (nr_points x 2 ) pointer to array of image points -\param nr_points number of points -\param orientation_preserving -\param allow_scaling compute scale (if 0, scale=1) -\param allow_rotation compute rotation (if 0, R=[I]) -\param allow_translation compute translation (if 0 t = [0,0]') -*/ -DB_API void db_StitchSimilarity2DRaw(double *scale,double R[4],double t[2], - double **Xp,double **X,int nr_points,int orientation_preserving=1, - int allow_scaling=1,int allow_rotation=1,int allow_translation=1); -/*! -See db_StitchRotationCommonFocalLength_3Points(). -\param H Image similarity transformation (out) -\param Xp (nr_points x 2) pointer to array of image points -\param X (nr_points x 2) pointer to array of image points -\param nr_points number of points -\param orientation_preserving -\param allow_scaling compute scale (if 0, scale=1) -\param allow_rotation compute rotation (if 0, R=[I]) -\param allow_translation compute translation (if 0 t = [0,0]') -*/ -inline void db_StitchSimilarity2D(double H[9],double **Xp,double **X,int nr_points,int orientation_preserving=1, - int allow_scaling=1,int allow_rotation=1,int allow_translation=1) -{ - double s,R[4],t[2]; - - db_StitchSimilarity2DRaw(&s,R,t,Xp,X,nr_points,orientation_preserving, - allow_scaling,allow_rotation,allow_translation); - - H[0]=s*R[0]; H[1]=s*R[1]; H[2]=t[0]; - H[3]=s*R[2]; H[4]=s*R[3]; H[5]=t[1]; - db_Zero2(H+6); - H[8]=1.0; -} -/*\}*/ -#endif /* DB_IMAGE_HOMOGRAPHY */ diff --git a/jni/feature_stab/db_vlvm/db_metrics.h b/jni/feature_stab/db_vlvm/db_metrics.h deleted file mode 100644 index 6b95458f1..000000000 --- a/jni/feature_stab/db_vlvm/db_metrics.h +++ /dev/null @@ -1,408 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_metrics.h,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */ - -#ifndef DB_METRICS -#define DB_METRICS - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ - -#include "db_utilities.h" -/*! - * \defgroup LMMetrics (LM) Metrics - */ -/*\{*/ - - - - -/*! -Compute function value fp and Jacobian J of robustifier given input value f*/ -inline void db_CauchyDerivative(double J[4],double fp[2],const double f[2],double one_over_scale2) -{ - double x2,y2,r,r2,r2s,one_over_r2,fu,r_fu,one_over_r_fu; - double one_plus_r2s,half_dfu_dx,half_dfu_dy,coeff,coeff2,coeff3; - int at_zero; - - /*The robustifier takes the input (x,y) and makes a new - vector (xp,yp) where - xp=sqrt(log(1+(x^2+y^2)*one_over_scale2))*x/sqrt(x^2+y^2) - yp=sqrt(log(1+(x^2+y^2)*one_over_scale2))*y/sqrt(x^2+y^2) - The new vector has the property - xp^2+yp^2=log(1+(x^2+y^2)*one_over_scale2) - i.e. when it is square-summed it gives the robust - reprojection error - Define - r2=(x^2+y^2) and - r2s=r2*one_over_scale2 - fu=log(1+r2s)/r2 - then - xp=sqrt(fu)*x - yp=sqrt(fu)*y - and - d(r2)/dx=2x - d(r2)/dy=2y - and - dfu/dx=d(r2)/dx*(r2s/(1+r2s)-log(1+r2s))/(r2*r2) - dfu/dy=d(r2)/dy*(r2s/(1+r2s)-log(1+r2s))/(r2*r2) - and - d(xp)/dx=1/(2sqrt(fu))*(dfu/dx)*x+sqrt(fu) - d(xp)/dy=1/(2sqrt(fu))*(dfu/dy)*x - d(yp)/dx=1/(2sqrt(fu))*(dfu/dx)*y - d(yp)/dy=1/(2sqrt(fu))*(dfu/dy)*y+sqrt(fu) - */ - - x2=db_sqr(f[0]); - y2=db_sqr(f[1]); - r2=x2+y2; - r=sqrt(r2); - - if(r2<=0.0) at_zero=1; - else - { - one_over_r2=1.0/r2; - r2s=r2*one_over_scale2; - one_plus_r2s=1.0+r2s; - fu=log(one_plus_r2s)*one_over_r2; - r_fu=sqrt(fu); - if(r_fu<=0.0) at_zero=1; - else - { - one_over_r_fu=1.0/r_fu; - fp[0]=r_fu*f[0]; - fp[1]=r_fu*f[1]; - /*r2s is always >= 0*/ - coeff=(r2s/one_plus_r2s*one_over_r2-fu)*one_over_r2; - half_dfu_dx=f[0]*coeff; - half_dfu_dy=f[1]*coeff; - coeff2=one_over_r_fu*half_dfu_dx; - coeff3=one_over_r_fu*half_dfu_dy; - - J[0]=coeff2*f[0]+r_fu; - J[1]=coeff3*f[0]; - J[2]=coeff2*f[1]; - J[3]=coeff3*f[1]+r_fu; - at_zero=0; - } - } - if(at_zero) - { - /*Close to zero the robustifying mapping - becomes identity*sqrt(one_over_scale2)*/ - fp[0]=0.0; - fp[1]=0.0; - J[0]=sqrt(one_over_scale2); - J[1]=0.0; - J[2]=0.0; - J[3]=J[0]; - } -} - -inline double db_SquaredReprojectionErrorHomography(const double y[2],const double H[9],const double x[3]) -{ - double x0,x1,x2,mult; - double sd; - - x0=H[0]*x[0]+H[1]*x[1]+H[2]*x[2]; - x1=H[3]*x[0]+H[4]*x[1]+H[5]*x[2]; - x2=H[6]*x[0]+H[7]*x[1]+H[8]*x[2]; - mult=1.0/((x2!=0.0)?x2:1.0); - sd=db_sqr((y[0]-x0*mult))+db_sqr((y[1]-x1*mult)); - - return(sd); -} - -inline double db_SquaredInhomogenousHomographyError(const double y[2],const double H[9],const double x[2]) -{ - double x0,x1,x2,mult; - double sd; - - x0=H[0]*x[0]+H[1]*x[1]+H[2]; - x1=H[3]*x[0]+H[4]*x[1]+H[5]; - x2=H[6]*x[0]+H[7]*x[1]+H[8]; - mult=1.0/((x2!=0.0)?x2:1.0); - sd=db_sqr((y[0]-x0*mult))+db_sqr((y[1]-x1*mult)); - - return(sd); -} - -/*! -Return a constant divided by likelihood of a Cauchy distributed -reprojection error given the image point y, homography H, image point -point x and the squared scale coefficient one_over_scale2=1.0/(scale*scale) -where scale is the half width at half maximum (hWahM) of the -Cauchy distribution*/ -inline double db_ExpCauchyInhomogenousHomographyError(const double y[2],const double H[9],const double x[2], - double one_over_scale2) -{ - double sd; - sd=db_SquaredInhomogenousHomographyError(y,H,x); - return(1.0+sd*one_over_scale2); -} - -/*! -Compute residual vector f between image point y and homography Hx of -image point x. Also compute Jacobian of f with respect -to an update dx of H*/ -inline void db_DerivativeInhomHomographyError(double Jf_dx[18],double f[2],const double y[2],const double H[9], - const double x[2]) -{ - double xh,yh,zh,mult,mult2,xh_mult2,yh_mult2; - /*The Jacobian of the inhomogenous coordinates with respect to - the homogenous is - [1/zh 0 -xh/(zh*zh)] - [ 0 1/zh -yh/(zh*zh)] - The Jacobian of the homogenous coordinates with respect to dH is - [x0 x1 1 0 0 0 0 0 0] - [ 0 0 0 x0 x1 1 0 0 0] - [ 0 0 0 0 0 0 x0 x1 1] - The output Jacobian is minus their product, i.e. - [-x0/zh -x1/zh -1/zh 0 0 0 x0*xh/(zh*zh) x1*xh/(zh*zh) xh/(zh*zh)] - [ 0 0 0 -x0/zh -x1/zh -1/zh x0*yh/(zh*zh) x1*yh/(zh*zh) yh/(zh*zh)]*/ - - /*Compute warped point, which is the same as - homogenous coordinates of reprojection*/ - xh=H[0]*x[0]+H[1]*x[1]+H[2]; - yh=H[3]*x[0]+H[4]*x[1]+H[5]; - zh=H[6]*x[0]+H[7]*x[1]+H[8]; - mult=1.0/((zh!=0.0)?zh:1.0); - /*Compute inhomogenous residual*/ - f[0]=y[0]-xh*mult; - f[1]=y[1]-yh*mult; - /*Compute Jacobian*/ - mult2=mult*mult; - xh_mult2=xh*mult2; - yh_mult2=yh*mult2; - Jf_dx[0]= -x[0]*mult; - Jf_dx[1]= -x[1]*mult; - Jf_dx[2]= -mult; - Jf_dx[3]=0; - Jf_dx[4]=0; - Jf_dx[5]=0; - Jf_dx[6]=x[0]*xh_mult2; - Jf_dx[7]=x[1]*xh_mult2; - Jf_dx[8]=xh_mult2; - Jf_dx[9]=0; - Jf_dx[10]=0; - Jf_dx[11]=0; - Jf_dx[12]=Jf_dx[0]; - Jf_dx[13]=Jf_dx[1]; - Jf_dx[14]=Jf_dx[2]; - Jf_dx[15]=x[0]*yh_mult2; - Jf_dx[16]=x[1]*yh_mult2; - Jf_dx[17]=yh_mult2; -} - -/*! -Compute robust residual vector f between image point y and homography Hx of -image point x. Also compute Jacobian of f with respect -to an update dH of H*/ -inline void db_DerivativeCauchyInhomHomographyReprojection(double Jf_dx[18],double f[2],const double y[2],const double H[9], - const double x[2],double one_over_scale2) -{ - double Jf_dx_loc[18],f_loc[2]; - double J[4],J0,J1,J2,J3; - - /*Compute reprojection Jacobian*/ - db_DerivativeInhomHomographyError(Jf_dx_loc,f_loc,y,H,x); - /*Compute robustifier Jacobian*/ - db_CauchyDerivative(J,f,f_loc,one_over_scale2); - - /*Multiply the robustifier Jacobian with - the reprojection Jacobian*/ - J0=J[0];J1=J[1];J2=J[2];J3=J[3]; - Jf_dx[0]=J0*Jf_dx_loc[0]; - Jf_dx[1]=J0*Jf_dx_loc[1]; - Jf_dx[2]=J0*Jf_dx_loc[2]; - Jf_dx[3]= J1*Jf_dx_loc[12]; - Jf_dx[4]= J1*Jf_dx_loc[13]; - Jf_dx[5]= J1*Jf_dx_loc[14]; - Jf_dx[6]=J0*Jf_dx_loc[6]+J1*Jf_dx_loc[15]; - Jf_dx[7]=J0*Jf_dx_loc[7]+J1*Jf_dx_loc[16]; - Jf_dx[8]=J0*Jf_dx_loc[8]+J1*Jf_dx_loc[17]; - Jf_dx[9]= J2*Jf_dx_loc[0]; - Jf_dx[10]=J2*Jf_dx_loc[1]; - Jf_dx[11]=J2*Jf_dx_loc[2]; - Jf_dx[12]= J3*Jf_dx_loc[12]; - Jf_dx[13]= J3*Jf_dx_loc[13]; - Jf_dx[14]= J3*Jf_dx_loc[14]; - Jf_dx[15]=J2*Jf_dx_loc[6]+J3*Jf_dx_loc[15]; - Jf_dx[16]=J2*Jf_dx_loc[7]+J3*Jf_dx_loc[16]; - Jf_dx[17]=J2*Jf_dx_loc[8]+J3*Jf_dx_loc[17]; -} -/*! -Compute residual vector f between image point y and rotation of -image point x by R. Also compute Jacobian of f with respect -to an update dx of R*/ -inline void db_DerivativeInhomRotationReprojection(double Jf_dx[6],double f[2],const double y[2],const double R[9], - const double x[2]) -{ - double xh,yh,zh,mult,mult2,xh_mult2,yh_mult2; - /*The Jacobian of the inhomogenous coordinates with respect to - the homogenous is - [1/zh 0 -xh/(zh*zh)] - [ 0 1/zh -yh/(zh*zh)] - The Jacobian at zero of the homogenous coordinates with respect to - [sin(phi) sin(ohm) sin(kap)] is - [-rx2 0 rx1 ] - [ 0 rx2 -rx0 ] - [ rx0 -rx1 0 ] - The output Jacobian is minus their product, i.e. - [1+xh*xh/(zh*zh) -xh*yh/(zh*zh) -yh/zh] - [xh*yh/(zh*zh) -1-yh*yh/(zh*zh) xh/zh]*/ - - /*Compute rotated point, which is the same as - homogenous coordinates of reprojection*/ - xh=R[0]*x[0]+R[1]*x[1]+R[2]; - yh=R[3]*x[0]+R[4]*x[1]+R[5]; - zh=R[6]*x[0]+R[7]*x[1]+R[8]; - mult=1.0/((zh!=0.0)?zh:1.0); - /*Compute inhomogenous residual*/ - f[0]=y[0]-xh*mult; - f[1]=y[1]-yh*mult; - /*Compute Jacobian*/ - mult2=mult*mult; - xh_mult2=xh*mult2; - yh_mult2=yh*mult2; - Jf_dx[0]= 1.0+xh*xh_mult2; - Jf_dx[1]= -yh*xh_mult2; - Jf_dx[2]= -yh*mult; - Jf_dx[3]= -Jf_dx[1]; - Jf_dx[4]= -1-yh*yh_mult2; - Jf_dx[5]= xh*mult; -} - -/*! -Compute robust residual vector f between image point y and rotation of -image point x by R. Also compute Jacobian of f with respect -to an update dx of R*/ -inline void db_DerivativeCauchyInhomRotationReprojection(double Jf_dx[6],double f[2],const double y[2],const double R[9], - const double x[2],double one_over_scale2) -{ - double Jf_dx_loc[6],f_loc[2]; - double J[4],J0,J1,J2,J3; - - /*Compute reprojection Jacobian*/ - db_DerivativeInhomRotationReprojection(Jf_dx_loc,f_loc,y,R,x); - /*Compute robustifier Jacobian*/ - db_CauchyDerivative(J,f,f_loc,one_over_scale2); - - /*Multiply the robustifier Jacobian with - the reprojection Jacobian*/ - J0=J[0];J1=J[1];J2=J[2];J3=J[3]; - Jf_dx[0]=J0*Jf_dx_loc[0]+J1*Jf_dx_loc[3]; - Jf_dx[1]=J0*Jf_dx_loc[1]+J1*Jf_dx_loc[4]; - Jf_dx[2]=J0*Jf_dx_loc[2]+J1*Jf_dx_loc[5]; - Jf_dx[3]=J2*Jf_dx_loc[0]+J3*Jf_dx_loc[3]; - Jf_dx[4]=J2*Jf_dx_loc[1]+J3*Jf_dx_loc[4]; - Jf_dx[5]=J2*Jf_dx_loc[2]+J3*Jf_dx_loc[5]; -} - - - -/*! -// remove the outliers whose projection error is larger than pre-defined -*/ -inline int db_RemoveOutliers_Homography(const double H[9], double *x_i,double *xp_i, double *wp,double *im, double *im_p, double *im_r, double *im_raw,double *im_raw_p,int point_count,double scale, double thresh=DB_OUTLIER_THRESHOLD) -{ - double temp_valueE, t2; - int c; - int k1=0; - int k2=0; - int k3=0; - int numinliers=0; - int ind1; - int ind2; - int ind3; - int isinlier; - - // experimentally determined - t2=1.0/(thresh*thresh*thresh*thresh); - - // count the inliers - for(c=0;c<point_count;c++) - { - ind1=c<<1; - ind2=c<<2; - ind3=3*c; - - temp_valueE=db_SquaredInhomogenousHomographyError(im_p+ind3,H,im+ind3); - - isinlier=((temp_valueE<=t2)?1:0); - - // if it is inlier, then copy the 3d and 2d correspondences - if (isinlier) - { - numinliers++; - - x_i[k1]=x_i[ind1]; - x_i[k1+1]=x_i[ind1+1]; - - xp_i[k1]=xp_i[ind1]; - xp_i[k1+1]=xp_i[ind1+1]; - - k1=k1+2; - - // original normalized pixel coordinates - im[k3]=im[ind3]; - im[k3+1]=im[ind3+1]; - im[k3+2]=im[ind3+2]; - - im_r[k3]=im_r[ind3]; - im_r[k3+1]=im_r[ind3+1]; - im_r[k3+2]=im_r[ind3+2]; - - im_p[k3]=im_p[ind3]; - im_p[k3+1]=im_p[ind3+1]; - im_p[k3+2]=im_p[ind3+2]; - - // left and right raw pixel coordinates - im_raw[k3] = im_raw[ind3]; - im_raw[k3+1] = im_raw[ind3+1]; - im_raw[k3+2] = im_raw[ind3+2]; // the index - - im_raw_p[k3] = im_raw_p[ind3]; - im_raw_p[k3+1] = im_raw_p[ind3+1]; - im_raw_p[k3+2] = im_raw_p[ind3+2]; // the index - - k3=k3+3; - - // 3D coordinates - wp[k2]=wp[ind2]; - wp[k2+1]=wp[ind2+1]; - wp[k2+2]=wp[ind2+2]; - wp[k2+3]=wp[ind2+3]; - - k2=k2+4; - - } - } - - return numinliers; -} - - - - - -/*\}*/ - -#endif /* DB_METRICS */ diff --git a/jni/feature_stab/db_vlvm/db_rob_image_homography.cpp b/jni/feature_stab/db_vlvm/db_rob_image_homography.cpp deleted file mode 100644 index 82dec0cbe..000000000 --- a/jni/feature_stab/db_vlvm/db_rob_image_homography.cpp +++ /dev/null @@ -1,1082 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_rob_image_homography.cpp,v 1.2 2011/06/17 14:03:31 mbansal Exp $ */ - -#include "db_utilities.h" -#include "db_rob_image_homography.h" -#include "db_bundle.h" - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ - -#include "db_image_homography.h" - -#ifdef _VERBOSE_ -#include <iostream> -using namespace std; -#endif /*VERBOSE*/ - -inline double db_RobImageHomography_Cost(double H[9],int point_count,double *x_i,double *xp_i,double one_over_scale2) -{ - int c; - double back,acc,*x_i_temp,*xp_i_temp; - - for(back=0.0,c=0;c<point_count;) - { - /*Take log of product of ten reprojection - errors to reduce nr of expensive log operations*/ - if(c+9<point_count) - { - x_i_temp=x_i+(c<<1); - xp_i_temp=xp_i+(c<<1); - - acc=db_ExpCauchyInhomogenousHomographyError(xp_i_temp,H,x_i_temp,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+2,H,x_i_temp+2,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+4,H,x_i_temp+4,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+6,H,x_i_temp+6,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+8,H,x_i_temp+8,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+10,H,x_i_temp+10,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+12,H,x_i_temp+12,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+14,H,x_i_temp+14,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+16,H,x_i_temp+16,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+18,H,x_i_temp+18,one_over_scale2); - c+=10; - } - else - { - for(acc=1.0;c<point_count;c++) - { - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i+(c<<1),H,x_i+(c<<1),one_over_scale2); - } - } - back+=log(acc); - } - return(back); -} - -inline double db_RobImageHomography_Statistics(double H[9],int point_count,double *x_i,double *xp_i,double one_over_scale2,db_Statistics *stat,double thresh=DB_OUTLIER_THRESHOLD) -{ - int c,i; - double t2,frac; - - t2=thresh*thresh; - for(i=0,c=0;c<point_count;c++) - { - i+=(db_SquaredInhomogenousHomographyError(xp_i+(c<<1),H,x_i+(c<<1))*one_over_scale2<=t2)?1:0; - } - frac=((double)i)/((double)(db_maxi(point_count,1))); - -#ifdef _VERBOSE_ - std::cout << "Inlier Percentage RobImageHomography: " << frac*100.0 << "% out of " << point_count << " constraints" << std::endl; -#endif /*_VERBOSE_*/ - - if(stat) - { - stat->nr_points=point_count; - stat->one_over_scale2=one_over_scale2; - stat->nr_inliers=i; - stat->inlier_fraction=frac; - - stat->cost=db_RobImageHomography_Cost(H,point_count,x_i,xp_i,one_over_scale2); - stat->model_dimension=0; - /*stat->nr_parameters=;*/ - - stat->lambda1=log(4.0); - stat->lambda2=log(4.0*((double)db_maxi(1,stat->nr_points))); - stat->lambda3=10.0; - stat->gric=stat->cost+stat->lambda1*stat->model_dimension*((double)stat->nr_points)+stat->lambda2*((double)stat->nr_parameters); - stat->inlier_evidence=((double)stat->nr_inliers)-stat->lambda3*((double)stat->nr_parameters); - } - - return(frac); -} - -/*Compute min_Jtf and upper right of JtJ. Return cost.*/ -inline double db_RobImageHomography_Jacobians(double JtJ[81],double min_Jtf[9],double H[9],int point_count,double *x_i,double *xp_i,double one_over_scale2) -{ - double back,Jf_dx[18],f[2],temp,temp2; - int i; - - db_Zero(JtJ,81); - db_Zero(min_Jtf,9); - for(back=0.0,i=0;i<point_count;i++) - { - /*Compute reprojection error vector and its Jacobian - for this point*/ - db_DerivativeCauchyInhomHomographyReprojection(Jf_dx,f,xp_i+(i<<1),H,x_i+(i<<1),one_over_scale2); - /*Perform - min_Jtf-=Jf_dx*f[0] and - min_Jtf-=(Jf_dx+9)*f[1] to accumulate -Jt%f*/ - db_RowOperation9(min_Jtf,Jf_dx,f[0]); - db_RowOperation9(min_Jtf,Jf_dx+9,f[1]); - /*Accumulate upper right of JtJ with outer product*/ - temp=Jf_dx[0]; temp2=Jf_dx[9]; - JtJ[0]+=temp*Jf_dx[0]+temp2*Jf_dx[9]; - JtJ[1]+=temp*Jf_dx[1]+temp2*Jf_dx[10]; - JtJ[2]+=temp*Jf_dx[2]+temp2*Jf_dx[11]; - JtJ[3]+=temp*Jf_dx[3]+temp2*Jf_dx[12]; - JtJ[4]+=temp*Jf_dx[4]+temp2*Jf_dx[13]; - JtJ[5]+=temp*Jf_dx[5]+temp2*Jf_dx[14]; - JtJ[6]+=temp*Jf_dx[6]+temp2*Jf_dx[15]; - JtJ[7]+=temp*Jf_dx[7]+temp2*Jf_dx[16]; - JtJ[8]+=temp*Jf_dx[8]+temp2*Jf_dx[17]; - temp=Jf_dx[1]; temp2=Jf_dx[10]; - JtJ[10]+=temp*Jf_dx[1]+temp2*Jf_dx[10]; - JtJ[11]+=temp*Jf_dx[2]+temp2*Jf_dx[11]; - JtJ[12]+=temp*Jf_dx[3]+temp2*Jf_dx[12]; - JtJ[13]+=temp*Jf_dx[4]+temp2*Jf_dx[13]; - JtJ[14]+=temp*Jf_dx[5]+temp2*Jf_dx[14]; - JtJ[15]+=temp*Jf_dx[6]+temp2*Jf_dx[15]; - JtJ[16]+=temp*Jf_dx[7]+temp2*Jf_dx[16]; - JtJ[17]+=temp*Jf_dx[8]+temp2*Jf_dx[17]; - temp=Jf_dx[2]; temp2=Jf_dx[11]; - JtJ[20]+=temp*Jf_dx[2]+temp2*Jf_dx[11]; - JtJ[21]+=temp*Jf_dx[3]+temp2*Jf_dx[12]; - JtJ[22]+=temp*Jf_dx[4]+temp2*Jf_dx[13]; - JtJ[23]+=temp*Jf_dx[5]+temp2*Jf_dx[14]; - JtJ[24]+=temp*Jf_dx[6]+temp2*Jf_dx[15]; - JtJ[25]+=temp*Jf_dx[7]+temp2*Jf_dx[16]; - JtJ[26]+=temp*Jf_dx[8]+temp2*Jf_dx[17]; - temp=Jf_dx[3]; temp2=Jf_dx[12]; - JtJ[30]+=temp*Jf_dx[3]+temp2*Jf_dx[12]; - JtJ[31]+=temp*Jf_dx[4]+temp2*Jf_dx[13]; - JtJ[32]+=temp*Jf_dx[5]+temp2*Jf_dx[14]; - JtJ[33]+=temp*Jf_dx[6]+temp2*Jf_dx[15]; - JtJ[34]+=temp*Jf_dx[7]+temp2*Jf_dx[16]; - JtJ[35]+=temp*Jf_dx[8]+temp2*Jf_dx[17]; - temp=Jf_dx[4]; temp2=Jf_dx[13]; - JtJ[40]+=temp*Jf_dx[4]+temp2*Jf_dx[13]; - JtJ[41]+=temp*Jf_dx[5]+temp2*Jf_dx[14]; - JtJ[42]+=temp*Jf_dx[6]+temp2*Jf_dx[15]; - JtJ[43]+=temp*Jf_dx[7]+temp2*Jf_dx[16]; - JtJ[44]+=temp*Jf_dx[8]+temp2*Jf_dx[17]; - temp=Jf_dx[5]; temp2=Jf_dx[14]; - JtJ[50]+=temp*Jf_dx[5]+temp2*Jf_dx[14]; - JtJ[51]+=temp*Jf_dx[6]+temp2*Jf_dx[15]; - JtJ[52]+=temp*Jf_dx[7]+temp2*Jf_dx[16]; - JtJ[53]+=temp*Jf_dx[8]+temp2*Jf_dx[17]; - temp=Jf_dx[6]; temp2=Jf_dx[15]; - JtJ[60]+=temp*Jf_dx[6]+temp2*Jf_dx[15]; - JtJ[61]+=temp*Jf_dx[7]+temp2*Jf_dx[16]; - JtJ[62]+=temp*Jf_dx[8]+temp2*Jf_dx[17]; - temp=Jf_dx[7]; temp2=Jf_dx[16]; - JtJ[70]+=temp*Jf_dx[7]+temp2*Jf_dx[16]; - JtJ[71]+=temp*Jf_dx[8]+temp2*Jf_dx[17]; - temp=Jf_dx[8]; temp2=Jf_dx[17]; - JtJ[80]+=temp*Jf_dx[8]+temp2*Jf_dx[17]; - - /*Add square-sum to cost*/ - back+=db_sqr(f[0])+db_sqr(f[1]); - } - - return(back); -} - -/*Compute min_Jtf and upper right of JtJ. Return cost*/ -inline double db_RobCamRotation_Jacobians(double JtJ[9],double min_Jtf[3],double H[9],int point_count,double *x_i,double *xp_i,double one_over_scale2) -{ - double back,Jf_dx[6],f[2]; - int i,j; - - db_Zero(JtJ,9); - db_Zero(min_Jtf,3); - for(back=0.0,i=0;i<point_count;i++) - { - /*Compute reprojection error vector and its Jacobian - for this point*/ - j=(i<<1); - db_DerivativeCauchyInhomRotationReprojection(Jf_dx,f,xp_i+j,H,x_i+j,one_over_scale2); - /*Perform - min_Jtf-=Jf_dx*f[0] and - min_Jtf-=(Jf_dx+3)*f[1] to accumulate -Jt%f*/ - db_RowOperation3(min_Jtf,Jf_dx,f[0]); - db_RowOperation3(min_Jtf,Jf_dx+3,f[1]); - /*Accumulate upper right of JtJ with outer product*/ - JtJ[0]+=Jf_dx[0]*Jf_dx[0]+Jf_dx[3]*Jf_dx[3]; - JtJ[1]+=Jf_dx[0]*Jf_dx[1]+Jf_dx[3]*Jf_dx[4]; - JtJ[2]+=Jf_dx[0]*Jf_dx[2]+Jf_dx[3]*Jf_dx[5]; - JtJ[4]+=Jf_dx[1]*Jf_dx[1]+Jf_dx[4]*Jf_dx[4]; - JtJ[5]+=Jf_dx[1]*Jf_dx[2]+Jf_dx[4]*Jf_dx[5]; - JtJ[8]+=Jf_dx[2]*Jf_dx[2]+Jf_dx[5]*Jf_dx[5]; - - /*Add square-sum to cost*/ - back+=db_sqr(f[0])+db_sqr(f[1]); - } - - return(back); -} - -void db_RobCamRotation_Polish(double H[9],int point_count,double *x_i,double *xp_i,double one_over_scale2, - int max_iterations,double improvement_requirement) -{ - int i,update,stop; - double lambda,cost,current_cost; - double JtJ[9],min_Jtf[3],dx[3],H_p_dx[9]; - - lambda=0.001; - for(update=1,stop=0,i=0;(stop<2) && (i<max_iterations);i++) - { - /*if first time since improvement, compute Jacobian and residual*/ - if(update) - { - current_cost=db_RobCamRotation_Jacobians(JtJ,min_Jtf,H,point_count,x_i,xp_i,one_over_scale2); - update=0; - } - -#ifdef _VERBOSE_ - /*std::cout << "Cost:" << current_cost << " ";*/ -#endif /*_VERBOSE_*/ - - /*Come up with a hypothesis dx - based on the current lambda*/ - db_Compute_dx_3x3(dx,JtJ,min_Jtf,lambda); - - /*Compute Cost(x+dx)*/ - db_UpdateRotation(H_p_dx,H,dx); - cost=db_RobImageHomography_Cost(H_p_dx,point_count,x_i,xp_i,one_over_scale2); - - /*Is there an improvement?*/ - if(cost<current_cost) - { - /*improvement*/ - if(current_cost-cost<current_cost*improvement_requirement) stop++; - else stop=0; - lambda*=0.1; - /*Move to the hypothesised position x+dx*/ - current_cost=cost; - db_Copy9(H,H_p_dx); - db_OrthonormalizeRotation(H); - update=1; - -#ifdef _VERBOSE_ - std::cout << "Step" << i << "Imp,Lambda=" << lambda << "Cost:" << current_cost << std::endl; -#endif /*_VERBOSE_*/ - } - else - { - /*no improvement*/ - lambda*=10.0; - stop=0; - } - } -} - -inline void db_RobImageHomographyFetchJacobian(double **JtJ_ref,double *min_Jtf,double **JtJ_temp_ref,double *min_Jtf_temp,int n,int *fetch_vector) -{ - int i,j,t; - double *t1,*t2; - - for(i=0;i<n;i++) - { - t=fetch_vector[i]; - min_Jtf[i]=min_Jtf_temp[t]; - t1=JtJ_ref[i]; - t2=JtJ_temp_ref[t]; - for(j=i;j<n;j++) - { - t1[j]=t2[fetch_vector[j]]; - } - } -} - -inline void db_RobImageHomographyMultiplyJacobian(double **JtJ_ref,double *min_Jtf,double **JtJ_temp_ref,double *min_Jtf_temp,double **JE_dx_ref,int n) -{ - double JtJ_JE[72],*JtJ_JE_ref[9]; - - db_SetupMatrixRefs(JtJ_JE_ref,9,8,JtJ_JE); - - db_SymmetricExtendUpperToLower(JtJ_temp_ref,9,9); - db_MultiplyMatricesAB(JtJ_JE_ref,JtJ_temp_ref,JE_dx_ref,9,9,n); - db_UpperMultiplyMatricesAtB(JtJ_ref,JE_dx_ref,JtJ_JE_ref,n,9,n); - db_MultiplyMatrixVectorAtb(min_Jtf,JE_dx_ref,min_Jtf_temp,n,9); -} - -inline void db_RobImageHomographyJH_Js(double **JE_dx_ref,int j,double H[9]) -{ - /*Update of upper 2x2 is multiplication by - [s 0][ cos(theta) sin(theta)] - [0 s][-sin(theta) cos(theta)]*/ - JE_dx_ref[0][j]=H[0]; - JE_dx_ref[1][j]=H[1]; - JE_dx_ref[2][j]=0; - JE_dx_ref[3][j]=H[2]; - JE_dx_ref[4][j]=H[3]; - JE_dx_ref[5][j]=0; - JE_dx_ref[6][j]=0; - JE_dx_ref[7][j]=0; - JE_dx_ref[8][j]=0; -} - -inline void db_RobImageHomographyJH_JR(double **JE_dx_ref,int j,double H[9]) -{ - /*Update of upper 2x2 is multiplication by - [s 0][ cos(theta) sin(theta)] - [0 s][-sin(theta) cos(theta)]*/ - JE_dx_ref[0][j]= H[3]; - JE_dx_ref[1][j]= H[4]; - JE_dx_ref[2][j]=0; - JE_dx_ref[3][j]= -H[0]; - JE_dx_ref[4][j]= -H[1]; - JE_dx_ref[5][j]=0; - JE_dx_ref[6][j]=0; - JE_dx_ref[7][j]=0; - JE_dx_ref[8][j]=0; -} - -inline void db_RobImageHomographyJH_Jt(double **JE_dx_ref,int j,int k,double H[9]) -{ - JE_dx_ref[0][j]=0; - JE_dx_ref[1][j]=0; - JE_dx_ref[2][j]=1.0; - JE_dx_ref[3][j]=0; - JE_dx_ref[4][j]=0; - JE_dx_ref[5][j]=0; - JE_dx_ref[6][j]=0; - JE_dx_ref[7][j]=0; - JE_dx_ref[8][j]=0; - - JE_dx_ref[0][k]=0; - JE_dx_ref[1][k]=0; - JE_dx_ref[2][k]=0; - JE_dx_ref[3][k]=0; - JE_dx_ref[4][k]=0; - JE_dx_ref[5][k]=1.0; - JE_dx_ref[6][k]=0; - JE_dx_ref[7][k]=0; - JE_dx_ref[8][k]=0; -} - -inline void db_RobImageHomographyJH_dRotFocal(double **JE_dx_ref,int j,int k,int l,int m,double H[9]) -{ - double f,fi,fi2; - double R[9],J[9]; - - /*Updated matrix is diag(f+df,f+df)*dR*R*diag(1/(f+df),1/(f+df),1)*/ - f=db_FocalAndRotFromCamRotFocalHomography(R,H); - fi=db_SafeReciprocal(f); - fi2=db_sqr(fi); - db_JacobianOfRotatedPointStride(J,R,3); - JE_dx_ref[0][j]= J[0]; - JE_dx_ref[1][j]= J[1]; - JE_dx_ref[2][j]=f* J[2]; - JE_dx_ref[3][j]= J[3]; - JE_dx_ref[4][j]= J[4]; - JE_dx_ref[5][j]=f* J[5]; - JE_dx_ref[6][j]=fi*J[6]; - JE_dx_ref[7][j]=fi*J[7]; - JE_dx_ref[8][j]= J[8]; - db_JacobianOfRotatedPointStride(J,R+1,3); - JE_dx_ref[0][k]= J[0]; - JE_dx_ref[1][k]= J[1]; - JE_dx_ref[2][k]=f* J[2]; - JE_dx_ref[3][k]= J[3]; - JE_dx_ref[4][k]= J[4]; - JE_dx_ref[5][k]=f* J[5]; - JE_dx_ref[6][k]=fi*J[6]; - JE_dx_ref[7][k]=fi*J[7]; - JE_dx_ref[8][k]= J[8]; - db_JacobianOfRotatedPointStride(J,R+2,3); - JE_dx_ref[0][l]= J[0]; - JE_dx_ref[1][l]= J[1]; - JE_dx_ref[2][l]=f* J[2]; - JE_dx_ref[3][l]= J[3]; - JE_dx_ref[4][l]= J[4]; - JE_dx_ref[5][l]=f* J[5]; - JE_dx_ref[6][l]=fi*J[6]; - JE_dx_ref[7][l]=fi*J[7]; - JE_dx_ref[8][l]= J[8]; - - JE_dx_ref[0][m]=0; - JE_dx_ref[1][m]=0; - JE_dx_ref[2][m]=H[2]; - JE_dx_ref[3][m]=0; - JE_dx_ref[4][m]=0; - JE_dx_ref[5][m]=H[5]; - JE_dx_ref[6][m]= -fi2*H[6]; - JE_dx_ref[7][m]= -fi2*H[7]; - JE_dx_ref[8][m]=0; -} - -inline double db_RobImageHomography_Jacobians_Generic(double *JtJ_ref[8],double min_Jtf[8],int *num_param,int *frozen_coord,double H[9],int point_count,double *x_i,double *xp_i,int homography_type,double one_over_scale2) -{ - double back; - int i,j,fetch_vector[8],n; - double JtJ_temp[81],min_Jtf_temp[9],JE_dx[72]; - double *JE_dx_ref[9],*JtJ_temp_ref[9]; - - /*Compute cost and JtJ,min_Jtf with respect to H*/ - back=db_RobImageHomography_Jacobians(JtJ_temp,min_Jtf_temp,H,point_count,x_i,xp_i,one_over_scale2); - - /*Compute JtJ,min_Jtf with respect to the right parameters - The formulas are - JtJ=transpose(JE_dx)*JtJ*JE_dx and - min_Jtf=transpose(JE_dx)*min_Jtf, - where the 9xN matrix JE_dx is the Jacobian of H with respect - to the update*/ - db_SetupMatrixRefs(JtJ_temp_ref,9,9,JtJ_temp); - db_SetupMatrixRefs(JE_dx_ref,9,8,JE_dx); - switch(homography_type) - { - case DB_HOMOGRAPHY_TYPE_SIMILARITY: - case DB_HOMOGRAPHY_TYPE_SIMILARITY_U: - n=4; - db_RobImageHomographyJH_Js(JE_dx_ref,0,H); - db_RobImageHomographyJH_JR(JE_dx_ref,1,H); - db_RobImageHomographyJH_Jt(JE_dx_ref,2,3,H); - db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n); - break; - case DB_HOMOGRAPHY_TYPE_ROTATION: - case DB_HOMOGRAPHY_TYPE_ROTATION_U: - n=1; - db_RobImageHomographyJH_JR(JE_dx_ref,0,H); - db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n); - break; - case DB_HOMOGRAPHY_TYPE_SCALING: - n=1; - db_RobImageHomographyJH_Js(JE_dx_ref,0,H); - db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n); - break; - case DB_HOMOGRAPHY_TYPE_S_T: - n=3; - db_RobImageHomographyJH_Js(JE_dx_ref,0,H); - db_RobImageHomographyJH_Jt(JE_dx_ref,1,2,H); - db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n); - break; - case DB_HOMOGRAPHY_TYPE_R_T: - n=3; - db_RobImageHomographyJH_JR(JE_dx_ref,0,H); - db_RobImageHomographyJH_Jt(JE_dx_ref,1,2,H); - db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n); - break; - case DB_HOMOGRAPHY_TYPE_R_S: - n=2; - db_RobImageHomographyJH_Js(JE_dx_ref,0,H); - db_RobImageHomographyJH_JR(JE_dx_ref,1,H); - db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n); - break; - - case DB_HOMOGRAPHY_TYPE_TRANSLATION: - n=2; - fetch_vector[0]=2; - fetch_vector[1]=5; - db_RobImageHomographyFetchJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,n,fetch_vector); - break; - case DB_HOMOGRAPHY_TYPE_AFFINE: - n=6; - fetch_vector[0]=0; - fetch_vector[1]=1; - fetch_vector[2]=2; - fetch_vector[3]=3; - fetch_vector[4]=4; - fetch_vector[5]=5; - db_RobImageHomographyFetchJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,n,fetch_vector); - break; - case DB_HOMOGRAPHY_TYPE_PROJECTIVE: - n=8; - *frozen_coord=db_MaxAbsIndex9(H); - for(j=0,i=0;i<9;i++) if(i!=(*frozen_coord)) - { - fetch_vector[j]=i; - j++; - } - db_RobImageHomographyFetchJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,n,fetch_vector); - break; - case DB_HOMOGRAPHY_TYPE_CAMROTATION_F: - case DB_HOMOGRAPHY_TYPE_CAMROTATION_F_UD: - n=4; - db_RobImageHomographyJH_dRotFocal(JE_dx_ref,0,1,2,3,H); - db_RobImageHomographyMultiplyJacobian(JtJ_ref,min_Jtf,JtJ_temp_ref,min_Jtf_temp,JE_dx_ref,n); - break; - } - *num_param=n; - - return(back); -} - -inline void db_ImageHomographyUpdateGeneric(double H_p_dx[9],double H[9],double *dx,int homography_type,int frozen_coord) -{ - switch(homography_type) - { - case DB_HOMOGRAPHY_TYPE_SIMILARITY: - case DB_HOMOGRAPHY_TYPE_SIMILARITY_U: - db_Copy9(H_p_dx,H); - db_MultiplyScaleOntoImageHomography(H,1.0+dx[0]); - db_MultiplyRotationOntoImageHomography(H,dx[1]); - H_p_dx[2]+=dx[2]; - H_p_dx[5]+=dx[3]; - break; - case DB_HOMOGRAPHY_TYPE_ROTATION: - case DB_HOMOGRAPHY_TYPE_ROTATION_U: - db_MultiplyRotationOntoImageHomography(H,dx[0]); - break; - case DB_HOMOGRAPHY_TYPE_SCALING: - db_MultiplyScaleOntoImageHomography(H,1.0+dx[0]); - break; - case DB_HOMOGRAPHY_TYPE_S_T: - db_Copy9(H_p_dx,H); - db_MultiplyScaleOntoImageHomography(H,1.0+dx[0]); - H_p_dx[2]+=dx[1]; - H_p_dx[5]+=dx[2]; - break; - case DB_HOMOGRAPHY_TYPE_R_T: - db_Copy9(H_p_dx,H); - db_MultiplyRotationOntoImageHomography(H,dx[0]); - H_p_dx[2]+=dx[1]; - H_p_dx[5]+=dx[2]; - break; - case DB_HOMOGRAPHY_TYPE_R_S: - db_Copy9(H_p_dx,H); - db_MultiplyScaleOntoImageHomography(H,1.0+dx[0]); - db_MultiplyRotationOntoImageHomography(H,dx[1]); - break; - case DB_HOMOGRAPHY_TYPE_TRANSLATION: - db_Copy9(H_p_dx,H); - H_p_dx[2]+=dx[0]; - H_p_dx[5]+=dx[1]; - break; - case DB_HOMOGRAPHY_TYPE_AFFINE: - db_UpdateImageHomographyAffine(H_p_dx,H,dx); - break; - case DB_HOMOGRAPHY_TYPE_PROJECTIVE: - db_UpdateImageHomographyProjective(H_p_dx,H,dx,frozen_coord); - break; - case DB_HOMOGRAPHY_TYPE_CAMROTATION_F: - case DB_HOMOGRAPHY_TYPE_CAMROTATION_F_UD: - db_UpdateRotFocalHomography(H_p_dx,H,dx); - break; - } -} - -void db_RobCamRotation_Polish_Generic(double H[9],int point_count,int homography_type,double *x_i,double *xp_i,double one_over_scale2, - int max_iterations,double improvement_requirement) -{ - int i,update,stop,n; - int frozen_coord = 0; - double lambda,cost,current_cost; - double JtJ[72],min_Jtf[9],dx[8],H_p_dx[9]; - double *JtJ_ref[9],d[8]; - - lambda=0.001; - for(update=1,stop=0,i=0;(stop<2) && (i<max_iterations);i++) - { - /*if first time since improvement, compute Jacobian and residual*/ - if(update) - { - db_SetupMatrixRefs(JtJ_ref,9,8,JtJ); - current_cost=db_RobImageHomography_Jacobians_Generic(JtJ_ref,min_Jtf,&n,&frozen_coord,H,point_count,x_i,xp_i,homography_type,one_over_scale2); - update=0; - } - -#ifdef _VERBOSE_ - /*std::cout << "Cost:" << current_cost << " ";*/ -#endif /*_VERBOSE_*/ - - /*Come up with a hypothesis dx - based on the current lambda*/ - db_Compute_dx(dx,JtJ_ref,min_Jtf,lambda,d,n); - - /*Compute Cost(x+dx)*/ - db_ImageHomographyUpdateGeneric(H_p_dx,H,dx,homography_type,frozen_coord); - cost=db_RobImageHomography_Cost(H_p_dx,point_count,x_i,xp_i,one_over_scale2); - - /*Is there an improvement?*/ - if(cost<current_cost) - { - /*improvement*/ - if(current_cost-cost<current_cost*improvement_requirement) stop++; - else stop=0; - lambda*=0.1; - /*Move to the hypothesised position x+dx*/ - current_cost=cost; - db_Copy9(H,H_p_dx); - update=1; - -#ifdef _VERBOSE_ - std::cout << "Step" << i << "Imp,Lambda=" << lambda << "Cost:" << current_cost << std::endl; -#endif /*_VERBOSE_*/ - } - else - { - /*no improvement*/ - lambda*=10.0; - stop=0; - } - } -} -void db_RobImageHomography( - /*Best homography*/ - double H[9], - /*2DPoint to 2DPoint constraints - Points are assumed to be given in - homogenous coordinates*/ - double *im, double *im_p, - /*Nr of points in total*/ - int nr_points, - /*Calibration matrices - used to normalize the points*/ - double K[9], - double Kp[9], - /*Pre-allocated space temp_d - should point to at least - 12*nr_samples+10*nr_points - allocated positions*/ - double *temp_d, - /*Pre-allocated space temp_i - should point to at least - max(nr_samples,nr_points) - allocated positions*/ - int *temp_i, - int homography_type, - db_Statistics *stat, - int max_iterations, - int max_points, - double scale, - int nr_samples, - int chunk_size, - ///////////////////////////////////////////// - // regular use: set outlierremoveflagE =0; - // flag for the outlier removal - int outlierremoveflagE, - // if flag is 1, then the following variables - // need the input - ////////////////////////////////////// - // 3D coordinates - double *wp, - // its corresponding stereo pair's points - double *im_r, - // raw image coordinates - double *im_raw, double *im_raw_p, - // final matches - int *finalNumE) -{ - /*Random seed*/ - int r_seed; - - int point_count_new; - /*Counters*/ - int i,j,c,point_count,hyp_count; - int last_hyp,new_last_hyp,last_corr; - int pos,point_pos,last_point; - /*Accumulator*/ - double acc; - /*Hypothesis pointer*/ - double *hyp_point; - /*Random sample*/ - int s[4]; - /*Pivot for hypothesis pruning*/ - double pivot; - /*Best hypothesis position*/ - int best_pos; - /*Best score*/ - double lowest_cost; - /*One over the squared scale of - Cauchy distribution*/ - double one_over_scale2; - /*temporary pointers*/ - double *x_i_temp,*xp_i_temp; - /*Temporary space for inverse calibration matrices*/ - double K_inv[9]; - double Kp_inv[9]; - /*Temporary space for homography*/ - double H_temp[9],H_temp2[9]; - /*Pointers to homogenous coordinates*/ - double *x_h_point,*xp_h_point; - /*Array of pointers to inhomogenous coordinates*/ - double *X[3],*Xp[3]; - /*Similarity parameters*/ - int orientation_preserving,allow_scaling,allow_rotation,allow_translation,sample_size; - - /*Homogenous coordinates of image points in first image*/ - double *x_h; - /*Homogenous coordinates of image points in second image*/ - double *xp_h; - /*Inhomogenous coordinates of image points in first image*/ - double *x_i; - /*Inhomogenous coordinates of image points in second image*/ - double *xp_i; - /*Homography hypotheses*/ - double *hyp_H_array; - /*Cost array*/ - double *hyp_cost_array; - /*Permutation of the hypotheses*/ - int *hyp_perm; - /*Sample of the points*/ - int *point_perm; - /*Temporary space for quick-select - 2*nr_samples*/ - double *temp_select; - - /*Get inverse calibration matrices*/ - db_InvertCalibrationMatrix(K_inv,K); - db_InvertCalibrationMatrix(Kp_inv,Kp); - /*Compute scale coefficient*/ - one_over_scale2=1.0/(scale*scale); - /*Initialize random seed*/ - r_seed=12345; - /*Set pointers to pre-allocated space*/ - hyp_cost_array=temp_d; - hyp_H_array=temp_d+nr_samples; - temp_select=temp_d+10*nr_samples; - x_h=temp_d+12*nr_samples; - xp_h=temp_d+12*nr_samples+3*nr_points; - x_i=temp_d+12*nr_samples+6*nr_points; - xp_i=temp_d+12*nr_samples+8*nr_points; - hyp_perm=temp_i; - point_perm=temp_i; - - /*Prepare a randomly permuted subset of size - point_count from the input points*/ - - point_count=db_mini(nr_points,(int)(chunk_size*log((double)nr_samples)/DB_LN2)); - - point_count_new = point_count; - - for(i=0;i<nr_points;i++) point_perm[i]=i; - - for(last_point=nr_points-1,i=0;i<point_count;i++,last_point--) - { - pos=db_RandomInt(r_seed,last_point); - point_pos=point_perm[pos]; - point_perm[pos]=point_perm[last_point]; - - /*Normalize image points with calibration - matrices and move them to x_h and xp_h*/ - c=3*point_pos; - j=3*i; - x_h_point=x_h+j; - xp_h_point=xp_h+j; - db_Multiply3x3_3x1(x_h_point,K_inv,im+c); - db_Multiply3x3_3x1(xp_h_point,Kp_inv,im_p+c); - - db_HomogenousNormalize3(x_h_point); - db_HomogenousNormalize3(xp_h_point); - - /*Dehomogenize image points and move them - to x_i and xp_i*/ - c=(i<<1); - db_DeHomogenizeImagePoint(x_i+c,x_h_point); // 2-dimension - db_DeHomogenizeImagePoint(xp_i+c,xp_h_point); //2-dimension - } - - - /*Generate Hypotheses*/ - hyp_count=0; - switch(homography_type) - { - case DB_HOMOGRAPHY_TYPE_SIMILARITY: - case DB_HOMOGRAPHY_TYPE_SIMILARITY_U: - case DB_HOMOGRAPHY_TYPE_TRANSLATION: - case DB_HOMOGRAPHY_TYPE_ROTATION: - case DB_HOMOGRAPHY_TYPE_ROTATION_U: - case DB_HOMOGRAPHY_TYPE_SCALING: - case DB_HOMOGRAPHY_TYPE_S_T: - case DB_HOMOGRAPHY_TYPE_R_T: - case DB_HOMOGRAPHY_TYPE_R_S: - - switch(homography_type) - { - case DB_HOMOGRAPHY_TYPE_SIMILARITY: - orientation_preserving=1; - allow_scaling=1; - allow_rotation=1; - allow_translation=1; - sample_size=2; - break; - case DB_HOMOGRAPHY_TYPE_SIMILARITY_U: - orientation_preserving=0; - allow_scaling=1; - allow_rotation=1; - allow_translation=1; - sample_size=3; - break; - case DB_HOMOGRAPHY_TYPE_TRANSLATION: - orientation_preserving=1; - allow_scaling=0; - allow_rotation=0; - allow_translation=1; - sample_size=1; - break; - case DB_HOMOGRAPHY_TYPE_ROTATION: - orientation_preserving=1; - allow_scaling=0; - allow_rotation=1; - allow_translation=0; - sample_size=1; - break; - case DB_HOMOGRAPHY_TYPE_ROTATION_U: - orientation_preserving=0; - allow_scaling=0; - allow_rotation=1; - allow_translation=0; - sample_size=2; - break; - case DB_HOMOGRAPHY_TYPE_SCALING: - orientation_preserving=1; - allow_scaling=1; - allow_rotation=0; - allow_translation=0; - sample_size=1; - break; - case DB_HOMOGRAPHY_TYPE_S_T: - orientation_preserving=1; - allow_scaling=1; - allow_rotation=0; - allow_translation=1; - sample_size=2; - break; - case DB_HOMOGRAPHY_TYPE_R_T: - orientation_preserving=1; - allow_scaling=0; - allow_rotation=1; - allow_translation=1; - sample_size=2; - break; - case DB_HOMOGRAPHY_TYPE_R_S: - orientation_preserving=1; - allow_scaling=1; - allow_rotation=0; - allow_translation=0; - sample_size=1; - break; - } - - if(point_count>=sample_size) for(i=0;i<nr_samples;i++) - { - db_RandomSample(s,3,point_count,r_seed); - X[0]= &x_i[s[0]<<1]; - X[1]= &x_i[s[1]<<1]; - X[2]= &x_i[s[2]<<1]; - Xp[0]= &xp_i[s[0]<<1]; - Xp[1]= &xp_i[s[1]<<1]; - Xp[2]= &xp_i[s[2]<<1]; - db_StitchSimilarity2D(&hyp_H_array[9*hyp_count],Xp,X,sample_size,orientation_preserving, - allow_scaling,allow_rotation,allow_translation); - hyp_count++; - } - break; - - case DB_HOMOGRAPHY_TYPE_CAMROTATION: - if(point_count>=2) for(i=0;i<nr_samples;i++) - { - db_RandomSample(s,2,point_count,r_seed); - db_StitchCameraRotation_2Points(&hyp_H_array[9*hyp_count], - &x_h[3*s[0]],&x_h[3*s[1]], - &xp_h[3*s[0]],&xp_h[3*s[1]]); - hyp_count++; - } - break; - - case DB_HOMOGRAPHY_TYPE_CAMROTATION_F: - if(point_count>=3) for(i=0;i<nr_samples;i++) - { - db_RandomSample(s,3,point_count,r_seed); - hyp_count+=db_StitchRotationCommonFocalLength_3Points(&hyp_H_array[9*hyp_count], - &x_h[3*s[0]],&x_h[3*s[1]],&x_h[3*s[2]], - &xp_h[3*s[0]],&xp_h[3*s[1]],&xp_h[3*s[2]]); - } - break; - - case DB_HOMOGRAPHY_TYPE_CAMROTATION_F_UD: - if(point_count>=3) for(i=0;i<nr_samples;i++) - { - db_RandomSample(s,3,point_count,r_seed); - hyp_count+=db_StitchRotationCommonFocalLength_3Points(&hyp_H_array[9*hyp_count], - &x_h[3*s[0]],&x_h[3*s[1]],&x_h[3*s[2]], - &xp_h[3*s[0]],&xp_h[3*s[1]],&xp_h[3*s[2]],NULL,0); - } - break; - - case DB_HOMOGRAPHY_TYPE_AFFINE: - if(point_count>=3) for(i=0;i<nr_samples;i++) - { - db_RandomSample(s,3,point_count,r_seed); - db_StitchAffine2D_3Points(&hyp_H_array[9*hyp_count], - &x_h[3*s[0]],&x_h[3*s[1]],&x_h[3*s[2]], - &xp_h[3*s[0]],&xp_h[3*s[1]],&xp_h[3*s[2]]); - hyp_count++; - } - break; - - case DB_HOMOGRAPHY_TYPE_PROJECTIVE: - default: - if(point_count>=4) for(i=0;i<nr_samples;i++) - { - db_RandomSample(s,4,point_count,r_seed); - db_StitchProjective2D_4Points(&hyp_H_array[9*hyp_count], - &x_h[3*s[0]],&x_h[3*s[1]],&x_h[3*s[2]],&x_h[3*s[3]], - &xp_h[3*s[0]],&xp_h[3*s[1]],&xp_h[3*s[2]],&xp_h[3*s[3]]); - hyp_count++; - } - } - - if(hyp_count) - { - /*Count cost in chunks and decimate hypotheses - until only one remains or the correspondences are - exhausted*/ - for(i=0;i<hyp_count;i++) - { - hyp_perm[i]=i; - hyp_cost_array[i]=0.0; - } - for(i=0,last_hyp=hyp_count-1;(last_hyp>0) && (i<point_count);i+=chunk_size) - { - /*Update cost with the next chunk*/ - last_corr=db_mini(i+chunk_size-1,point_count-1); - for(j=0;j<=last_hyp;j++) - { - hyp_point=hyp_H_array+9*hyp_perm[j]; - for(c=i;c<=last_corr;) - { - /*Take log of product of ten reprojection - errors to reduce nr of expensive log operations*/ - if(c+9<=last_corr) - { - x_i_temp=x_i+(c<<1); - xp_i_temp=xp_i+(c<<1); - - acc=db_ExpCauchyInhomogenousHomographyError(xp_i_temp,hyp_point,x_i_temp,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+2,hyp_point,x_i_temp+2,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+4,hyp_point,x_i_temp+4,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+6,hyp_point,x_i_temp+6,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+8,hyp_point,x_i_temp+8,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+10,hyp_point,x_i_temp+10,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+12,hyp_point,x_i_temp+12,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+14,hyp_point,x_i_temp+14,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+16,hyp_point,x_i_temp+16,one_over_scale2); - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i_temp+18,hyp_point,x_i_temp+18,one_over_scale2); - c+=10; - } - else - { - for(acc=1.0;c<=last_corr;c++) - { - acc*=db_ExpCauchyInhomogenousHomographyError(xp_i+(c<<1),hyp_point,x_i+(c<<1),one_over_scale2); - } - } - hyp_cost_array[j]+=log(acc); - } - } - if (chunk_size<point_count){ - /*Prune out half of the hypotheses*/ - new_last_hyp=(last_hyp+1)/2-1; - pivot=db_LeanQuickSelect(hyp_cost_array,last_hyp+1,new_last_hyp,temp_select); - for(j=0,c=0;(j<=last_hyp) && (c<=new_last_hyp);j++) - { - if(hyp_cost_array[j]<=pivot) - { - hyp_cost_array[c]=hyp_cost_array[j]; - hyp_perm[c]=hyp_perm[j]; - c++; - } - } - last_hyp=new_last_hyp; - } - } - /*Find the best hypothesis*/ - lowest_cost=hyp_cost_array[0]; - best_pos=0; - for(j=1;j<=last_hyp;j++) - { - if(hyp_cost_array[j]<lowest_cost) - { - lowest_cost=hyp_cost_array[j]; - best_pos=j; - } - } - - /*Move the best hypothesis*/ - db_Copy9(H_temp,hyp_H_array+9*hyp_perm[best_pos]); - - // outlier removal - if (outlierremoveflagE) // no polishment needed - { - point_count_new = db_RemoveOutliers_Homography(H_temp,x_i,xp_i,wp,im,im_p,im_r,im_raw,im_raw_p,point_count,one_over_scale2); - } - else - { - /*Polish*/ - switch(homography_type) - { - case DB_HOMOGRAPHY_TYPE_SIMILARITY: - case DB_HOMOGRAPHY_TYPE_SIMILARITY_U: - case DB_HOMOGRAPHY_TYPE_TRANSLATION: - case DB_HOMOGRAPHY_TYPE_ROTATION: - case DB_HOMOGRAPHY_TYPE_ROTATION_U: - case DB_HOMOGRAPHY_TYPE_SCALING: - case DB_HOMOGRAPHY_TYPE_S_T: - case DB_HOMOGRAPHY_TYPE_R_T: - case DB_HOMOGRAPHY_TYPE_R_S: - case DB_HOMOGRAPHY_TYPE_AFFINE: - case DB_HOMOGRAPHY_TYPE_PROJECTIVE: - case DB_HOMOGRAPHY_TYPE_CAMROTATION_F: - case DB_HOMOGRAPHY_TYPE_CAMROTATION_F_UD: - db_RobCamRotation_Polish_Generic(H_temp,db_mini(point_count,max_points),homography_type,x_i,xp_i,one_over_scale2,max_iterations); - break; - case DB_HOMOGRAPHY_TYPE_CAMROTATION: - db_RobCamRotation_Polish(H_temp,db_mini(point_count,max_points),x_i,xp_i,one_over_scale2,max_iterations); - break; - } - - } - - } - else db_Identity3x3(H_temp); - - switch(homography_type) - { - case DB_HOMOGRAPHY_TYPE_PROJECTIVE: - if(stat) stat->nr_parameters=8; - break; - case DB_HOMOGRAPHY_TYPE_AFFINE: - if(stat) stat->nr_parameters=6; - break; - case DB_HOMOGRAPHY_TYPE_SIMILARITY: - case DB_HOMOGRAPHY_TYPE_SIMILARITY_U: - case DB_HOMOGRAPHY_TYPE_CAMROTATION_F: - case DB_HOMOGRAPHY_TYPE_CAMROTATION_F_UD: - if(stat) stat->nr_parameters=4; - break; - case DB_HOMOGRAPHY_TYPE_CAMROTATION: - if(stat) stat->nr_parameters=3; - break; - case DB_HOMOGRAPHY_TYPE_TRANSLATION: - case DB_HOMOGRAPHY_TYPE_S_T: - case DB_HOMOGRAPHY_TYPE_R_T: - case DB_HOMOGRAPHY_TYPE_R_S: - if(stat) stat->nr_parameters=2; - break; - case DB_HOMOGRAPHY_TYPE_ROTATION: - case DB_HOMOGRAPHY_TYPE_ROTATION_U: - case DB_HOMOGRAPHY_TYPE_SCALING: - if(stat) stat->nr_parameters=1; - break; - } - - db_RobImageHomography_Statistics(H_temp,db_mini(point_count,max_points),x_i,xp_i,one_over_scale2,stat); - - /*Put on the calibration matrices*/ - db_Multiply3x3_3x3(H_temp2,H_temp,K_inv); - db_Multiply3x3_3x3(H,Kp,H_temp2); - - if (finalNumE) - *finalNumE = point_count_new; - -} diff --git a/jni/feature_stab/db_vlvm/db_rob_image_homography.h b/jni/feature_stab/db_vlvm/db_rob_image_homography.h deleted file mode 100644 index 59cde7daa..000000000 --- a/jni/feature_stab/db_vlvm/db_rob_image_homography.h +++ /dev/null @@ -1,148 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_rob_image_homography.h,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */ - -#ifndef DB_ROB_IMAGE_HOMOGRAPHY -#define DB_ROB_IMAGE_HOMOGRAPHY - -#include "db_utilities.h" -#include "db_robust.h" -#include "db_metrics.h" - -#include <stdlib.h> // for NULL - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ -/*! - * \defgroup LMRobImageHomography (LM) Robust Image Homography - */ -/*\{*/ - -#define DB_HOMOGRAPHY_TYPE_DEFAULT 0 -#define DB_HOMOGRAPHY_TYPE_PROJECTIVE 0 -#define DB_HOMOGRAPHY_TYPE_AFFINE 1 -#define DB_HOMOGRAPHY_TYPE_SIMILARITY 2 -#define DB_HOMOGRAPHY_TYPE_SIMILARITY_U 3 -#define DB_HOMOGRAPHY_TYPE_TRANSLATION 4 -#define DB_HOMOGRAPHY_TYPE_ROTATION 5 -#define DB_HOMOGRAPHY_TYPE_ROTATION_U 6 -#define DB_HOMOGRAPHY_TYPE_SCALING 7 -#define DB_HOMOGRAPHY_TYPE_S_T 8 -#define DB_HOMOGRAPHY_TYPE_R_T 9 -#define DB_HOMOGRAPHY_TYPE_R_S 10 -#define DB_HOMOGRAPHY_TYPE_CAMROTATION 11 -#define DB_HOMOGRAPHY_TYPE_CAMROTATION_F 12 -#define DB_HOMOGRAPHY_TYPE_CAMROTATION_F_UD 13 - -/*! -Solve for homography H such that xp~Hx -\param H best homography - -2D point to 2D point constraints: - -\param im first image points -\param im_p second image points -\param nr_points number of points - -Calibration matrices: - -\param K first camera -\param Kp second camera - - Temporary space: - - \param temp_d pre-allocated space of size 12*nr_samples+10*nr_points doubles - \param temp_i pre-allocated space of size max(nr_samples,nr_points) ints - - Statistics for this estimation - - \param stat NULL - do not compute - - \param homography_type see DB_HOMOGRAPHY_TYPE_* definitions above - - Estimation parameters: - - \param max_iterations max number of polishing steps - \param max_points only use this many points - \param scale Cauchy scale coefficient (see db_ExpCauchyReprojectionError() ) - \param nr_samples number of times to compute a hypothesis - \param chunk_size size of cost chunks -*/ -DB_API void db_RobImageHomography( - /*Best homography*/ - double H[9], - /*2DPoint to 2DPoint constraints - Points are assumed to be given in - homogenous coordinates*/ - double *im,double *im_p, - /*Nr of points in total*/ - int nr_points, - /*Calibration matrices - used to normalize the points*/ - double K[9], - double Kp[9], - /*Pre-allocated space temp_d - should point to at least - 12*nr_samples+10*nr_points - allocated positions*/ - double *temp_d, - /*Pre-allocated space temp_i - should point to at least - max(nr_samples,nr_points) - allocated positions*/ - int *temp_i, - int homography_type=DB_HOMOGRAPHY_TYPE_DEFAULT, - db_Statistics *stat=NULL, - int max_iterations=DB_DEFAULT_MAX_ITERATIONS, - int max_points=DB_DEFAULT_MAX_POINTS, - double scale=DB_POINT_STANDARDDEV, - int nr_samples=DB_DEFAULT_NR_SAMPLES, - int chunk_size=DB_DEFAULT_CHUNK_SIZE, - /////////////////////////////////////////////////// - // flag for the outlier removal - int outlierremoveflagE = 0, - // if flag is 1, then the following variables - // need to input - /////////////////////////////////////////////////// - // 3D coordinates - double *wp=NULL, - // its corresponding stereo pair's points - double *im_r=NULL, - // raw image coordinates - double *im_raw=NULL, double *im_raw_p=NULL, - // final matches - int *final_NumE=0); - -DB_API double db_RobImageHomography_Cost(double H[9],int point_count,double *x_i, - double *xp_i,double one_over_scale2); - - -DB_API void db_RobCamRotation_Polish(double H[9],int point_count,double *x_i, - double *xp_i, double one_over_scale2, - int max_iterations=DB_DEFAULT_MAX_ITERATIONS, - double improvement_requirement=DB_DEFAULT_IMP_REQ); - - -DB_API void db_RobCamRotation_Polish_Generic(double H[9],int point_count,int homography_type, - double *x_i,double *xp_i,double one_over_scale2, - int max_iterations=DB_DEFAULT_MAX_ITERATIONS, - double improvement_requirement=DB_DEFAULT_IMP_REQ); - - -#endif /* DB_ROB_IMAGE_HOMOGRAPHY */ diff --git a/jni/feature_stab/db_vlvm/db_robust.h b/jni/feature_stab/db_vlvm/db_robust.h deleted file mode 100644 index be0794c6e..000000000 --- a/jni/feature_stab/db_vlvm/db_robust.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_robust.h,v 1.4 2011/06/17 14:03:31 mbansal Exp $ */ - -#ifndef DB_ROBUST -#define DB_ROBUST - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ -/*! - * \defgroup LMRobust (LM) Robust Estimation - */ - -/*! - \struct db_Statistics - \ingroup LMRobust - \brief (LnM) Sampling problem statistics - \date Mon Sep 10 10:28:08 EDT 2007 - \par Copyright: 2007 Sarnoff Corporation. All Rights Reserved - */ - struct db_stat_struct - { - int nr_points; - int nr_inliers; - double inlier_fraction; - double cost; - double one_over_scale2; - double lambda1; - double lambda2; - double lambda3; - int nr_parameters; - int model_dimension; - double gric; - double inlier_evidence; - double posestd[6]; - double rotationvecCov[9]; - double translationvecCov[9]; - int posecov_inliercount; - int posecovready; - double median_reprojection_error; - }; - typedef db_stat_struct db_Statistics; - -#endif /* DB_ROBUST */ diff --git a/jni/feature_stab/db_vlvm/db_utilities.cpp b/jni/feature_stab/db_vlvm/db_utilities.cpp deleted file mode 100644 index ce2093b01..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities.cpp +++ /dev/null @@ -1,176 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities.cpp,v 1.4 2011/06/17 14:03:31 mbansal Exp $ */ - -#include "db_utilities.h" -#include <string.h> -#include <stdio.h> - -float** db_SetupImageReferences_f(float *im,int w,int h) -{ - int i; - float **img; - assert(im); - img=new float* [h]; - for(i=0;i<h;i++) - { - img[i]=im+w*i; - } - return(img); -} - -unsigned char** db_SetupImageReferences_u(unsigned char *im,int w,int h) -{ - int i; - unsigned char **img; - - assert(im); - - img=new unsigned char* [h]; - for(i=0;i<h;i++) - { - img[i]=im+w*i; - } - return(img); -} -float** db_AllocImage_f(int w,int h,int over_allocation) -{ - float **img,*im; - - im=new float [w*h+over_allocation]; - img=db_SetupImageReferences_f(im,w,h); - - return(img); -} - -unsigned char** db_AllocImage_u(int w,int h,int over_allocation) -{ - unsigned char **img,*im; - - im=new unsigned char [w*h+over_allocation]; - img=db_SetupImageReferences_u(im,w,h); - - return(img); -} - -void db_FreeImage_f(float **img,int h) -{ - delete [] (img[0]); - delete [] img; -} - -void db_FreeImage_u(unsigned char **img,int h) -{ - delete [] (img[0]); - delete [] img; -} - -// ----------------------------------------------------------------------------------------------------------- ; -// -// copy image (source to destination) -// ---> must be a 2D image array with the same image size -// ---> the size of the input and output images must be same -// -// ------------------------------------------------------------------------------------------------------------ ; -void db_CopyImage_u(unsigned char **d,const unsigned char * const *s, int w, int h, int over_allocation) -{ - int i; - - for (i=0;i<h;i++) - { - memcpy(d[i],s[i],w*sizeof(unsigned char)); - } - - memcpy(&d[h],&d[h],over_allocation); - -} - -inline void db_WarpImageLutFast_u(const unsigned char * const * src, unsigned char ** dst, int w, int h, - const float * const * lut_x, const float * const * lut_y) -{ - assert(src && dst); - int xd=0, yd=0; - - for ( int i = 0; i < w; ++i ) - for ( int j = 0; j < h; ++j ) - { - //xd = static_cast<unsigned int>(lut_x[j][i]); - //yd = static_cast<unsigned int>(lut_y[j][i]); - xd = (unsigned int)(lut_x[j][i]); - yd = (unsigned int)(lut_y[j][i]); - if ( xd >= w || yd >= h || - xd < 0 || yd < 0) - dst[j][i] = 0; - else - dst[j][i] = src[yd][xd]; - } -} - -inline void db_WarpImageLutBilinear_u(const unsigned char * const * src, unsigned char ** dst, int w, int h, - const float * const * lut_x,const float * const* lut_y) -{ - assert(src && dst); - double xd=0.0, yd=0.0; - - for ( int i = 0; i < w; ++i ) - for ( int j = 0; j < h; ++j ) - { - xd = static_cast<double>(lut_x[j][i]); - yd = static_cast<double>(lut_y[j][i]); - if ( xd > w || yd > h || - xd < 0.0 || yd < 0.0) - dst[j][i] = 0; - else - dst[j][i] = db_BilinearInterpolation(yd, xd, src); - } -} - - -void db_WarpImageLut_u(const unsigned char * const * src, unsigned char ** dst, int w, int h, - const float * const * lut_x,const float * const * lut_y, int type) -{ - switch (type) - { - case DB_WARP_FAST: - db_WarpImageLutFast_u(src,dst,w,h,lut_x,lut_y); - break; - case DB_WARP_BILINEAR: - db_WarpImageLutBilinear_u(src,dst,w,h,lut_x,lut_y); - break; - default: - break; - } -} - - -void db_PrintDoubleVector(double *a,long size) -{ - printf("[ "); - for(long i=0;i<size;i++) printf("%lf ",a[i]); - printf("]"); -} - -void db_PrintDoubleMatrix(double *a,long rows,long cols) -{ - printf("[\n"); - for(long i=0;i<rows;i++) - { - for(long j=0;j<cols;j++) printf("%lf ",a[i*cols+j]); - printf("\n"); - } - printf("]"); -} diff --git a/jni/feature_stab/db_vlvm/db_utilities.h b/jni/feature_stab/db_vlvm/db_utilities.h deleted file mode 100644 index fa9c87745..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities.h +++ /dev/null @@ -1,571 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities.h,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */ - -#ifndef DB_UTILITIES_H -#define DB_UTILITIES_H - - -#ifdef _WIN32 -#pragma warning(disable: 4275) -#pragma warning(disable: 4251) -#pragma warning(disable: 4786) -#pragma warning(disable: 4800) -#pragma warning(disable: 4018) /* signed-unsigned mismatch */ -#endif /* _WIN32 */ - -#ifdef _WIN32 - #ifdef DBDYNAMIC_EXPORTS - #define DB_API __declspec(dllexport) - #else - #ifdef DBDYNAMIC_IMPORTS - #define DB_API __declspec(dllimport) - #else - #define DB_API - #endif - #endif -#else - #define DB_API -#endif /* _WIN32 */ - -#ifdef _VERBOSE_ -#include <iostream> -#endif - -#include <math.h> - -#include <assert.h> -#include "db_utilities_constants.h" -/*! - * \defgroup LMBasicUtilities (LM) Utility Functions (basic math, linear algebra and array manipulations) - */ -/*\{*/ - -/*! - * Round double into int using fld and fistp instructions. - */ -inline int db_roundi (double x) { -#ifdef WIN32_ASM - int n; - __asm - { - fld x; - fistp n; - } - return n; -#else - return static_cast<int>(floor(x+0.5)); -#endif -} - -/*! - * Square a double. - */ -inline double db_sqr(double a) -{ - return(a*a); -} - -/*! - * Square a long. - */ -inline long db_sqr(long a) -{ - return(a*a); -} - -/*! - * Square an int. - */ -inline long db_sqr(int a) -{ - return(a*a); -} - -/*! - * Maximum of two doubles. - */ -inline double db_maxd(double a,double b) -{ - if(b>a) return(b); - else return(a); -} -/*! - * Minumum of two doubles. - */ -inline double db_mind(double a,double b) -{ - if(b<a) return(b); - else return(a); -} - - -/*! - * Maximum of two ints. - */ -inline int db_maxi(int a,int b) -{ - if(b>a) return(b); - else return(a); -} - -/*! - * Minimum of two numbers. - */ -inline int db_mini(int a,int b) -{ - if(b<a) return(b); - else return(a); -} -/*! - * Maximum of two numbers. - */ -inline long db_maxl(long a,long b) -{ - if(b>a) return(b); - else return(a); -} - -/*! - * Minimum of two numbers. - */ -inline long db_minl(long a,long b) -{ - if(b<a) return(b); - else return(a); -} - -/*! - * Sign of a number. - * \return -1.0 if negative, 1.0 if positive. - */ -inline double db_sign(double x) -{ - if(x>=0.0) return(1.0); - else return(-1.0); -} -/*! - * Absolute value. - */ -inline int db_absi(int a) -{ - if(a<0) return(-a); - else return(a); -} -/*! - * Absolute value. - */ -inline float db_absf(float a) -{ - if(a<0) return(-a); - else return(a); -} - -/*! - * Absolute value. - */ -inline double db_absd(double a) -{ - if(a<0) return(-a); - else return(a); -} - -/*! - * Reciprocal (1/a). Prevents divide by 0. - * \return 1/a if a != 0. 1.0 otherwise. - */ -inline double db_SafeReciprocal(double a) -{ - return((a!=0.0)?(1.0/a):1.0); -} - -/*! - * Division. Prevents divide by 0. - * \return a/b if b!=0. a otherwise. - */ -inline double db_SafeDivision(double a,double b) -{ - return((b!=0.0)?(a/b):a); -} - -/*! - * Square root. Prevents imaginary output. - * \return sqrt(a) if a > 0.0. 0.0 otherewise. - */ -inline double db_SafeSqrt(double a) -{ - return((a>=0.0)?(sqrt(a)):0.0); -} - -/*! - * Square root of a reciprocal. Prevents divide by 0 and imaginary output. - * \return sqrt(1/a) if a > 0.0. 1.0 otherewise. - */ -inline double db_SafeSqrtReciprocal(double a) -{ - return((a>0.0)?(sqrt(1.0/a)):1.0); -} -/*! - * Cube root. - */ -inline double db_CubRoot(double x) -{ - if(x>=0.0) return(pow(x,1.0/3.0)); - else return(-pow(-x,1.0/3.0)); -} -/*! - * Sum of squares of elements of x. - */ -inline double db_SquareSum3(const double x[3]) -{ - return(db_sqr(x[0])+db_sqr(x[1])+db_sqr(x[2])); -} -/*! - * Sum of squares of elements of x. - */ -inline double db_SquareSum7(double x[7]) -{ - return(db_sqr(x[0])+db_sqr(x[1])+db_sqr(x[2])+ - db_sqr(x[3])+db_sqr(x[4])+db_sqr(x[5])+ - db_sqr(x[6])); -} -/*! - * Sum of squares of elements of x. - */ -inline double db_SquareSum9(double x[9]) -{ - return(db_sqr(x[0])+db_sqr(x[1])+db_sqr(x[2])+ - db_sqr(x[3])+db_sqr(x[4])+db_sqr(x[5])+ - db_sqr(x[6])+db_sqr(x[7])+db_sqr(x[8])); -} -/*! - * Copy a vector. - * \param xd destination - * \param xs source - */ -void inline db_Copy3(double xd[3],const double xs[3]) -{ - xd[0]=xs[0];xd[1]=xs[1];xd[2]=xs[2]; -} -/*! - * Copy a vector. - * \param xd destination - * \param xs source - */ -void inline db_Copy6(double xd[6],const double xs[6]) -{ - xd[0]=xs[0];xd[1]=xs[1];xd[2]=xs[2]; - xd[3]=xs[3];xd[4]=xs[4];xd[5]=xs[5]; -} -/*! - * Copy a vector. - * \param xd destination - * \param xs source - */ -void inline db_Copy9(double xd[9],const double xs[9]) -{ - xd[0]=xs[0];xd[1]=xs[1];xd[2]=xs[2]; - xd[3]=xs[3];xd[4]=xs[4];xd[5]=xs[5]; - xd[6]=xs[6];xd[7]=xs[7];xd[8]=xs[8]; -} - -/*! - * Scalar product: Transpose(A)*B. - */ -inline double db_ScalarProduct4(const double A[4],const double B[4]) -{ - return(A[0]*B[0]+A[1]*B[1]+A[2]*B[2]+A[3]*B[3]); -} -/*! - * Scalar product: Transpose(A)*B. - */ -inline double db_ScalarProduct7(const double A[7],const double B[7]) -{ - return(A[0]*B[0]+A[1]*B[1]+A[2]*B[2]+ - A[3]*B[3]+A[4]*B[4]+A[5]*B[5]+ - A[6]*B[6]); -} -/*! - * Scalar product: Transpose(A)*B. - */ -inline double db_ScalarProduct9(const double A[9],const double B[9]) -{ - return(A[0]*B[0]+A[1]*B[1]+A[2]*B[2]+ - A[3]*B[3]+A[4]*B[4]+A[5]*B[5]+ - A[6]*B[6]+A[7]*B[7]+A[8]*B[8]); -} -/*! - * Vector addition: S=A+B. - */ -inline void db_AddVectors6(double S[6],const double A[6],const double B[6]) -{ - S[0]=A[0]+B[0]; S[1]=A[1]+B[1]; S[2]=A[2]+B[2]; S[3]=A[3]+B[3]; S[4]=A[4]+B[4]; - S[5]=A[5]+B[5]; -} -/*! - * Multiplication: C(3x1)=A(3x3)*B(3x1). - */ -inline void db_Multiply3x3_3x1(double y[3],const double A[9],const double x[3]) -{ - y[0]=A[0]*x[0]+A[1]*x[1]+A[2]*x[2]; - y[1]=A[3]*x[0]+A[4]*x[1]+A[5]*x[2]; - y[2]=A[6]*x[0]+A[7]*x[1]+A[8]*x[2]; -} -inline void db_Multiply3x3_3x3(double C[9], const double A[9],const double B[9]) -{ - C[0]=A[0]*B[0]+A[1]*B[3]+A[2]*B[6]; - C[1]=A[0]*B[1]+A[1]*B[4]+A[2]*B[7]; - C[2]=A[0]*B[2]+A[1]*B[5]+A[2]*B[8]; - - C[3]=A[3]*B[0]+A[4]*B[3]+A[5]*B[6]; - C[4]=A[3]*B[1]+A[4]*B[4]+A[5]*B[7]; - C[5]=A[3]*B[2]+A[4]*B[5]+A[5]*B[8]; - - C[6]=A[6]*B[0]+A[7]*B[3]+A[8]*B[6]; - C[7]=A[6]*B[1]+A[7]*B[4]+A[8]*B[7]; - C[8]=A[6]*B[2]+A[7]*B[5]+A[8]*B[8]; -} -/*! - * Multiplication: C(4x1)=A(4x4)*B(4x1). - */ -inline void db_Multiply4x4_4x1(double y[4],const double A[16],const double x[4]) -{ - y[0]=A[0]*x[0]+A[1]*x[1]+A[2]*x[2]+A[3]*x[3]; - y[1]=A[4]*x[0]+A[5]*x[1]+A[6]*x[2]+A[7]*x[3]; - y[2]=A[8]*x[0]+A[9]*x[1]+A[10]*x[2]+A[11]*x[3]; - y[3]=A[12]*x[0]+A[13]*x[1]+A[14]*x[2]+A[15]*x[3]; -} -/*! - * Scalar multiplication in place: A(3)=mult*A(3). - */ -inline void db_MultiplyScalar3(double *A,double mult) -{ - (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; -} - -/*! - * Scalar multiplication: A(3)=mult*B(3). - */ -inline void db_MultiplyScalarCopy3(double *A,const double *B,double mult) -{ - (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; -} - -/*! - * Scalar multiplication: A(4)=mult*B(4). - */ -inline void db_MultiplyScalarCopy4(double *A,const double *B,double mult) -{ - (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; -} -/*! - * Scalar multiplication: A(7)=mult*B(7). - */ -inline void db_MultiplyScalarCopy7(double *A,const double *B,double mult) -{ - (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; - (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; -} -/*! - * Scalar multiplication: A(9)=mult*B(9). - */ -inline void db_MultiplyScalarCopy9(double *A,const double *B,double mult) -{ - (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; - (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; (*A++)=(*B++)*mult; -} - -/*! - * \defgroup LMImageBasicUtilities (LM) Basic Image Utility Functions - - Images in db are simply 2D arrays of unsigned char or float types. - Only the very basic operations are supported: allocation/deallocation, -copying, simple pyramid construction and LUT warping. These images are used -by db_CornerDetector_u and db_Matcher_u. The db_Image class is an attempt -to wrap these images. It has not been tested well. - - */ -/*\{*/ -/*! - * Given a float image array, allocates and returns the set of row poiners. - * \param im image pointer - * \param w image width - * \param h image height - */ -DB_API float** db_SetupImageReferences_f(float *im,int w,int h); -/*! - * Allocate a float image. - * Note: for feature detection images must be overallocated by 256 bytes. - * \param w width - * \param h height - * \param over_allocation allocate this many extra bytes at the end - * \return row array pointer - */ -DB_API float** db_AllocImage_f(int w,int h,int over_allocation=256); -/*! - * Free a float image - * \param img row array pointer - * \param h image height (number of rows) - */ -DB_API void db_FreeImage_f(float **img,int h); -/*! - * Given an unsigned char image array, allocates and returns the set of row poiners. - * \param im image pointer - * \param w image width - * \param h image height - */ -DB_API unsigned char** db_SetupImageReferences_u(unsigned char *im,int w,int h); -/*! - * Allocate an unsigned char image. - * Note: for feature detection images must be overallocated by 256 bytes. - * \param w width - * \param h height - * \param over_allocation allocate this many extra bytes at the end - * \return row array pointer - */ -DB_API unsigned char** db_AllocImage_u(int w,int h,int over_allocation=256); -/*! - * Free an unsigned char image - * \param img row array pointer - * \param h image height (number of rows) - */ -DB_API void db_FreeImage_u(unsigned char **img,int h); - -/*! - Copy an image from s to d. Both s and d must be pre-allocated at of the same size. - Copy is done row by row. - \param s source - \param d destination - \param w width - \param h height - \param over_allocation copy this many bytes after the end of the last line - */ -DB_API void db_CopyImage_u(unsigned char **d,const unsigned char * const *s,int w,int h,int over_allocation=0); - -DB_API inline unsigned char db_BilinearInterpolation(double y, double x, const unsigned char * const * v) -{ - int floor_x=(int) x; - int floor_y=(int) y; - - int ceil_x=floor_x+1; - int ceil_y=floor_y+1; - - unsigned char f00 = v[floor_y][floor_x]; - unsigned char f01 = v[floor_y][ceil_x]; - unsigned char f10 = v[ceil_y][floor_x]; - unsigned char f11 = v[ceil_y][ceil_x]; - - double xl = x-floor_x; - double yl = y-floor_y; - - return (unsigned char)(f00*(1-yl)*(1-xl) + f10*yl*(1-xl) + f01*(1-yl)*xl + f11*yl*xl); -} -/*\}*/ -/*! - * \ingroup LMRotation - * Compute an incremental rotation matrix using the update dx=[sin(phi) sin(ohm) sin(kap)] - */ -inline void db_IncrementalRotationMatrix(double R[9],const double dx[3]) -{ - double sp,so,sk,om_sp2,om_so2,om_sk2,cp,co,ck,sp_so,cp_so; - - /*Store sines*/ - sp=dx[0]; so=dx[1]; sk=dx[2]; - om_sp2=1.0-sp*sp; - om_so2=1.0-so*so; - om_sk2=1.0-sk*sk; - /*Compute cosines*/ - cp=(om_sp2>=0.0)?sqrt(om_sp2):1.0; - co=(om_so2>=0.0)?sqrt(om_so2):1.0; - ck=(om_sk2>=0.0)?sqrt(om_sk2):1.0; - /*Compute matrix*/ - sp_so=sp*so; - cp_so=cp*so; - R[0]=sp_so*sk+cp*ck; R[1]=co*sk; R[2]=cp_so*sk-sp*ck; - R[3]=sp_so*ck-cp*sk; R[4]=co*ck; R[5]=cp_so*ck+sp*sk; - R[6]=sp*co; R[7]= -so; R[8]=cp*co; -} -/*! - * Zero out 2 vector in place. - */ -void inline db_Zero2(double x[2]) -{ - x[0]=x[1]=0; -} -/*! - * Zero out 3 vector in place. - */ -void inline db_Zero3(double x[3]) -{ - x[0]=x[1]=x[2]=0; -} -/*! - * Zero out 4 vector in place. - */ -void inline db_Zero4(double x[4]) -{ - x[0]=x[1]=x[2]=x[3]=0; -} -/*! - * Zero out 9 vector in place. - */ -void inline db_Zero9(double x[9]) -{ - x[0]=x[1]=x[2]=x[3]=x[4]=x[5]=x[6]=x[7]=x[8]=0; -} - -#define DB_WARP_FAST 0 -#define DB_WARP_BILINEAR 1 - -/*! - * Perform a look-up table warp. - * The LUTs must be float images of the same size as source image. - * The source value x_s is determined from destination (x_d,y_d) through lut_x - * and y_s is determined from lut_y: - \code - x_s = lut_x[y_d][x_d]; - y_s = lut_y[y_d][x_d]; - \endcode - - * \param src source image - * \param dst destination image - * \param w width - * \param h height - * \param lut_x LUT for x - * \param lut_y LUT for y - * \param type warp type (DB_WARP_FAST or DB_WARP_BILINEAR) - */ -DB_API void db_WarpImageLut_u(const unsigned char * const * src,unsigned char ** dst, int w, int h, - const float * const * lut_x, const float * const * lut_y, int type=DB_WARP_BILINEAR); - -DB_API void db_PrintDoubleVector(double *a,long size); -DB_API void db_PrintDoubleMatrix(double *a,long rows,long cols); - -#include "db_utilities_constants.h" -#include "db_utilities_algebra.h" -#include "db_utilities_indexing.h" -#include "db_utilities_linalg.h" -#include "db_utilities_poly.h" -#include "db_utilities_geometry.h" -#include "db_utilities_random.h" -#include "db_utilities_rotation.h" -#include "db_utilities_camera.h" - -#define DB_INVALID (-1) - - -#endif /* DB_UTILITIES_H */ diff --git a/jni/feature_stab/db_vlvm/db_utilities_algebra.h b/jni/feature_stab/db_vlvm/db_utilities_algebra.h deleted file mode 100644 index 2aedd74d5..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities_algebra.h +++ /dev/null @@ -1,41 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities_algebra.h,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */ - -#ifndef DB_UTILITIES_ALGEBRA -#define DB_UTILITIES_ALGEBRA - -#include "db_utilities.h" - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ -/*! - * \defgroup LMAlgebra (LM) Algebra utilities - */ -/*\{*/ - -inline void db_HomogenousNormalize3(double *x) -{ - db_MultiplyScalar3(x,db_SafeSqrtReciprocal(db_SquareSum3(x))); -} - -/*\}*/ - -#endif /* DB_UTILITIES_ALGEBRA */ diff --git a/jni/feature_stab/db_vlvm/db_utilities_camera.cpp b/jni/feature_stab/db_vlvm/db_utilities_camera.cpp deleted file mode 100644 index dceba9b62..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities_camera.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities_camera.cpp,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */ - -#include "db_utilities_camera.h" -#include "db_utilities.h" -#include <assert.h> - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ - -void db_Approx3DCalMat(double K[9],double Kinv[9],int im_width,int im_height,double f_correction,int field) -{ - double iw,ih,av_size,field_fact; - - if(field) field_fact=2.0; - else field_fact=1.0; - - iw=(double)im_width; - ih=(double)(im_height*field_fact); - av_size=(iw+ih)/2.0; - K[0]=f_correction*av_size; - K[1]=0; - K[2]=iw/2.0; - K[3]=0; - K[4]=f_correction*av_size/field_fact; - K[5]=ih/2.0/field_fact; - K[6]=0; - K[7]=0; - K[8]=1; - - db_InvertCalibrationMatrix(Kinv,K); -} diff --git a/jni/feature_stab/db_vlvm/db_utilities_camera.h b/jni/feature_stab/db_vlvm/db_utilities_camera.h deleted file mode 100644 index 26ba4420a..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities_camera.h +++ /dev/null @@ -1,332 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities_camera.h,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */ - -#ifndef DB_UTILITIES_CAMERA -#define DB_UTILITIES_CAMERA - -#include "db_utilities.h" - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ -/*! - * \defgroup LMCamera (LM) Camera Utilities - */ -/*\{*/ - -#include "db_utilities.h" - -#define DB_RADDISTMODE_BOUGEUT 4 -#define DB_RADDISTMODE_2NDORDER 5 -#define DB_RADDISTMODE_IDENTITY 6 - -/*! -Give reasonable guess of the calibration matrix for normalization purposes. -Use real K matrix when doing real geometry. -focal length = (w+h)/2.0*f_correction. -\param K calibration matrix (out) -\param Kinv inverse of K (out) -\param im_width image width -\param im_height image height -\param f_correction focal length correction factor -\param field set to 1 if this is a field image (fy = fx/2) -\return K(3x3) intrinsic calibration matrix -*/ -DB_API void db_Approx3DCalMat(double K[9],double Kinv[9],int im_width,int im_height,double f_correction=1.0,int field=0); - -/*! - Make a 2x2 identity matrix - */ -void inline db_Identity2x2(double A[4]) -{ - A[0]=1;A[1]=0; - A[2]=0;A[3]=1; -} -/*! - Make a 3x3 identity matrix - */ -void inline db_Identity3x3(double A[9]) -{ - A[0]=1;A[1]=0;A[2]=0; - A[3]=0;A[4]=1;A[5]=0; - A[6]=0;A[7]=0;A[8]=1; -} -/*! - Invert intrinsic calibration matrix K(3x3) - If fx or fy is 0, I is returned. - */ -void inline db_InvertCalibrationMatrix(double Kinv[9],const double K[9]) -{ - double a,b,c,d,e,f,ainv,dinv,adinv; - - a=K[0];b=K[1];c=K[2];d=K[4];e=K[5];f=K[8]; - if((a==0.0)||(d==0.0)) db_Identity3x3(Kinv); - else - { - Kinv[3]=0.0; - Kinv[6]=0.0; - Kinv[7]=0.0; - Kinv[8]=1.0; - - ainv=1.0/a; - dinv=1.0/d; - adinv=ainv*dinv; - Kinv[0]=f*ainv; - Kinv[1]= -b*f*adinv; - Kinv[2]=(b*e-c*d)*adinv; - Kinv[4]=f*dinv; - Kinv[5]= -e*dinv; - } -} -/*! - De-homogenize image point: xd(1:2) = xs(1:2)/xs(3). - If xs(3) is 0, xd will become 0 - \param xd destination point - \param xs source point - */ -void inline db_DeHomogenizeImagePoint(double xd[2],const double xs[3]) -{ - double temp,div; - - temp=xs[2]; - if(temp!=0) - { - div=1.0/temp; - xd[0]=xs[0]*div;xd[1]=xs[1]*div; - } - else - { - xd[0]=0.0;xd[1]=0.0; - } -} - - -/*! - Orthonormalize 3D rotation R - */ -inline void db_OrthonormalizeRotation(double R[9]) -{ - double s,mult; - /*Normalize first vector*/ - s=db_sqr(R[0])+db_sqr(R[1])+db_sqr(R[2]); - mult=sqrt(1.0/(s?s:1)); - R[0]*=mult; R[1]*=mult; R[2]*=mult; - /*Subtract scalar product from second vector*/ - s=R[0]*R[3]+R[1]*R[4]+R[2]*R[5]; - R[3]-=s*R[0]; R[4]-=s*R[1]; R[5]-=s*R[2]; - /*Normalize second vector*/ - s=db_sqr(R[3])+db_sqr(R[4])+db_sqr(R[5]); - mult=sqrt(1.0/(s?s:1)); - R[3]*=mult; R[4]*=mult; R[5]*=mult; - /*Get third vector by vector product*/ - R[6]=R[1]*R[5]-R[4]*R[2]; - R[7]=R[2]*R[3]-R[5]*R[0]; - R[8]=R[0]*R[4]-R[3]*R[1]; -} -/*! -Update a rotation with the update dx=[sin(phi) sin(ohm) sin(kap)] -*/ -inline void db_UpdateRotation(double R_p_dx[9],double R[9],const double dx[3]) -{ - double R_temp[9]; - /*Update rotation*/ - db_IncrementalRotationMatrix(R_temp,dx); - db_Multiply3x3_3x3(R_p_dx,R_temp,R); -} -/*! - Compute xp = Hx for inhomogenous image points. - */ -inline void db_ImageHomographyInhomogenous(double xp[2],const double H[9],const double x[2]) -{ - double x3,m; - - x3=H[6]*x[0]+H[7]*x[1]+H[8]; - if(x3!=0.0) - { - m=1.0/x3; - xp[0]=m*(H[0]*x[0]+H[1]*x[1]+H[2]); - xp[1]=m*(H[3]*x[0]+H[4]*x[1]+H[5]); - } - else - { - xp[0]=xp[1]=0.0; - } -} -inline double db_FocalFromCamRotFocalHomography(const double H[9]) -{ - double k1,k2; - - k1=db_sqr(H[2])+db_sqr(H[5]); - k2=db_sqr(H[6])+db_sqr(H[7]); - if(k1>=k2) - { - return(db_SafeSqrt(db_SafeDivision(k1,1.0-db_sqr(H[8])))); - } - else - { - return(db_SafeSqrt(db_SafeDivision(1.0-db_sqr(H[8]),k2))); - } -} - -inline double db_FocalAndRotFromCamRotFocalHomography(double R[9],const double H[9]) -{ - double back,fi; - - back=db_FocalFromCamRotFocalHomography(H); - fi=db_SafeReciprocal(back); - R[0]=H[0]; R[1]=H[1]; R[2]=fi*H[2]; - R[3]=H[3]; R[4]=H[4]; R[5]=fi*H[5]; - R[6]=back*H[6]; R[7]=back*H[7]; R[8]=H[8]; - return(back); -} -/*! -Compute Jacobian at zero of three coordinates dR*x with -respect to the update dR([sin(phi) sin(ohm) sin(kap)]) given x. - -The Jacobian at zero of the homogenous coordinates with respect to - [sin(phi) sin(ohm) sin(kap)] is -\code - [-rx2 0 rx1 ] - [ 0 rx2 -rx0 ] - [ rx0 -rx1 0 ]. -\endcode - -*/ -inline void db_JacobianOfRotatedPointStride(double J[9],const double x[3],int stride) -{ - /*The Jacobian at zero of the homogenous coordinates with respect to - [sin(phi) sin(ohm) sin(kap)] is - [-rx2 0 rx1 ] - [ 0 rx2 -rx0 ] - [ rx0 -rx1 0 ]*/ - - J[0]= -x[stride<<1]; - J[1]=0; - J[2]= x[stride]; - J[3]=0; - J[4]= x[stride<<1]; - J[5]= -x[0]; - J[6]= x[0]; - J[7]= -x[stride]; - J[8]=0; -} -/*! - Invert an affine (if possible) - \param Hinv inverted matrix - \param H input matrix - \return true if success and false if matrix is ill-conditioned (det < 1e-7) - */ -inline bool db_InvertAffineTransform(double Hinv[9],const double H[9]) -{ - double det=H[0]*H[4]-H[3]*H[1]; - if (det<1e-7) - { - db_Copy9(Hinv,H); - return false; - } - else - { - Hinv[0]=H[4]/det; - Hinv[1]=-H[1]/det; - Hinv[3]=-H[3]/det; - Hinv[4]=H[0]/det; - Hinv[2]= -Hinv[0]*H[2]-Hinv[1]*H[5]; - Hinv[5]= -Hinv[3]*H[2]-Hinv[4]*H[5]; - } - return true; -} - -/*! -Update of upper 2x2 is multiplication by -\code -[s 0][ cos(theta) sin(theta)] -[0 s][-sin(theta) cos(theta)] -\endcode -*/ -inline void db_MultiplyScaleOntoImageHomography(double H[9],double s) -{ - - H[0]*=s; - H[1]*=s; - H[3]*=s; - H[4]*=s; -} -/*! -Update of upper 2x2 is multiplication by -\code -[s 0][ cos(theta) sin(theta)] -[0 s][-sin(theta) cos(theta)] -\endcode -*/ -inline void db_MultiplyRotationOntoImageHomography(double H[9],double theta) -{ - double c,s,H0,H1; - - - c=cos(theta); - s=db_SafeSqrt(1.0-db_sqr(c)); - H0= c*H[0]+s*H[3]; - H[3]= -s*H[0]+c*H[3]; - H[0]=H0; - H1=c*H[1]+s*H[4]; - H[4]= -s*H[1]+c*H[4]; - H[1]=H1; -} - -inline void db_UpdateImageHomographyAffine(double H_p_dx[9],const double H[9],const double dx[6]) -{ - db_AddVectors6(H_p_dx,H,dx); - db_Copy3(H_p_dx+6,H+6); -} - -inline void db_UpdateImageHomographyProjective(double H_p_dx[9],const double H[9],const double dx[8],int frozen_coord) -{ - int i,j; - - for(j=0,i=0;i<9;i++) - { - if(i!=frozen_coord) - { - H_p_dx[i]=H[i]+dx[j]; - j++; - } - else H_p_dx[i]=H[i]; - } -} - -inline void db_UpdateRotFocalHomography(double H_p_dx[9],const double H[9],const double dx[4]) -{ - double f,fp,fpi; - double R[9],dR[9]; - - /*Updated matrix is diag(f+df,f+df)*dR*R*diag(1/(f+df),1/(f+df),1)*/ - f=db_FocalAndRotFromCamRotFocalHomography(R,H); - db_IncrementalRotationMatrix(dR,dx); - db_Multiply3x3_3x3(H_p_dx,dR,R); - fp=f+dx[3]; - fpi=db_SafeReciprocal(fp); - H_p_dx[2]*=fp; - H_p_dx[5]*=fp; - H_p_dx[6]*=fpi; - H_p_dx[7]*=fpi; -} - -/*\}*/ -#endif /* DB_UTILITIES_CAMERA */ diff --git a/jni/feature_stab/db_vlvm/db_utilities_constants.h b/jni/feature_stab/db_vlvm/db_utilities_constants.h deleted file mode 100644 index 07565efd2..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities_constants.h +++ /dev/null @@ -1,208 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities_constants.h,v 1.2 2011/06/17 14:03:31 mbansal Exp $ */ - -#ifndef DB_UTILITIES_CONSTANTS -#define DB_UTILITIES_CONSTANTS - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ - -/****************Constants********************/ -#define DB_E 2.7182818284590452354 -#define DB_LOG2E 1.4426950408889634074 -#define DB_LOG10E 0.43429448190325182765 -#define DB_LN2 0.69314718055994530942 -#define DB_LN10 2.30258509299404568402 -#define DB_PI 3.1415926535897932384626433832795 -#define DB_PI_2 1.57079632679489661923 -#define DB_PI_4 0.78539816339744830962 -#define DB_1_PI 0.31830988618379067154 -#define DB_2_PI 0.63661977236758134308 -#define DB_SQRTPI 1.7724538509055160272981674833411 -#define DB_SQRT_2PI 2.506628274631000502415765284811 -#define DB_2_SQRTPI 1.12837916709551257390 -#define DB_SQRT2 1.41421356237309504880 -#define DB_SQRT3 1.7320508075688772935274463415059 -#define DB_SQRT1_2 0.70710678118654752440 -#define DB_EPS 2.220446049250313e-016 /* for 32 bit double */ - -/****************Default Parameters********************/ -/*Preemptive ransac parameters*/ -#define DB_DEFAULT_NR_SAMPLES 500 -#define DB_DEFAULT_CHUNK_SIZE 100 -#define DB_DEFAULT_GROUP_SIZE 10 - -/*Optimisation parameters*/ -#define DB_DEFAULT_MAX_POINTS 1000 -#define DB_DEFAULT_MAX_ITERATIONS 25 -#define DB_DEFAULT_IMP_REQ 0.001 - -/*Feature standard deviation parameters*/ -#define DB_POINT_STANDARDDEV (1.0/(826.0)) /*1 pixel for CIF (fraction of (image width+image height)/2)*/ -#define DB_OUTLIER_THRESHOLD 3.0 /*In number of DB_POINT_STANDARDDEV's*/ -#define DB_WORST_CASE 50.0 /*In number of DB_POINT_STANDARDDEV's*/ - -/*Front-end parameters*/ -#define DB_DEFAULT_TARGET_NR_CORNERS 5000 -#define DB_DEFAULT_NR_FEATURE_BLOCKS 10 -#define DB_DEFAULT_ABS_CORNER_THRESHOLD 50000000.0 -#define DB_DEFAULT_REL_CORNER_THRESHOLD 0.00005 -#define DB_DEFAULT_MAX_DISPARITY 0.1 -#define DB_DEFAULT_NO_DISPARITY -1.0 -#define DB_DEFAULT_MAX_TRACK_LENGTH 300 - -#define DB_DEFAULT_MAX_NR_CAMERAS 1000 - -#define DB_DEFAULT_TRIPLE_STEP 2 -#define DB_DEFAULT_DOUBLE_STEP 2 -#define DB_DEFAULT_SINGLE_STEP 1 -#define DB_DEFAULT_NR_SINGLES 10 -#define DB_DEFAULT_NR_DOUBLES 1 -#define DB_DEFAULT_NR_TRIPLES 1 - -#define DB_DEFAULT_TRIFOCAL_FOUR_STEPS 40 - -#define DB_DEFAULT_EPIPOLAR_ERROR 1 /*in pixels*/ - -////////////////////////// DOXYGEN ///////////////////// - -/*! - * \def DB_DEFAULT_GROUP_SIZE - * \ingroup LMRobust - * \brief Default group size for db_PreemptiveRansac class. - * Group size is the number of observation costs multiplied together - * before a log of the product is added to the total cost. -*/ - -/*! - * \def DB_DEFAULT_TARGET_NR_CORNERS - * \ingroup FeatureDetection - * \brief Default target number of corners -*/ -/*! - * \def DB_DEFAULT_NR_FEATURE_BLOCKS - * \ingroup FeatureDetection - * \brief Default number of regions (horizontal or vertical) that are considered separately - * for feature detection. The greater the number, the more uniform the distribution of - * detected features. -*/ -/*! - * \def DB_DEFAULT_ABS_CORNER_THRESHOLD - * \ingroup FeatureDetection - * \brief Absolute feature strength threshold. -*/ -/*! - * \def DB_DEFAULT_REL_CORNER_THRESHOLD - * \ingroup FeatureDetection - * \brief Relative feature strength threshold. -*/ -/*! - * \def DB_DEFAULT_MAX_DISPARITY - * \ingroup FeatureMatching - * \brief Maximum disparity (as fraction of image size) allowed in feature matching -*/ - /*! - * \def DB_DEFAULT_NO_DISPARITY - * \ingroup FeatureMatching - * \brief Indicates that vertical disparity is the same as horizontal disparity. -*/ -/////////////////////////////////////////////////////////////////////////////////// - /*! - * \def DB_E - * \ingroup LMBasicUtilities - * \brief e -*/ - /*! - * \def DB_LOG2E - * \ingroup LMBasicUtilities - * \brief log2(e) -*/ - /*! - * \def DB_LOG10E - * \ingroup LMBasicUtilities - * \brief log10(e) -*/ - /*! - * \def DB_LOG10E - * \ingroup LMBasicUtilities - * \brief log10(e) -*/ -/*! - * \def DB_LN2 - * \ingroup LMBasicUtilities - * \brief ln(2) -*/ -/*! - * \def DB_LN10 - * \ingroup LMBasicUtilities - * \brief ln(10) -*/ -/*! - * \def DB_PI - * \ingroup LMBasicUtilities - * \brief Pi -*/ -/*! - * \def DB_PI_2 - * \ingroup LMBasicUtilities - * \brief Pi/2 -*/ -/*! - * \def DB_PI_4 - * \ingroup LMBasicUtilities - * \brief Pi/4 -*/ -/*! - * \def DB_1_PI - * \ingroup LMBasicUtilities - * \brief 1/Pi -*/ -/*! - * \def DB_2_PI - * \ingroup LMBasicUtilities - * \brief 2/Pi -*/ -/*! - * \def DB_SQRTPI - * \ingroup LMBasicUtilities - * \brief sqrt(Pi) -*/ -/*! - * \def DB_SQRT_2PI - * \ingroup LMBasicUtilities - * \brief sqrt(2*Pi) -*/ -/*! - * \def DB_SQRT2 - * \ingroup LMBasicUtilities - * \brief sqrt(2) -*/ -/*! - * \def DB_SQRT3 - * \ingroup LMBasicUtilities - * \brief sqrt(3) -*/ -/*! - * \def DB_SQRT1_2 - * \ingroup LMBasicUtilities - * \brief sqrt(1/2) -*/ -#endif /* DB_UTILITIES_CONSTANTS */ - - diff --git a/jni/feature_stab/db_vlvm/db_utilities_geometry.h b/jni/feature_stab/db_vlvm/db_utilities_geometry.h deleted file mode 100644 index f21558467..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities_geometry.h +++ /dev/null @@ -1,121 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities_geometry.h,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */ - -#ifndef DB_UTILITIES_GEOMETRY_H -#define DB_UTILITIES_GEOMETRY_H - -#include "db_utilities.h" - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ -/*! Get the inhomogenous 2D-point centroid of nr_point inhomogenous -points in X*/ -inline void db_PointCentroid2D(double c[2],const double *X,int nr_points) -{ - int i; - double cx,cy,m; - - cx=0;cy=0; - for(i=0;i<nr_points;i++) - { - cx+= *X++; - cy+= *X++; - } - if(nr_points) - { - m=1.0/((double)nr_points); - c[0]=cx*m; - c[1]=cy*m; - } - else c[0]=c[1]=0; -} - -inline void db_PointCentroid2D(double c[2],const double * const *X,int nr_points) -{ - int i; - double cx,cy,m; - const double *temp; - - cx=0;cy=0; - for(i=0;i<nr_points;i++) - { - temp= *X++; - cx+=temp[0]; - cy+=temp[1]; - } - if(nr_points) - { - m=1.0/((double)nr_points); - c[0]=cx*m; - c[1]=cy*m; - } - else c[0]=c[1]=0; -} - -/*! Get the inhomogenous 3D-point centroid of nr_point inhomogenous -points in X*/ -inline void db_PointCentroid3D(double c[3],const double *X,int nr_points) -{ - int i; - double cx,cy,cz,m; - - cx=0;cy=0;cz=0; - for(i=0;i<nr_points;i++) - { - cx+= *X++; - cy+= *X++; - cz+= *X++; - } - if(nr_points) - { - m=1.0/((double)nr_points); - c[0]=cx*m; - c[1]=cy*m; - c[2]=cz*m; - } - else c[0]=c[1]=c[2]=0; -} - -inline void db_PointCentroid3D(double c[3],const double * const *X,int nr_points) -{ - int i; - double cx,cy,cz,m; - const double *temp; - - cx=0;cy=0;cz=0; - for(i=0;i<nr_points;i++) - { - temp= *X++; - cx+=temp[0]; - cy+=temp[1]; - cz+=temp[2]; - } - if(nr_points) - { - m=1.0/((double)nr_points); - c[0]=cx*m; - c[1]=cy*m; - c[2]=cz*m; - } - else c[0]=c[1]=c[2]=0; -} - -#endif /* DB_UTILITIES_GEOMETRY_H */ diff --git a/jni/feature_stab/db_vlvm/db_utilities_indexing.cpp b/jni/feature_stab/db_vlvm/db_utilities_indexing.cpp deleted file mode 100644 index 30ce03aa6..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities_indexing.cpp +++ /dev/null @@ -1,120 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities_indexing.cpp,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */ - -#include "db_utilities_indexing.h" -#include "db_utilities.h" - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ - -void db_Zero(double *d,long nr) -{ - long i; - for(i=0;i<nr;i++) d[i]=0.0; -} - -/*This routine breaks number in source into values smaller and larger than -a pivot element. Values equal to the pivot are ignored*/ -void db_LeanPartitionOnPivot(double pivot,double *dest,const double *source,long first,long last,long *first_equal,long *last_equal) -{ - double temp; - const double *s_point; - const double *s_top; - double *d_bottom; - double *d_top; - - s_point=source+first; - s_top=source+last; - d_bottom=dest+first; - d_top=dest+last; - - for(;s_point<=s_top;) - { - temp= *(s_point++); - if(temp<pivot) *(d_bottom++)=temp; - else if(temp>pivot) *(d_top--)=temp; - } - *first_equal=d_bottom-dest; - *last_equal=d_top-dest; -} - -double db_LeanQuickSelect(const double *s,long nr_elements,long pos,double *temp) -{ - long first=0; - long last=nr_elements-1; - double pivot; - long first_equal,last_equal; - double *tempA; - double *tempB; - double *tempC; - const double *source; - double *dest; - - tempA=temp; - tempB=temp+nr_elements; - source=s; - dest=tempA; - - for(;last-first>2;) - { - pivot=db_TripleMedian(source[first],source[last],source[(first+last)/2]); - db_LeanPartitionOnPivot(pivot,dest,source,first,last,&first_equal,&last_equal); - - if(first_equal>pos) last=first_equal-1; - else if(last_equal<pos) first=last_equal+1; - else - { - return(pivot); - } - - /*Swap pointers*/ - tempC=tempA; - tempA=tempB; - tempB=tempC; - source=tempB; - dest=tempA; - } - pivot=db_TripleMedian(source[first],source[last],source[(first+last)/2]); - - return(pivot); -} - -float* db_AlignPointer_f(float *p,unsigned long nr_bytes) -{ - float *ap; - unsigned long m; - - m=((unsigned long)p)%nr_bytes; - if(m) ap=(float*) (((unsigned long)p)-m+nr_bytes); - else ap=p; - return(ap); -} - -short* db_AlignPointer_s(short *p,unsigned long nr_bytes) -{ - short *ap; - unsigned long m; - - m=((unsigned long)p)%nr_bytes; - if(m) ap=(short*) (((unsigned long)p)-m+nr_bytes); - else ap=p; - return(ap); -} diff --git a/jni/feature_stab/db_vlvm/db_utilities_indexing.h b/jni/feature_stab/db_vlvm/db_utilities_indexing.h deleted file mode 100644 index 01eeb9ea2..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities_indexing.h +++ /dev/null @@ -1,270 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities_indexing.h,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */ - -#ifndef DB_UTILITIES_INDEXING -#define DB_UTILITIES_INDEXING - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ - -#include "db_utilities.h" - -/*! - * \defgroup LMIndexing (LM) Indexing Utilities (Order Statistics, Matrix Operations) - */ -/*\{*/ - -inline void db_SetupMatrixRefs(double **ar,long rows,long cols,double *a) -{ - long i; - for(i=0;i<rows;i++) ar[i]=&a[i*cols]; -} - -inline void db_SymmetricExtendUpperToLower(double **A,int rows,int cols) -{ - int i,j; - for(i=1;i<rows;i++) for(j=0;j<i;j++) A[i][j]=A[j][i]; -} - -void inline db_MultiplyMatrixVectorAtb(double *c,const double * const *At,const double *b,int arows,int acols) -{ - int i,j; - double acc; - - for(i=0;i<arows;i++) - { - acc=0; - for(j=0;j<acols;j++) acc+=At[j][i]*b[j]; - c[i]=acc; - } -} - -inline void db_MultiplyMatricesAB(double **C,const double * const *A,const double * const *B,int arows,int acols,int bcols) -{ - int i,j,k; - double acc; - - for(i=0;i<arows;i++) for(j=0;j<bcols;j++) - { - acc=0; - for(k=0;k<acols;k++) acc+=A[i][k]*B[k][j]; - C[i][j]=acc; - } -} - -inline void db_UpperMultiplyMatricesAtB(double **Cu,const double * const *At,const double * const *B,int arows,int acols,int bcols) -{ - int i,j,k; - double acc; - - for(i=0;i<arows;i++) for(j=i;j<bcols;j++) - { - acc=0; - for(k=0;k<acols;k++) acc+=At[k][i]*B[k][j]; - Cu[i][j]=acc; - } -} - -DB_API void db_Zero(double *d,long nr); - -inline int db_MaxIndex2(double s[2]) -{ - if(s[0]>=s[1]) return(0); - return(1); -} - -inline int db_MaxIndex3(const double s[3]) -{ - double best; - int pos; - - best=s[0];pos=0; - if(s[1]>best){best=s[1];pos=1;} - if(s[2]>best){best=s[2];pos=2;} - return(pos); -} - -inline int db_MaxIndex4(const double s[4]) -{ - double best; - int pos; - - best=s[0];pos=0; - if(s[1]>best){best=s[1];pos=1;} - if(s[2]>best){best=s[2];pos=2;} - if(s[3]>best){best=s[3];pos=3;} - return(pos); -} - -inline int db_MaxIndex5(const double s[5]) -{ - double best; - int pos; - - best=s[0];pos=0; - if(s[1]>best){best=s[1];pos=1;} - if(s[2]>best){best=s[2];pos=2;} - if(s[3]>best){best=s[3];pos=3;} - if(s[4]>best){best=s[4];pos=4;} - return(pos); -} - -inline int db_MaxIndex6(const double s[6]) -{ - double best; - int pos; - - best=s[0];pos=0; - if(s[1]>best){best=s[1];pos=1;} - if(s[2]>best){best=s[2];pos=2;} - if(s[3]>best){best=s[3];pos=3;} - if(s[4]>best){best=s[4];pos=4;} - if(s[5]>best){best=s[5];pos=5;} - return(pos); -} - -inline int db_MaxIndex7(const double s[7]) -{ - double best; - int pos; - - best=s[0];pos=0; - if(s[1]>best){best=s[1];pos=1;} - if(s[2]>best){best=s[2];pos=2;} - if(s[3]>best){best=s[3];pos=3;} - if(s[4]>best){best=s[4];pos=4;} - if(s[5]>best){best=s[5];pos=5;} - if(s[6]>best){best=s[6];pos=6;} - return(pos); -} - -inline int db_MinIndex7(const double s[7]) -{ - double best; - int pos; - - best=s[0];pos=0; - if(s[1]<best){best=s[1];pos=1;} - if(s[2]<best){best=s[2];pos=2;} - if(s[3]<best){best=s[3];pos=3;} - if(s[4]<best){best=s[4];pos=4;} - if(s[5]<best){best=s[5];pos=5;} - if(s[6]<best){best=s[6];pos=6;} - return(pos); -} - -inline int db_MinIndex9(const double s[9]) -{ - double best; - int pos; - - best=s[0];pos=0; - if(s[1]<best){best=s[1];pos=1;} - if(s[2]<best){best=s[2];pos=2;} - if(s[3]<best){best=s[3];pos=3;} - if(s[4]<best){best=s[4];pos=4;} - if(s[5]<best){best=s[5];pos=5;} - if(s[6]<best){best=s[6];pos=6;} - if(s[7]<best){best=s[7];pos=7;} - if(s[8]<best){best=s[8];pos=8;} - return(pos); -} - -inline int db_MaxAbsIndex3(const double *s) -{ - double t,best; - int pos; - - best=fabs(s[0]);pos=0; - t=fabs(s[1]);if(t>best){best=t;pos=1;} - t=fabs(s[2]);if(t>best){pos=2;} - return(pos); -} - -inline int db_MaxAbsIndex9(const double *s) -{ - double t,best; - int pos; - - best=fabs(s[0]);pos=0; - t=fabs(s[1]);if(t>best){best=t;pos=1;} - t=fabs(s[2]);if(t>best){best=t;pos=2;} - t=fabs(s[3]);if(t>best){best=t;pos=3;} - t=fabs(s[4]);if(t>best){best=t;pos=4;} - t=fabs(s[5]);if(t>best){best=t;pos=5;} - t=fabs(s[6]);if(t>best){best=t;pos=6;} - t=fabs(s[7]);if(t>best){best=t;pos=7;} - t=fabs(s[8]);if(t>best){best=t;pos=8;} - return(pos); -} - - -/*! -Select ordinal pos (zero based) out of nr_elements in s. -temp should point to alloced memory of at least nr_elements*2 -Optimized runtimes on 450MHz: -\code - 30 with 3 microsecs - 100 with 11 microsecs - 300 with 30 microsecs - 500 with 40 microsecs -1000 with 100 microsecs -5000 with 540 microsecs -\endcode -so the expected runtime is around -(nr_elements/10) microseconds -The total quickselect cost of splitting 500 hypotheses recursively -is thus around 100 microseconds - -Does the same operation as std::nth_element(). -*/ -DB_API double db_LeanQuickSelect(const double *s,long nr_elements,long pos,double *temp); - -/*! - Median of 3 doubles - */ -inline double db_TripleMedian(double a,double b,double c) -{ - if(a>b) - { - if(c>a) return(a); - else if(c>b) return(c); - else return(b); - } - else - { - if(c>b) return(b); - else if(c>a) return(c); - else return(a); - } -} - -/*! -Align float pointer to nr_bytes by moving forward -*/ -DB_API float* db_AlignPointer_f(float *p,unsigned long nr_bytes); - -/*! -Align short pointer to nr_bytes by moving forward -*/ -DB_API short* db_AlignPointer_s(short *p,unsigned long nr_bytes); - -#endif /* DB_UTILITIES_INDEXING */ diff --git a/jni/feature_stab/db_vlvm/db_utilities_linalg.cpp b/jni/feature_stab/db_vlvm/db_utilities_linalg.cpp deleted file mode 100644 index 8f68b303a..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities_linalg.cpp +++ /dev/null @@ -1,376 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities_linalg.cpp,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */ - -#include "db_utilities_linalg.h" -#include "db_utilities.h" - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ - -/*Cholesky-factorize symmetric positive definite 6 x 6 matrix A. Upper -part of A is used from the input. The Cholesky factor is output as -subdiagonal part in A and diagonal in d, which is 6-dimensional*/ -void db_CholeskyDecomp6x6(double A[36],double d[6]) -{ - double s,temp; - - /*[50 mult 35 add 6sqrt=85flops 6func]*/ - /*i=0*/ - s=A[0]; - d[0]=((s>0.0)?sqrt(s):1.0); - temp=db_SafeReciprocal(d[0]); - A[6]=A[1]*temp; - A[12]=A[2]*temp; - A[18]=A[3]*temp; - A[24]=A[4]*temp; - A[30]=A[5]*temp; - /*i=1*/ - s=A[7]-A[6]*A[6]; - d[1]=((s>0.0)?sqrt(s):1.0); - temp=db_SafeReciprocal(d[1]); - A[13]=(A[8]-A[6]*A[12])*temp; - A[19]=(A[9]-A[6]*A[18])*temp; - A[25]=(A[10]-A[6]*A[24])*temp; - A[31]=(A[11]-A[6]*A[30])*temp; - /*i=2*/ - s=A[14]-A[12]*A[12]-A[13]*A[13]; - d[2]=((s>0.0)?sqrt(s):1.0); - temp=db_SafeReciprocal(d[2]); - A[20]=(A[15]-A[12]*A[18]-A[13]*A[19])*temp; - A[26]=(A[16]-A[12]*A[24]-A[13]*A[25])*temp; - A[32]=(A[17]-A[12]*A[30]-A[13]*A[31])*temp; - /*i=3*/ - s=A[21]-A[18]*A[18]-A[19]*A[19]-A[20]*A[20]; - d[3]=((s>0.0)?sqrt(s):1.0); - temp=db_SafeReciprocal(d[3]); - A[27]=(A[22]-A[18]*A[24]-A[19]*A[25]-A[20]*A[26])*temp; - A[33]=(A[23]-A[18]*A[30]-A[19]*A[31]-A[20]*A[32])*temp; - /*i=4*/ - s=A[28]-A[24]*A[24]-A[25]*A[25]-A[26]*A[26]-A[27]*A[27]; - d[4]=((s>0.0)?sqrt(s):1.0); - temp=db_SafeReciprocal(d[4]); - A[34]=(A[29]-A[24]*A[30]-A[25]*A[31]-A[26]*A[32]-A[27]*A[33])*temp; - /*i=5*/ - s=A[35]-A[30]*A[30]-A[31]*A[31]-A[32]*A[32]-A[33]*A[33]-A[34]*A[34]; - d[5]=((s>0.0)?sqrt(s):1.0); -} - -/*Cholesky-factorize symmetric positive definite n x n matrix A.Part -above diagonal of A is used from the input, diagonal of A is assumed to -be stored in d. The Cholesky factor is output as -subdiagonal part in A and diagonal in d, which is n-dimensional*/ -void db_CholeskyDecompSeparateDiagonal(double **A,double *d,int n) -{ - int i,j,k; - double s; - double temp = 0.0; - - for(i=0;i<n;i++) for(j=i;j<n;j++) - { - if(i==j) s=d[i]; - else s=A[i][j]; - for(k=i-1;k>=0;k--) s-=A[i][k]*A[j][k]; - if(i==j) - { - d[i]=((s>0.0)?sqrt(s):1.0); - temp=db_SafeReciprocal(d[i]); - } - else A[j][i]=s*temp; - } -} - -/*Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition -of an n x n matrix and the right hand side b. The vector b is unchanged*/ -void db_CholeskyBacksub(double *x,const double * const *A,const double *d,int n,const double *b) -{ - int i,k; - double s; - - for(i=0;i<n;i++) - { - for(s=b[i],k=i-1;k>=0;k--) s-=A[i][k]*x[k]; - x[i]=db_SafeDivision(s,d[i]); - } - for(i=n-1;i>=0;i--) - { - for(s=x[i],k=i+1;k<n;k++) s-=A[k][i]*x[k]; - x[i]=db_SafeDivision(s,d[i]); - } -} - -/*Cholesky-factorize symmetric positive definite 3 x 3 matrix A. Part -above diagonal of A is used from the input, diagonal of A is assumed to -be stored in d. The Cholesky factor is output as subdiagonal part in A -and diagonal in d, which is 3-dimensional*/ -void db_CholeskyDecomp3x3SeparateDiagonal(double A[9],double d[3]) -{ - double s,temp; - - /*i=0*/ - s=d[0]; - d[0]=((s>0.0)?sqrt(s):1.0); - temp=db_SafeReciprocal(d[0]); - A[3]=A[1]*temp; - A[6]=A[2]*temp; - /*i=1*/ - s=d[1]-A[3]*A[3]; - d[1]=((s>0.0)?sqrt(s):1.0); - temp=db_SafeReciprocal(d[1]); - A[7]=(A[5]-A[3]*A[6])*temp; - /*i=2*/ - s=d[2]-A[6]*A[6]-A[7]*A[7]; - d[2]=((s>0.0)?sqrt(s):1.0); -} - -/*Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition -of a 3 x 3 matrix and the right hand side b. The vector b is unchanged*/ -void db_CholeskyBacksub3x3(double x[3],const double A[9],const double d[3],const double b[3]) -{ - /*[42 mult 30 add=72flops]*/ - x[0]=db_SafeDivision(b[0],d[0]); - x[1]=db_SafeDivision((b[1]-A[3]*x[0]),d[1]); - x[2]=db_SafeDivision((b[2]-A[6]*x[0]-A[7]*x[1]),d[2]); - x[2]=db_SafeDivision(x[2],d[2]); - x[1]=db_SafeDivision((x[1]-A[7]*x[2]),d[1]); - x[0]=db_SafeDivision((x[0]-A[6]*x[2]-A[3]*x[1]),d[0]); -} - -/*Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition -of a 6 x 6 matrix and the right hand side b. The vector b is unchanged*/ -void db_CholeskyBacksub6x6(double x[6],const double A[36],const double d[6],const double b[6]) -{ - /*[42 mult 30 add=72flops]*/ - x[0]=db_SafeDivision(b[0],d[0]); - x[1]=db_SafeDivision((b[1]-A[6]*x[0]),d[1]); - x[2]=db_SafeDivision((b[2]-A[12]*x[0]-A[13]*x[1]),d[2]); - x[3]=db_SafeDivision((b[3]-A[18]*x[0]-A[19]*x[1]-A[20]*x[2]),d[3]); - x[4]=db_SafeDivision((b[4]-A[24]*x[0]-A[25]*x[1]-A[26]*x[2]-A[27]*x[3]),d[4]); - x[5]=db_SafeDivision((b[5]-A[30]*x[0]-A[31]*x[1]-A[32]*x[2]-A[33]*x[3]-A[34]*x[4]),d[5]); - x[5]=db_SafeDivision(x[5],d[5]); - x[4]=db_SafeDivision((x[4]-A[34]*x[5]),d[4]); - x[3]=db_SafeDivision((x[3]-A[33]*x[5]-A[27]*x[4]),d[3]); - x[2]=db_SafeDivision((x[2]-A[32]*x[5]-A[26]*x[4]-A[20]*x[3]),d[2]); - x[1]=db_SafeDivision((x[1]-A[31]*x[5]-A[25]*x[4]-A[19]*x[3]-A[13]*x[2]),d[1]); - x[0]=db_SafeDivision((x[0]-A[30]*x[5]-A[24]*x[4]-A[18]*x[3]-A[12]*x[2]-A[6]*x[1]),d[0]); -} - - -void db_Orthogonalize6x7(double A[42],int orthonormalize) -{ - int i; - double ss[6]; - - /*Compute square sums of rows*/ - ss[0]=db_SquareSum7(A); - ss[1]=db_SquareSum7(A+7); - ss[2]=db_SquareSum7(A+14); - ss[3]=db_SquareSum7(A+21); - ss[4]=db_SquareSum7(A+28); - ss[5]=db_SquareSum7(A+35); - - ss[1]-=db_OrthogonalizePair7(A+7 ,A,ss[0]); - ss[2]-=db_OrthogonalizePair7(A+14,A,ss[0]); - ss[3]-=db_OrthogonalizePair7(A+21,A,ss[0]); - ss[4]-=db_OrthogonalizePair7(A+28,A,ss[0]); - ss[5]-=db_OrthogonalizePair7(A+35,A,ss[0]); - - /*Pivot on largest ss (could also be done on ss/(original_ss))*/ - i=db_MaxIndex5(ss+1); - db_OrthogonalizationSwap7(A+7,i,ss+1); - - ss[2]-=db_OrthogonalizePair7(A+14,A+7,ss[1]); - ss[3]-=db_OrthogonalizePair7(A+21,A+7,ss[1]); - ss[4]-=db_OrthogonalizePair7(A+28,A+7,ss[1]); - ss[5]-=db_OrthogonalizePair7(A+35,A+7,ss[1]); - - i=db_MaxIndex4(ss+2); - db_OrthogonalizationSwap7(A+14,i,ss+2); - - ss[3]-=db_OrthogonalizePair7(A+21,A+14,ss[2]); - ss[4]-=db_OrthogonalizePair7(A+28,A+14,ss[2]); - ss[5]-=db_OrthogonalizePair7(A+35,A+14,ss[2]); - - i=db_MaxIndex3(ss+3); - db_OrthogonalizationSwap7(A+21,i,ss+3); - - ss[4]-=db_OrthogonalizePair7(A+28,A+21,ss[3]); - ss[5]-=db_OrthogonalizePair7(A+35,A+21,ss[3]); - - i=db_MaxIndex2(ss+4); - db_OrthogonalizationSwap7(A+28,i,ss+4); - - ss[5]-=db_OrthogonalizePair7(A+35,A+28,ss[4]); - - if(orthonormalize) - { - db_MultiplyScalar7(A ,db_SafeSqrtReciprocal(ss[0])); - db_MultiplyScalar7(A+7 ,db_SafeSqrtReciprocal(ss[1])); - db_MultiplyScalar7(A+14,db_SafeSqrtReciprocal(ss[2])); - db_MultiplyScalar7(A+21,db_SafeSqrtReciprocal(ss[3])); - db_MultiplyScalar7(A+28,db_SafeSqrtReciprocal(ss[4])); - db_MultiplyScalar7(A+35,db_SafeSqrtReciprocal(ss[5])); - } -} - -void db_Orthogonalize8x9(double A[72],int orthonormalize) -{ - int i; - double ss[8]; - - /*Compute square sums of rows*/ - ss[0]=db_SquareSum9(A); - ss[1]=db_SquareSum9(A+9); - ss[2]=db_SquareSum9(A+18); - ss[3]=db_SquareSum9(A+27); - ss[4]=db_SquareSum9(A+36); - ss[5]=db_SquareSum9(A+45); - ss[6]=db_SquareSum9(A+54); - ss[7]=db_SquareSum9(A+63); - - ss[1]-=db_OrthogonalizePair9(A+9 ,A,ss[0]); - ss[2]-=db_OrthogonalizePair9(A+18,A,ss[0]); - ss[3]-=db_OrthogonalizePair9(A+27,A,ss[0]); - ss[4]-=db_OrthogonalizePair9(A+36,A,ss[0]); - ss[5]-=db_OrthogonalizePair9(A+45,A,ss[0]); - ss[6]-=db_OrthogonalizePair9(A+54,A,ss[0]); - ss[7]-=db_OrthogonalizePair9(A+63,A,ss[0]); - - /*Pivot on largest ss (could also be done on ss/(original_ss))*/ - i=db_MaxIndex7(ss+1); - db_OrthogonalizationSwap9(A+9,i,ss+1); - - ss[2]-=db_OrthogonalizePair9(A+18,A+9,ss[1]); - ss[3]-=db_OrthogonalizePair9(A+27,A+9,ss[1]); - ss[4]-=db_OrthogonalizePair9(A+36,A+9,ss[1]); - ss[5]-=db_OrthogonalizePair9(A+45,A+9,ss[1]); - ss[6]-=db_OrthogonalizePair9(A+54,A+9,ss[1]); - ss[7]-=db_OrthogonalizePair9(A+63,A+9,ss[1]); - - i=db_MaxIndex6(ss+2); - db_OrthogonalizationSwap9(A+18,i,ss+2); - - ss[3]-=db_OrthogonalizePair9(A+27,A+18,ss[2]); - ss[4]-=db_OrthogonalizePair9(A+36,A+18,ss[2]); - ss[5]-=db_OrthogonalizePair9(A+45,A+18,ss[2]); - ss[6]-=db_OrthogonalizePair9(A+54,A+18,ss[2]); - ss[7]-=db_OrthogonalizePair9(A+63,A+18,ss[2]); - - i=db_MaxIndex5(ss+3); - db_OrthogonalizationSwap9(A+27,i,ss+3); - - ss[4]-=db_OrthogonalizePair9(A+36,A+27,ss[3]); - ss[5]-=db_OrthogonalizePair9(A+45,A+27,ss[3]); - ss[6]-=db_OrthogonalizePair9(A+54,A+27,ss[3]); - ss[7]-=db_OrthogonalizePair9(A+63,A+27,ss[3]); - - i=db_MaxIndex4(ss+4); - db_OrthogonalizationSwap9(A+36,i,ss+4); - - ss[5]-=db_OrthogonalizePair9(A+45,A+36,ss[4]); - ss[6]-=db_OrthogonalizePair9(A+54,A+36,ss[4]); - ss[7]-=db_OrthogonalizePair9(A+63,A+36,ss[4]); - - i=db_MaxIndex3(ss+5); - db_OrthogonalizationSwap9(A+45,i,ss+5); - - ss[6]-=db_OrthogonalizePair9(A+54,A+45,ss[5]); - ss[7]-=db_OrthogonalizePair9(A+63,A+45,ss[5]); - - i=db_MaxIndex2(ss+6); - db_OrthogonalizationSwap9(A+54,i,ss+6); - - ss[7]-=db_OrthogonalizePair9(A+63,A+54,ss[6]); - - if(orthonormalize) - { - db_MultiplyScalar9(A ,db_SafeSqrtReciprocal(ss[0])); - db_MultiplyScalar9(A+9 ,db_SafeSqrtReciprocal(ss[1])); - db_MultiplyScalar9(A+18,db_SafeSqrtReciprocal(ss[2])); - db_MultiplyScalar9(A+27,db_SafeSqrtReciprocal(ss[3])); - db_MultiplyScalar9(A+36,db_SafeSqrtReciprocal(ss[4])); - db_MultiplyScalar9(A+45,db_SafeSqrtReciprocal(ss[5])); - db_MultiplyScalar9(A+54,db_SafeSqrtReciprocal(ss[6])); - db_MultiplyScalar9(A+63,db_SafeSqrtReciprocal(ss[7])); - } -} - -void db_NullVectorOrthonormal6x7(double x[7],const double A[42]) -{ - int i; - double omss[7]; - const double *B; - - /*Pivot by choosing row of the identity matrix - (the one corresponding to column of A with smallest square sum)*/ - omss[0]=db_SquareSum6Stride7(A); - omss[1]=db_SquareSum6Stride7(A+1); - omss[2]=db_SquareSum6Stride7(A+2); - omss[3]=db_SquareSum6Stride7(A+3); - omss[4]=db_SquareSum6Stride7(A+4); - omss[5]=db_SquareSum6Stride7(A+5); - omss[6]=db_SquareSum6Stride7(A+6); - i=db_MinIndex7(omss); - /*orthogonalize that row against all previous rows - and normalize it*/ - B=A+i; - db_MultiplyScalarCopy7(x,A,-B[0]); - db_RowOperation7(x,A+7 ,B[7]); - db_RowOperation7(x,A+14,B[14]); - db_RowOperation7(x,A+21,B[21]); - db_RowOperation7(x,A+28,B[28]); - db_RowOperation7(x,A+35,B[35]); - x[i]+=1.0; - db_MultiplyScalar7(x,db_SafeSqrtReciprocal(1.0-omss[i])); -} - -void db_NullVectorOrthonormal8x9(double x[9],const double A[72]) -{ - int i; - double omss[9]; - const double *B; - - /*Pivot by choosing row of the identity matrix - (the one corresponding to column of A with smallest square sum)*/ - omss[0]=db_SquareSum8Stride9(A); - omss[1]=db_SquareSum8Stride9(A+1); - omss[2]=db_SquareSum8Stride9(A+2); - omss[3]=db_SquareSum8Stride9(A+3); - omss[4]=db_SquareSum8Stride9(A+4); - omss[5]=db_SquareSum8Stride9(A+5); - omss[6]=db_SquareSum8Stride9(A+6); - omss[7]=db_SquareSum8Stride9(A+7); - omss[8]=db_SquareSum8Stride9(A+8); - i=db_MinIndex9(omss); - /*orthogonalize that row against all previous rows - and normalize it*/ - B=A+i; - db_MultiplyScalarCopy9(x,A,-B[0]); - db_RowOperation9(x,A+9 ,B[9]); - db_RowOperation9(x,A+18,B[18]); - db_RowOperation9(x,A+27,B[27]); - db_RowOperation9(x,A+36,B[36]); - db_RowOperation9(x,A+45,B[45]); - db_RowOperation9(x,A+54,B[54]); - db_RowOperation9(x,A+63,B[63]); - x[i]+=1.0; - db_MultiplyScalar9(x,db_SafeSqrtReciprocal(1.0-omss[i])); -} - diff --git a/jni/feature_stab/db_vlvm/db_utilities_linalg.h b/jni/feature_stab/db_vlvm/db_utilities_linalg.h deleted file mode 100644 index 1f63d4e57..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities_linalg.h +++ /dev/null @@ -1,802 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities_linalg.h,v 1.5 2011/06/17 14:03:31 mbansal Exp $ */ - -#ifndef DB_UTILITIES_LINALG -#define DB_UTILITIES_LINALG - -#include "db_utilities.h" - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ -/*! - * \defgroup LMLinAlg (LM) Linear Algebra Utilities (QR factorization, orthogonal basis, etc.) - */ - -/*! - \ingroup LMBasicUtilities - */ -inline void db_MultiplyScalar6(double A[6],double mult) -{ - (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; - (*A++) *= mult; -} -/*! - \ingroup LMBasicUtilities - */ -inline void db_MultiplyScalar7(double A[7],double mult) -{ - (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; - (*A++) *= mult; (*A++) *= mult; -} -/*! - \ingroup LMBasicUtilities - */ -inline void db_MultiplyScalar9(double A[9],double mult) -{ - (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; - (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; -} - -/*! - \ingroup LMBasicUtilities - */ -inline double db_SquareSum6Stride7(const double *x) -{ - return(db_sqr(x[0])+db_sqr(x[7])+db_sqr(x[14])+ - db_sqr(x[21])+db_sqr(x[28])+db_sqr(x[35])); -} - -/*! - \ingroup LMBasicUtilities - */ -inline double db_SquareSum8Stride9(const double *x) -{ - return(db_sqr(x[0])+db_sqr(x[9])+db_sqr(x[18])+ - db_sqr(x[27])+db_sqr(x[36])+db_sqr(x[45])+ - db_sqr(x[54])+db_sqr(x[63])); -} - -/*! - \ingroup LMLinAlg - Cholesky-factorize symmetric positive definite 6 x 6 matrix A. Upper -part of A is used from the input. The Cholesky factor is output as -subdiagonal part in A and diagonal in d, which is 6-dimensional -1.9 microseconds on 450MHz*/ -DB_API void db_CholeskyDecomp6x6(double A[36],double d[6]); - -/*! - \ingroup LMLinAlg - Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition -of a 6 x 6 matrix and the right hand side b. The vector b is unchanged -1.3 microseconds on 450MHz*/ -DB_API void db_CholeskyBacksub6x6(double x[6],const double A[36],const double d[6],const double b[6]); - -/*! - \ingroup LMLinAlg - Cholesky-factorize symmetric positive definite n x n matrix A.Part -above diagonal of A is used from the input, diagonal of A is assumed to -be stored in d. The Cholesky factor is output as -subdiagonal part in A and diagonal in d, which is n-dimensional*/ -DB_API void db_CholeskyDecompSeparateDiagonal(double **A,double *d,int n); - -/*! - \ingroup LMLinAlg - Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition -of an n x n matrix and the right hand side b. The vector b is unchanged*/ -DB_API void db_CholeskyBacksub(double *x,const double * const *A,const double *d,int n,const double *b); - -/*! - \ingroup LMLinAlg - Cholesky-factorize symmetric positive definite 3 x 3 matrix A. Part -above diagonal of A is used from the input, diagonal of A is assumed to -be stored in d. The Cholesky factor is output as subdiagonal part in A -and diagonal in d, which is 3-dimensional*/ -DB_API void db_CholeskyDecomp3x3SeparateDiagonal(double A[9],double d[3]); - -/*! - \ingroup LMLinAlg - Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition -of a 3 x 3 matrix and the right hand side b. The vector b is unchanged*/ -DB_API void db_CholeskyBacksub3x3(double x[3],const double A[9],const double d[3],const double b[3]); - -/*! - \ingroup LMLinAlg - perform A-=B*mult*/ -inline void db_RowOperation3(double A[3],const double B[3],double mult) -{ - *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); -} - -/*! - \ingroup LMLinAlg - */ -inline void db_RowOperation7(double A[7],const double B[7],double mult) -{ - *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); - *A++ -= mult*(*B++); *A++ -= mult*(*B++); -} - -/*! - \ingroup LMLinAlg - */ -inline void db_RowOperation9(double A[9],const double B[9],double mult) -{ - *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); - *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); -} - -/*! - \ingroup LMBasicUtilities - Swap values of A[7] and B[7] - */ -inline void db_Swap7(double A[7],double B[7]) -{ - double temp; - temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; - temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; - temp= *A; *A++ = *B; *B++ =temp; -} - -/*! - \ingroup LMBasicUtilities - Swap values of A[9] and B[9] - */ -inline void db_Swap9(double A[9],double B[9]) -{ - double temp; - temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; - temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; - temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; -} - - -/*! - \ingroup LMLinAlg - */ -DB_API void db_Orthogonalize6x7(double A[42],int orthonormalize=0); - -/*! - \ingroup LMLinAlg - */ -DB_API void db_Orthogonalize8x9(double A[72],int orthonormalize=0); - -/*! - \ingroup LMLinAlg - */ -inline double db_OrthogonalizePair7(double *x,const double *v,double ssv) -{ - double m,sp,sp_m; - - m=db_SafeReciprocal(ssv); - sp=db_ScalarProduct7(x,v); - sp_m=sp*m; - db_RowOperation7(x,v,sp_m); - return(sp*sp_m); -} - -/*! - \ingroup LMLinAlg - */ -inline double db_OrthogonalizePair9(double *x,const double *v,double ssv) -{ - double m,sp,sp_m; - - m=db_SafeReciprocal(ssv); - sp=db_ScalarProduct9(x,v); - sp_m=sp*m; - db_RowOperation9(x,v,sp_m); - return(sp*sp_m); -} - -/*! - \ingroup LMLinAlg - */ -inline void db_OrthogonalizationSwap7(double *A,int i,double *ss) -{ - double temp; - - db_Swap7(A,A+7*i); - temp=ss[0]; ss[0]=ss[i]; ss[i]=temp; -} -/*! - \ingroup LMLinAlg - */ -inline void db_OrthogonalizationSwap9(double *A,int i,double *ss) -{ - double temp; - - db_Swap9(A,A+9*i); - temp=ss[0]; ss[0]=ss[i]; ss[i]=temp; -} - -/*! - \ingroup LMLinAlg - */ -DB_API void db_NullVectorOrthonormal6x7(double x[7],const double A[42]); -/*! - \ingroup LMLinAlg - */ -DB_API void db_NullVectorOrthonormal8x9(double x[9],const double A[72]); - -/*! - \ingroup LMLinAlg - */ -inline void db_NullVector6x7Destructive(double x[7],double A[42]) -{ - db_Orthogonalize6x7(A,1); - db_NullVectorOrthonormal6x7(x,A); -} - -/*! - \ingroup LMLinAlg - */ -inline void db_NullVector8x9Destructive(double x[9],double A[72]) -{ - db_Orthogonalize8x9(A,1); - db_NullVectorOrthonormal8x9(x,A); -} - -inline int db_ScalarProduct512_s(const short *f,const short *g) -{ -#ifndef DB_USE_MMX - int back; - back=0; - for(int i=1; i<=512; i++) - back+=(*f++)*(*g++); - - return(back); -#endif -} - - -inline int db_ScalarProduct32_s(const short *f,const short *g) -{ -#ifndef DB_USE_MMX - int back; - back=0; - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); - - return(back); -#endif -} - -/*! - \ingroup LMLinAlg - Scalar product of 128-vectors (short) - Compile-time control: MMX, SSE2 or standard C - */ -inline int db_ScalarProduct128_s(const short *f,const short *g) -{ -#ifndef DB_USE_MMX - int back; - back=0; - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - - return(back); -#else -#ifdef DB_USE_SSE2 - int back; - - _asm - { - mov eax,f - mov ecx,g - /*First iteration************************************/ - movdqa xmm0,[eax] - pxor xmm7,xmm7 /*Set xmm7 to zero*/ - pmaddwd xmm0,[ecx] - /*Stall*/ - /*Standard iteration************************************/ - movdqa xmm2,[eax+16] - paddd xmm7,xmm0 - pmaddwd xmm2,[ecx+16] - /*Stall*/ - movdqa xmm1,[eax+32] - paddd xmm7,xmm2 - pmaddwd xmm1,[ecx+32] - /*Stall*/ - /*Standard iteration************************************/ - movdqa xmm0,[eax+48] - paddd xmm7,xmm1 - pmaddwd xmm0,[ecx+48] - /*Stall*/ - /*Standard iteration************************************/ - movdqa xmm2,[eax+64] - paddd xmm7,xmm0 - pmaddwd xmm2,[ecx+64] - /*Stall*/ - movdqa xmm1,[eax+80] - paddd xmm7,xmm2 - pmaddwd xmm1,[ecx+80] - /*Stall*/ - /*Standard iteration************************************/ - movdqa xmm0,[eax+96] - paddd xmm7,xmm1 - pmaddwd xmm0,[ecx+96] - /*Stall*/ - /*Standard iteration************************************/ - movdqa xmm2,[eax+112] - paddd xmm7,xmm0 - pmaddwd xmm2,[ecx+112] - /*Stall*/ - movdqa xmm1,[eax+128] - paddd xmm7,xmm2 - pmaddwd xmm1,[ecx+128] - /*Stall*/ - /*Standard iteration************************************/ - movdqa xmm0,[eax+144] - paddd xmm7,xmm1 - pmaddwd xmm0,[ecx+144] - /*Stall*/ - /*Standard iteration************************************/ - movdqa xmm2,[eax+160] - paddd xmm7,xmm0 - pmaddwd xmm2,[ecx+160] - /*Stall*/ - movdqa xmm1,[eax+176] - paddd xmm7,xmm2 - pmaddwd xmm1,[ecx+176] - /*Stall*/ - /*Standard iteration************************************/ - movdqa xmm0,[eax+192] - paddd xmm7,xmm1 - pmaddwd xmm0,[ecx+192] - /*Stall*/ - /*Standard iteration************************************/ - movdqa xmm2,[eax+208] - paddd xmm7,xmm0 - pmaddwd xmm2,[ecx+208] - /*Stall*/ - movdqa xmm1,[eax+224] - paddd xmm7,xmm2 - pmaddwd xmm1,[ecx+224] - /*Stall*/ - /*Standard iteration************************************/ - movdqa xmm0,[eax+240] - paddd xmm7,xmm1 - pmaddwd xmm0,[ecx+240] - /*Stall*/ - /*Rest iteration************************************/ - paddd xmm7,xmm0 - - /* add up the sum squares */ - movhlps xmm0,xmm7 /* high half to low half */ - paddd xmm7,xmm0 /* add high to low */ - pshuflw xmm0,xmm7, 0xE /* reshuffle */ - paddd xmm7,xmm0 /* add remaining */ - movd back,xmm7 - - emms - } - - return(back); -#else - int back; - - _asm - { - mov eax,f - mov ecx,g - /*First iteration************************************/ - movq mm0,[eax] - pxor mm7,mm7 /*Set mm7 to zero*/ - pmaddwd mm0,[ecx] - /*Stall*/ - movq mm1,[eax+8] - /*Stall*/ - pmaddwd mm1,[ecx+8] - /*Stall*/ - /*Standard iteration************************************/ - movq mm2,[eax+16] - paddd mm7,mm0 - pmaddwd mm2,[ecx+16] - /*Stall*/ - movq mm0,[eax+24] - paddd mm7,mm1 - pmaddwd mm0,[ecx+24] - /*Stall*/ - movq mm1,[eax+32] - paddd mm7,mm2 - pmaddwd mm1,[ecx+32] - /*Stall*/ - /*Standard iteration************************************/ - movq mm2,[eax+40] - paddd mm7,mm0 - pmaddwd mm2,[ecx+40] - /*Stall*/ - movq mm0,[eax+48] - paddd mm7,mm1 - pmaddwd mm0,[ecx+48] - /*Stall*/ - movq mm1,[eax+56] - paddd mm7,mm2 - pmaddwd mm1,[ecx+56] - /*Stall*/ - /*Standard iteration************************************/ - movq mm2,[eax+64] - paddd mm7,mm0 - pmaddwd mm2,[ecx+64] - /*Stall*/ - movq mm0,[eax+72] - paddd mm7,mm1 - pmaddwd mm0,[ecx+72] - /*Stall*/ - movq mm1,[eax+80] - paddd mm7,mm2 - pmaddwd mm1,[ecx+80] - /*Stall*/ - /*Standard iteration************************************/ - movq mm2,[eax+88] - paddd mm7,mm0 - pmaddwd mm2,[ecx+88] - /*Stall*/ - movq mm0,[eax+96] - paddd mm7,mm1 - pmaddwd mm0,[ecx+96] - /*Stall*/ - movq mm1,[eax+104] - paddd mm7,mm2 - pmaddwd mm1,[ecx+104] - /*Stall*/ - /*Standard iteration************************************/ - movq mm2,[eax+112] - paddd mm7,mm0 - pmaddwd mm2,[ecx+112] - /*Stall*/ - movq mm0,[eax+120] - paddd mm7,mm1 - pmaddwd mm0,[ecx+120] - /*Stall*/ - movq mm1,[eax+128] - paddd mm7,mm2 - pmaddwd mm1,[ecx+128] - /*Stall*/ - /*Standard iteration************************************/ - movq mm2,[eax+136] - paddd mm7,mm0 - pmaddwd mm2,[ecx+136] - /*Stall*/ - movq mm0,[eax+144] - paddd mm7,mm1 - pmaddwd mm0,[ecx+144] - /*Stall*/ - movq mm1,[eax+152] - paddd mm7,mm2 - pmaddwd mm1,[ecx+152] - /*Stall*/ - /*Standard iteration************************************/ - movq mm2,[eax+160] - paddd mm7,mm0 - pmaddwd mm2,[ecx+160] - /*Stall*/ - movq mm0,[eax+168] - paddd mm7,mm1 - pmaddwd mm0,[ecx+168] - /*Stall*/ - movq mm1,[eax+176] - paddd mm7,mm2 - pmaddwd mm1,[ecx+176] - /*Stall*/ - /*Standard iteration************************************/ - movq mm2,[eax+184] - paddd mm7,mm0 - pmaddwd mm2,[ecx+184] - /*Stall*/ - movq mm0,[eax+192] - paddd mm7,mm1 - pmaddwd mm0,[ecx+192] - /*Stall*/ - movq mm1,[eax+200] - paddd mm7,mm2 - pmaddwd mm1,[ecx+200] - /*Stall*/ - /*Standard iteration************************************/ - movq mm2,[eax+208] - paddd mm7,mm0 - pmaddwd mm2,[ecx+208] - /*Stall*/ - movq mm0,[eax+216] - paddd mm7,mm1 - pmaddwd mm0,[ecx+216] - /*Stall*/ - movq mm1,[eax+224] - paddd mm7,mm2 - pmaddwd mm1,[ecx+224] - /*Stall*/ - /*Standard iteration************************************/ - movq mm2,[eax+232] - paddd mm7,mm0 - pmaddwd mm2,[ecx+232] - /*Stall*/ - movq mm0,[eax+240] - paddd mm7,mm1 - pmaddwd mm0,[ecx+240] - /*Stall*/ - movq mm1,[eax+248] - paddd mm7,mm2 - pmaddwd mm1,[ecx+248] - /*Stall*/ - /*Rest iteration************************************/ - paddd mm7,mm0 - /*Stall*/ - /*Stall*/ - /*Stall*/ - paddd mm7,mm1 - /*Stall*/ - movq mm0,mm7 - psrlq mm7,32 - paddd mm0,mm7 - /*Stall*/ - /*Stall*/ - /*Stall*/ - movd back,mm0 - emms - } - - return(back); -#endif -#endif /*DB_USE_MMX*/ -} - -/*! - \ingroup LMLinAlg - Scalar product of 16 byte aligned 128-vectors (float). - Compile-time control: SIMD (SSE) or standard C. - */ -inline float db_ScalarProduct128Aligned16_f(const float *f,const float *g) -{ -#ifndef DB_USE_SIMD - float back; - back=0.0; - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); - - return(back); -#else - float back; - - _asm - { - mov eax,f - mov ecx,g - /*First iteration************************************/ - movaps xmm0,[eax] - xorps xmm7,xmm7 /*Set mm7 to zero*/ - mulps xmm0,[ecx] - /*Stall*/ - movaps xmm1,[eax+16] - /*Stall*/ - mulps xmm1,[ecx+16] - /*Stall*/ - /*Standard iteration************************************/ - movaps xmm2,[eax+32] - addps xmm7,xmm0 - mulps xmm2,[ecx+32] - /*Stall*/ - movaps xmm0,[eax+48] - addps xmm7,xmm1 - mulps xmm0,[ecx+48] - /*Stall*/ - movaps xmm1,[eax+64] - addps xmm7,xmm2 - mulps xmm1,[ecx+64] - /*Stall*/ - /*Standard iteration************************************/ - movaps xmm2,[eax+80] - addps xmm7,xmm0 - mulps xmm2,[ecx+80] - /*Stall*/ - movaps xmm0,[eax+96] - addps xmm7,xmm1 - mulps xmm0,[ecx+96] - /*Stall*/ - movaps xmm1,[eax+112] - addps xmm7,xmm2 - mulps xmm1,[ecx+112] - /*Stall*/ - /*Standard iteration************************************/ - movaps xmm2,[eax+128] - addps xmm7,xmm0 - mulps xmm2,[ecx+128] - /*Stall*/ - movaps xmm0,[eax+144] - addps xmm7,xmm1 - mulps xmm0,[ecx+144] - /*Stall*/ - movaps xmm1,[eax+160] - addps xmm7,xmm2 - mulps xmm1,[ecx+160] - /*Stall*/ - /*Standard iteration************************************/ - movaps xmm2,[eax+176] - addps xmm7,xmm0 - mulps xmm2,[ecx+176] - /*Stall*/ - movaps xmm0,[eax+192] - addps xmm7,xmm1 - mulps xmm0,[ecx+192] - /*Stall*/ - movaps xmm1,[eax+208] - addps xmm7,xmm2 - mulps xmm1,[ecx+208] - /*Stall*/ - /*Standard iteration************************************/ - movaps xmm2,[eax+224] - addps xmm7,xmm0 - mulps xmm2,[ecx+224] - /*Stall*/ - movaps xmm0,[eax+240] - addps xmm7,xmm1 - mulps xmm0,[ecx+240] - /*Stall*/ - movaps xmm1,[eax+256] - addps xmm7,xmm2 - mulps xmm1,[ecx+256] - /*Stall*/ - /*Standard iteration************************************/ - movaps xmm2,[eax+272] - addps xmm7,xmm0 - mulps xmm2,[ecx+272] - /*Stall*/ - movaps xmm0,[eax+288] - addps xmm7,xmm1 - mulps xmm0,[ecx+288] - /*Stall*/ - movaps xmm1,[eax+304] - addps xmm7,xmm2 - mulps xmm1,[ecx+304] - /*Stall*/ - /*Standard iteration************************************/ - movaps xmm2,[eax+320] - addps xmm7,xmm0 - mulps xmm2,[ecx+320] - /*Stall*/ - movaps xmm0,[eax+336] - addps xmm7,xmm1 - mulps xmm0,[ecx+336] - /*Stall*/ - movaps xmm1,[eax+352] - addps xmm7,xmm2 - mulps xmm1,[ecx+352] - /*Stall*/ - /*Standard iteration************************************/ - movaps xmm2,[eax+368] - addps xmm7,xmm0 - mulps xmm2,[ecx+368] - /*Stall*/ - movaps xmm0,[eax+384] - addps xmm7,xmm1 - mulps xmm0,[ecx+384] - /*Stall*/ - movaps xmm1,[eax+400] - addps xmm7,xmm2 - mulps xmm1,[ecx+400] - /*Stall*/ - /*Standard iteration************************************/ - movaps xmm2,[eax+416] - addps xmm7,xmm0 - mulps xmm2,[ecx+416] - /*Stall*/ - movaps xmm0,[eax+432] - addps xmm7,xmm1 - mulps xmm0,[ecx+432] - /*Stall*/ - movaps xmm1,[eax+448] - addps xmm7,xmm2 - mulps xmm1,[ecx+448] - /*Stall*/ - /*Standard iteration************************************/ - movaps xmm2,[eax+464] - addps xmm7,xmm0 - mulps xmm2,[ecx+464] - /*Stall*/ - movaps xmm0,[eax+480] - addps xmm7,xmm1 - mulps xmm0,[ecx+480] - /*Stall*/ - movaps xmm1,[eax+496] - addps xmm7,xmm2 - mulps xmm1,[ecx+496] - /*Stall*/ - /*Rest iteration************************************/ - addps xmm7,xmm0 - /*Stall*/ - addps xmm7,xmm1 - /*Stall*/ - movaps xmm6,xmm7 - /*Stall*/ - shufps xmm6,xmm6,4Eh - /*Stall*/ - addps xmm7,xmm6 - /*Stall*/ - movaps xmm6,xmm7 - /*Stall*/ - shufps xmm6,xmm6,11h - /*Stall*/ - addps xmm7,xmm6 - /*Stall*/ - movss back,xmm7 - } - - return(back); -#endif /*DB_USE_SIMD*/ -} - -#endif /* DB_UTILITIES_LINALG */ diff --git a/jni/feature_stab/db_vlvm/db_utilities_poly.cpp b/jni/feature_stab/db_vlvm/db_utilities_poly.cpp deleted file mode 100644 index 013ac726e..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities_poly.cpp +++ /dev/null @@ -1,235 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities_poly.cpp,v 1.2 2010/09/03 12:00:10 bsouthall Exp $ */ - -#include "db_utilities_poly.h" -#include "db_utilities.h" - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ - -void db_SolveCubic(double *roots,int *nr_roots,double a,double b,double c,double d) -{ - double bp,bp2,cp,dp,q,r,srq; - double r2_min_q3,theta,bp_through3,theta_through3; - double cos_theta_through3,sin_theta_through3,min2_cos_theta_plu,min2_cos_theta_min; - double si_r_srq,A; - - /*For nondegenerate cubics with three roots - [24 mult 9 add 2sqrt 1acos 1cos=33flops 4func] - For nondegenerate cubics with one root - [16 mult 6 add 1sqrt 1qbrt=24flops 3func]*/ - - if(a==0.0) db_SolveQuadratic(roots,nr_roots,b,c,d); - else - { - bp=b/a; - bp2=bp*bp; - cp=c/a; - dp=d/a; - - q=(bp2-3.0*cp)/9.0; - r=(2.0*bp2*bp-9.0*bp*cp+27.0*dp)/54.0; - r2_min_q3=r*r-q*q*q; - if(r2_min_q3<0.0) - { - *nr_roots=3; - /*q has to be > 0*/ - srq=sqrt(q); - theta=acos(db_maxd(-1.0,db_mind(1.0,r/(q*srq)))); - bp_through3=bp/3.0; - theta_through3=theta/3.0; - cos_theta_through3=cos(theta_through3); - sin_theta_through3=sqrt(db_maxd(0.0,1.0-cos_theta_through3*cos_theta_through3)); - - /*cos(theta_through3+2*pi/3)=cos_theta_through3*cos(2*pi/3)-sin_theta_through3*sin(2*pi/3) - = -0.5*cos_theta_through3-sqrt(3)/2.0*sin_theta_through3 - = -0.5*(cos_theta_through3+sqrt(3)*sin_theta_through3)*/ - min2_cos_theta_plu=cos_theta_through3+DB_SQRT3*sin_theta_through3; - min2_cos_theta_min=cos_theta_through3-DB_SQRT3*sin_theta_through3; - - roots[0]= -2.0*srq*cos_theta_through3-bp_through3; - roots[1]=srq*min2_cos_theta_plu-bp_through3; - roots[2]=srq*min2_cos_theta_min-bp_through3; - } - else if(r2_min_q3>0.0) - { - *nr_roots=1; - A= -db_sign(r)*db_CubRoot(db_absd(r)+sqrt(r2_min_q3)); - bp_through3=bp/3.0; - if(A!=0.0) roots[0]=A+q/A-bp_through3; - else roots[0]= -bp_through3; - } - else - { - *nr_roots=2; - bp_through3=bp/3.0; - /*q has to be >= 0*/ - si_r_srq=db_sign(r)*sqrt(q); - /*Single root*/ - roots[0]= -2.0*si_r_srq-bp_through3; - /*Double root*/ - roots[1]=si_r_srq-bp_through3; - } - } -} - -void db_SolveQuartic(double *roots,int *nr_roots,double a,double b,double c,double d,double e) -{ - /*Normalized coefficients*/ - double c0,c1,c2,c3; - /*Temporary coefficients*/ - double c3through2,c3through4,c3c3through4_min_c2,min4_c0; - double lz,ms,ns,mn,m,n,lz_through2; - /*Cubic polynomial roots, nr of roots and coefficients*/ - double c_roots[3]; - int nr_c_roots; - double k0,k1; - /*nr additional roots from second quadratic*/ - int addroots; - - /*For nondegenerate quartics - [16mult 11add 2sqrt 1cubic 2quadratic=74flops 8funcs]*/ - - if(a==0.0) db_SolveCubic(roots,nr_roots,b,c,d,e); - else if(e==0.0) - { - db_SolveCubic(roots,nr_roots,a,b,c,d); - roots[*nr_roots]=0.0; - *nr_roots+=1; - } - else - { - /*Compute normalized coefficients*/ - c3=b/a; - c2=c/a; - c1=d/a; - c0=e/a; - /*Compute temporary coefficients*/ - c3through2=c3/2.0; - c3through4=c3/4.0; - c3c3through4_min_c2=c3*c3through4-c2; - min4_c0= -4.0*c0; - /*Compute coefficients of cubic*/ - k0=min4_c0*c3c3through4_min_c2-c1*c1; - k1=c1*c3+min4_c0; - /*k2= -c2*/ - /*k3=1.0*/ - - /*Solve it for roots*/ - db_SolveCubic(c_roots,&nr_c_roots,1.0,-c2,k1,k0); - - if(nr_c_roots>0) - { - lz=c_roots[0]; - lz_through2=lz/2.0; - ms=lz+c3c3through4_min_c2; - ns=lz_through2*lz_through2-c0; - mn=lz*c3through4-c1/2.0; - - if((ms>=0.0)&&(ns>=0.0)) - { - m=sqrt(ms); - n=sqrt(ns)*db_sign(mn); - - db_SolveQuadratic(roots,nr_roots, - 1.0,c3through2+m,lz_through2+n); - - db_SolveQuadratic(&roots[*nr_roots],&addroots, - 1.0,c3through2-m,lz_through2-n); - - *nr_roots+=addroots; - } - else *nr_roots=0; - } - else *nr_roots=0; - } -} - -void db_SolveQuarticForced(double *roots,int *nr_roots,double a,double b,double c,double d,double e) -{ - /*Normalized coefficients*/ - double c0,c1,c2,c3; - /*Temporary coefficients*/ - double c3through2,c3through4,c3c3through4_min_c2,min4_c0; - double lz,ms,ns,mn,m,n,lz_through2; - /*Cubic polynomial roots, nr of roots and coefficients*/ - double c_roots[3]; - int nr_c_roots; - double k0,k1; - /*nr additional roots from second quadratic*/ - int addroots; - - /*For nondegenerate quartics - [16mult 11add 2sqrt 1cubic 2quadratic=74flops 8funcs]*/ - - if(a==0.0) db_SolveCubic(roots,nr_roots,b,c,d,e); - else if(e==0.0) - { - db_SolveCubic(roots,nr_roots,a,b,c,d); - roots[*nr_roots]=0.0; - *nr_roots+=1; - } - else - { - /*Compute normalized coefficients*/ - c3=b/a; - c2=c/a; - c1=d/a; - c0=e/a; - /*Compute temporary coefficients*/ - c3through2=c3/2.0; - c3through4=c3/4.0; - c3c3through4_min_c2=c3*c3through4-c2; - min4_c0= -4.0*c0; - /*Compute coefficients of cubic*/ - k0=min4_c0*c3c3through4_min_c2-c1*c1; - k1=c1*c3+min4_c0; - /*k2= -c2*/ - /*k3=1.0*/ - - /*Solve it for roots*/ - db_SolveCubic(c_roots,&nr_c_roots,1.0,-c2,k1,k0); - - if(nr_c_roots>0) - { - lz=c_roots[0]; - lz_through2=lz/2.0; - ms=lz+c3c3through4_min_c2; - ns=lz_through2*lz_through2-c0; - mn=lz*c3through4-c1/2.0; - - if(ms<0.0) ms=0.0; - if(ns<0.0) ns=0.0; - - m=sqrt(ms); - n=sqrt(ns)*db_sign(mn); - - db_SolveQuadratic(roots,nr_roots, - 1.0,c3through2+m,lz_through2+n); - - db_SolveQuadratic(&roots[*nr_roots],&addroots, - 1.0,c3through2-m,lz_through2-n); - - *nr_roots+=addroots; - } - else *nr_roots=0; - } -} diff --git a/jni/feature_stab/db_vlvm/db_utilities_poly.h b/jni/feature_stab/db_vlvm/db_utilities_poly.h deleted file mode 100644 index 1f8789077..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities_poly.h +++ /dev/null @@ -1,383 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities_poly.h,v 1.2 2010/09/03 12:00:11 bsouthall Exp $ */ - -#ifndef DB_UTILITIES_POLY -#define DB_UTILITIES_POLY - -#include "db_utilities.h" - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ -/*! - * \defgroup LMPolynomial (LM) Polynomial utilities (solvers, arithmetic, evaluation, etc.) - */ -/*\{*/ - -/*! -In debug mode closed form quadratic solving takes on the order of 15 microseconds -while eig of the companion matrix takes about 1.1 milliseconds -Speed-optimized code in release mode solves a quadratic in 0.3 microseconds on 450MHz -*/ -inline void db_SolveQuadratic(double *roots,int *nr_roots,double a,double b,double c) -{ - double rs,srs,q; - - /*For non-degenerate quadratics - [5 mult 2 add 1 sqrt=7flops 1func]*/ - if(a==0.0) - { - if(b==0.0) *nr_roots=0; - else - { - roots[0]= -c/b; - *nr_roots=1; - } - } - else - { - rs=b*b-4.0*a*c; - if(rs>=0.0) - { - *nr_roots=2; - srs=sqrt(rs); - q= -0.5*(b+db_sign(b)*srs); - roots[0]=q/a; - /*If b is zero db_sign(b) returns 1, - so q is only zero when b=0 and c=0*/ - if(q==0.0) *nr_roots=1; - else roots[1]=c/q; - } - else *nr_roots=0; - } -} - -/*! -In debug mode closed form cubic solving takes on the order of 45 microseconds -while eig of the companion matrix takes about 1.3 milliseconds -Speed-optimized code in release mode solves a cubic in 1.5 microseconds on 450MHz -For a non-degenerate cubic with two roots, the first root is the single root and -the second root is the double root -*/ -DB_API void db_SolveCubic(double *roots,int *nr_roots,double a,double b,double c,double d); -/*! -In debug mode closed form quartic solving takes on the order of 0.1 milliseconds -while eig of the companion matrix takes about 1.5 milliseconds -Speed-optimized code in release mode solves a quartic in 2.6 microseconds on 450MHz*/ -DB_API void db_SolveQuartic(double *roots,int *nr_roots,double a,double b,double c,double d,double e); -/*! -Quartic solving where a solution is forced when splitting into quadratics, which -can be good if the quartic is sometimes in fact a quadratic, such as in absolute orientation -when the data is planar*/ -DB_API void db_SolveQuarticForced(double *roots,int *nr_roots,double a,double b,double c,double d,double e); - -inline double db_PolyEval1(const double p[2],double x) -{ - return(p[0]+x*p[1]); -} - -inline void db_MultiplyPoly1_1(double *d,const double *a,const double *b) -{ - double a0,a1; - double b0,b1; - a0=a[0];a1=a[1]; - b0=b[0];b1=b[1]; - - d[0]=a0*b0; - d[1]=a0*b1+a1*b0; - d[2]= a1*b1; -} - -inline void db_MultiplyPoly0_2(double *d,const double *a,const double *b) -{ - double a0; - double b0,b1,b2; - a0=a[0]; - b0=b[0];b1=b[1];b2=b[2]; - - d[0]=a0*b0; - d[1]=a0*b1; - d[2]=a0*b2; -} - -inline void db_MultiplyPoly1_2(double *d,const double *a,const double *b) -{ - double a0,a1; - double b0,b1,b2; - a0=a[0];a1=a[1]; - b0=b[0];b1=b[1];b2=b[2]; - - d[0]=a0*b0; - d[1]=a0*b1+a1*b0; - d[2]=a0*b2+a1*b1; - d[3]= a1*b2; -} - - -inline void db_MultiplyPoly1_3(double *d,const double *a,const double *b) -{ - double a0,a1; - double b0,b1,b2,b3; - a0=a[0];a1=a[1]; - b0=b[0];b1=b[1];b2=b[2];b3=b[3]; - - d[0]=a0*b0; - d[1]=a0*b1+a1*b0; - d[2]=a0*b2+a1*b1; - d[3]=a0*b3+a1*b2; - d[4]= a1*b3; -} -/*! -Multiply d=a*b where a is one degree and b is two degree*/ -inline void db_AddPolyProduct0_1(double *d,const double *a,const double *b) -{ - double a0; - double b0,b1; - a0=a[0]; - b0=b[0];b1=b[1]; - - d[0]+=a0*b0; - d[1]+=a0*b1; -} -inline void db_AddPolyProduct0_2(double *d,const double *a,const double *b) -{ - double a0; - double b0,b1,b2; - a0=a[0]; - b0=b[0];b1=b[1];b2=b[2]; - - d[0]+=a0*b0; - d[1]+=a0*b1; - d[2]+=a0*b2; -} -/*! -Multiply d=a*b where a is one degree and b is two degree*/ -inline void db_SubtractPolyProduct0_0(double *d,const double *a,const double *b) -{ - double a0; - double b0; - a0=a[0]; - b0=b[0]; - - d[0]-=a0*b0; -} - -inline void db_SubtractPolyProduct0_1(double *d,const double *a,const double *b) -{ - double a0; - double b0,b1; - a0=a[0]; - b0=b[0];b1=b[1]; - - d[0]-=a0*b0; - d[1]-=a0*b1; -} - -inline void db_SubtractPolyProduct0_2(double *d,const double *a,const double *b) -{ - double a0; - double b0,b1,b2; - a0=a[0]; - b0=b[0];b1=b[1];b2=b[2]; - - d[0]-=a0*b0; - d[1]-=a0*b1; - d[2]-=a0*b2; -} - -inline void db_SubtractPolyProduct1_3(double *d,const double *a,const double *b) -{ - double a0,a1; - double b0,b1,b2,b3; - a0=a[0];a1=a[1]; - b0=b[0];b1=b[1];b2=b[2];b3=b[3]; - - d[0]-=a0*b0; - d[1]-=a0*b1+a1*b0; - d[2]-=a0*b2+a1*b1; - d[3]-=a0*b3+a1*b2; - d[4]-= a1*b3; -} - -inline void db_CharacteristicPolynomial4x4(double p[5],const double A[16]) -{ - /*All two by two determinants of the first two rows*/ - double two01[3],two02[3],two03[3],two12[3],two13[3],two23[3]; - /*Polynomials representing third and fourth row of A*/ - double P0[2],P1[2],P2[2],P3[2]; - double P4[2],P5[2],P6[2],P7[2]; - /*All three by three determinants of the first three rows*/ - double neg_three0[4],neg_three1[4],three2[4],three3[4]; - - /*Compute 2x2 determinants*/ - two01[0]=A[0]*A[5]-A[1]*A[4]; - two01[1]= -(A[0]+A[5]); - two01[2]=1.0; - - two02[0]=A[0]*A[6]-A[2]*A[4]; - two02[1]= -A[6]; - - two03[0]=A[0]*A[7]-A[3]*A[4]; - two03[1]= -A[7]; - - two12[0]=A[1]*A[6]-A[2]*A[5]; - two12[1]=A[2]; - - two13[0]=A[1]*A[7]-A[3]*A[5]; - two13[1]=A[3]; - - two23[0]=A[2]*A[7]-A[3]*A[6]; - - P0[0]=A[8]; - P1[0]=A[9]; - P2[0]=A[10];P2[1]= -1.0; - P3[0]=A[11]; - - P4[0]=A[12]; - P5[0]=A[13]; - P6[0]=A[14]; - P7[0]=A[15];P7[1]= -1.0; - - /*Compute 3x3 determinants.Note that the highest - degree polynomial goes first and the smaller ones - are added or subtracted from it*/ - db_MultiplyPoly1_1( neg_three0,P2,two13); - db_SubtractPolyProduct0_0(neg_three0,P1,two23); - db_SubtractPolyProduct0_1(neg_three0,P3,two12); - - db_MultiplyPoly1_1( neg_three1,P2,two03); - db_SubtractPolyProduct0_1(neg_three1,P3,two02); - db_SubtractPolyProduct0_0(neg_three1,P0,two23); - - db_MultiplyPoly0_2( three2,P3,two01); - db_AddPolyProduct0_1( three2,P0,two13); - db_SubtractPolyProduct0_1(three2,P1,two03); - - db_MultiplyPoly1_2( three3,P2,two01); - db_AddPolyProduct0_1( three3,P0,two12); - db_SubtractPolyProduct0_1(three3,P1,two02); - - /*Compute 4x4 determinants*/ - db_MultiplyPoly1_3( p,P7,three3); - db_AddPolyProduct0_2( p,P4,neg_three0); - db_SubtractPolyProduct0_2(p,P5,neg_three1); - db_SubtractPolyProduct0_2(p,P6,three2); -} - -inline void db_RealEigenvalues4x4(double lambda[4],int *nr_roots,const double A[16],int forced=0) -{ - double p[5]; - - db_CharacteristicPolynomial4x4(p,A); - if(forced) db_SolveQuarticForced(lambda,nr_roots,p[4],p[3],p[2],p[1],p[0]); - else db_SolveQuartic(lambda,nr_roots,p[4],p[3],p[2],p[1],p[0]); -} - -/*! -Compute the unit norm eigenvector v of the matrix A corresponding -to the eigenvalue lambda -[96mult 60add 1sqrt=156flops 1sqrt]*/ -inline void db_EigenVector4x4(double v[4],double lambda,const double A[16]) -{ - double a0,a5,a10,a15; - double d01,d02,d03,d12,d13,d23; - double e01,e02,e03,e12,e13,e23; - double C[16],n0,n1,n2,n3,m; - - /*Compute diagonal - [4add=4flops]*/ - a0=A[0]-lambda; - a5=A[5]-lambda; - a10=A[10]-lambda; - a15=A[15]-lambda; - - /*Compute 2x2 determinants of rows 1,2 and 3,4 - [24mult 12add=36flops]*/ - d01=a0*a5 -A[1]*A[4]; - d02=a0*A[6] -A[2]*A[4]; - d03=a0*A[7] -A[3]*A[4]; - d12=A[1]*A[6]-A[2]*a5; - d13=A[1]*A[7]-A[3]*a5; - d23=A[2]*A[7]-A[3]*A[6]; - - e01=A[8]*A[13]-A[9] *A[12]; - e02=A[8]*A[14]-a10 *A[12]; - e03=A[8]*a15 -A[11]*A[12]; - e12=A[9]*A[14]-a10 *A[13]; - e13=A[9]*a15 -A[11]*A[13]; - e23=a10 *a15 -A[11]*A[14]; - - /*Compute matrix of cofactors - [48mult 32 add=80flops*/ - C[0]= (a5 *e23-A[6]*e13+A[7]*e12); - C[1]= -(A[4]*e23-A[6]*e03+A[7]*e02); - C[2]= (A[4]*e13-a5 *e03+A[7]*e01); - C[3]= -(A[4]*e12-a5 *e02+A[6]*e01); - - C[4]= -(A[1]*e23-A[2]*e13+A[3]*e12); - C[5]= (a0 *e23-A[2]*e03+A[3]*e02); - C[6]= -(a0 *e13-A[1]*e03+A[3]*e01); - C[7]= (a0 *e12-A[1]*e02+A[2]*e01); - - C[8]= (A[13]*d23-A[14]*d13+a15 *d12); - C[9]= -(A[12]*d23-A[14]*d03+a15 *d02); - C[10]= (A[12]*d13-A[13]*d03+a15 *d01); - C[11]= -(A[12]*d12-A[13]*d02+A[14]*d01); - - C[12]= -(A[9]*d23-a10 *d13+A[11]*d12); - C[13]= (A[8]*d23-a10 *d03+A[11]*d02); - C[14]= -(A[8]*d13-A[9]*d03+A[11]*d01); - C[15]= (A[8]*d12-A[9]*d02+a10 *d01); - - /*Compute square sums of rows - [16mult 12add=28flops*/ - n0=db_sqr(C[0]) +db_sqr(C[1]) +db_sqr(C[2]) +db_sqr(C[3]); - n1=db_sqr(C[4]) +db_sqr(C[5]) +db_sqr(C[6]) +db_sqr(C[7]); - n2=db_sqr(C[8]) +db_sqr(C[9]) +db_sqr(C[10])+db_sqr(C[11]); - n3=db_sqr(C[12])+db_sqr(C[13])+db_sqr(C[14])+db_sqr(C[15]); - - /*Take the largest norm row and normalize - [4mult 1 sqrt=4flops 1sqrt]*/ - if(n0>=n1 && n0>=n2 && n0>=n3) - { - m=db_SafeReciprocal(sqrt(n0)); - db_MultiplyScalarCopy4(v,C,m); - } - else if(n1>=n2 && n1>=n3) - { - m=db_SafeReciprocal(sqrt(n1)); - db_MultiplyScalarCopy4(v,&(C[4]),m); - } - else if(n2>=n3) - { - m=db_SafeReciprocal(sqrt(n2)); - db_MultiplyScalarCopy4(v,&(C[8]),m); - } - else - { - m=db_SafeReciprocal(sqrt(n3)); - db_MultiplyScalarCopy4(v,&(C[12]),m); - } -} - - - -/*\}*/ -#endif /* DB_UTILITIES_POLY */ diff --git a/jni/feature_stab/db_vlvm/db_utilities_random.h b/jni/feature_stab/db_vlvm/db_utilities_random.h deleted file mode 100644 index ef24039c1..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities_random.h +++ /dev/null @@ -1,98 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities_random.h,v 1.1 2010/08/19 18:09:20 bsouthall Exp $ */ - -#ifndef DB_UTILITIES_RANDOM -#define DB_UTILITIES_RANDOM - -#include "db_utilities.h" - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ -/*! - * \defgroup LMRandom (LM) Random numbers, random sampling - */ -/*\{*/ -/*! - Random Number generator. Initialize with non-zero -integer value r. A double between zero and one is -returned. -\param r seed -\return random double -*/ -inline double db_QuickRandomDouble(int &r) -{ - int c; - c=r/127773; - r=16807*(r-c*127773)-2836*c; - if(r<0) r+=2147483647; - return((1.0/((double)2147483647))*r); - //return (((double)rand())/(double)RAND_MAX); -} - -/*! -Random Number generator. Initialize with non-zero -integer value r. An int between and including 0 and max - \param r seed - \param max upped limit - \return random int -*/ -inline int db_RandomInt(int &r,int max) -{ - double dtemp; - int itemp; - dtemp=db_QuickRandomDouble(r)*(max+1); - itemp=(int) dtemp; - if(itemp<=0) return(0); - if(itemp>=max) return(max); - return(itemp); -} - -/*! - Generate a random sample indexing into [0..pool_size-1]. - \param s sample (out) pre-allocated array of size sample_size - \param sample_size size of sample - \param pool_size upper limit on item index - \param r_seed random number generator seed - */ -inline void db_RandomSample(int *s,int sample_size,int pool_size,int &r_seed) -{ - int temp,temp2,i,j; - - for(i=0;i<sample_size;i++) - { - temp=db_RandomInt(r_seed,pool_size-1-i); - - for(j=0;j<i;j++) - { - if(s[j]<=temp) temp++; - else - { - /*swap*/ - temp2=temp; - temp=s[j]; - s[j]=temp2; - } - } - s[i]=temp; - } -} -/*\}*/ -#endif /* DB_UTILITIES_RANDOM */ diff --git a/jni/feature_stab/db_vlvm/db_utilities_rotation.h b/jni/feature_stab/db_vlvm/db_utilities_rotation.h deleted file mode 100644 index 7f5f937b4..000000000 --- a/jni/feature_stab/db_vlvm/db_utilities_rotation.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - * 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. - */ - -/* $Id: db_utilities_rotation.h,v 1.2 2010/09/03 12:00:11 bsouthall Exp $ */ - -#ifndef DB_UTILITIES_ROTATION -#define DB_UTILITIES_ROTATION - -#include "db_utilities.h" - - - -/***************************************************************** -* Lean and mean begins here * -*****************************************************************/ -/*! - * \defgroup LMRotation (LM) Rotation Utilities (quaternions, orthonormal) - */ -/*\{*/ -/*! - Takes a unit quaternion and gives its corresponding rotation matrix. - \param R rotation matrix (out) - \param q quaternion - */ -inline void db_QuaternionToRotation(double R[9],const double q[4]) -{ - double q0q0,q0qx,q0qy,q0qz,qxqx,qxqy,qxqz,qyqy,qyqz,qzqz; - - q0q0=q[0]*q[0]; - q0qx=q[0]*q[1]; - q0qy=q[0]*q[2]; - q0qz=q[0]*q[3]; - qxqx=q[1]*q[1]; - qxqy=q[1]*q[2]; - qxqz=q[1]*q[3]; - qyqy=q[2]*q[2]; - qyqz=q[2]*q[3]; - qzqz=q[3]*q[3]; - - R[0]=q0q0+qxqx-qyqy-qzqz; R[1]=2.0*(qxqy-q0qz); R[2]=2.0*(qxqz+q0qy); - R[3]=2.0*(qxqy+q0qz); R[4]=q0q0-qxqx+qyqy-qzqz; R[5]=2.0*(qyqz-q0qx); - R[6]=2.0*(qxqz-q0qy); R[7]=2.0*(qyqz+q0qx); R[8]=q0q0-qxqx-qyqy+qzqz; -} - -/*\}*/ -#endif /* DB_UTILITIES_ROTATION */ |