1 /*
2 * Copyright (C) 2011 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 /*$Id: db_feature_matching.cpp,v 1.4 2011/06/17 14:03:30 mbansal Exp $*/
18
19 /*****************************************************************
20 * Lean and mean begins here *
21 *****************************************************************/
22
23 #include "db_utilities.h"
24 #include "db_feature_matching.h"
25 #ifdef _VERBOSE_
26 #include <iostream>
27 #endif
28
29
30 int AffineWarpPoint_NN_LUT_x[11][11];
31 int AffineWarpPoint_NN_LUT_y[11][11];
32
33 float AffineWarpPoint_BL_LUT_x[11][11];
34 float AffineWarpPoint_BL_LUT_y[11][11];
35
36
db_SignedSquareNormCorr7x7_u(unsigned char ** f_img,unsigned char ** g_img,int x_f,int y_f,int x_g,int y_g)37 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)
38 {
39 unsigned char *pf,*pg;
40 float f,g,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den;
41 int xm_f,xm_g;
42
43 xm_f=x_f-3;
44 xm_g=x_g-3;
45 fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0;
46
47 pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g;
48 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
49 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
50 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
51 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
52 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
53 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
54 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
55
56 pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g;
57 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
58 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
59 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
60 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
61 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
62 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
63 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
64
65 pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g;
66 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
67 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
68 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
69 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
70 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
71 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
72 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
73
74 pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g;
75 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
76 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
77 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
78 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
79 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
80 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
81 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
82
83 pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g;
84 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
85 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
86 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
87 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
88 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
89 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
90 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
91
92 pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g;
93 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
94 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
95 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
96 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
97 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
98 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
99 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
100
101 pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g;
102 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
103 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
104 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
105 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
106 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
107 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
108 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
109
110 fg_corr=49.0f*fgsum-fsum*gsum;
111 den=(49.0f*f2sum-fsum*fsum)*(49.0f*g2sum-gsum*gsum);
112 if(den!=0.0)
113 {
114 if(fg_corr>=0.0) return(fg_corr*fg_corr/den);
115 return(-fg_corr*fg_corr/den);
116 }
117 return(0.0);
118 }
119
db_SignedSquareNormCorr9x9_u(unsigned char ** f_img,unsigned char ** g_img,int x_f,int y_f,int x_g,int y_g)120 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)
121 {
122 unsigned char *pf,*pg;
123 float f,g,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den;
124 int xm_f,xm_g;
125
126 xm_f=x_f-4;
127 xm_g=x_g-4;
128 fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0;
129
130 pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g;
131 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
132 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
133 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
134 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
135 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
136 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
137 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
138 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
139 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
140
141 pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g;
142 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
143 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
144 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
145 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
146 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
147 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
148 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
149 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
150 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
151
152 pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g;
153 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
154 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
155 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
156 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
157 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
158 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
159 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
160 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
161 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
162
163 pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g;
164 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
165 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
166 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
167 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
168 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
169 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
170 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
171 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
172 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
173
174 pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g;
175 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
176 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
177 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
178 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
179 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
180 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
181 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
182 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
183 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
184
185 pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g;
186 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
187 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
188 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
189 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
190 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
191 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
192 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
193 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
194 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
195
196 pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g;
197 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
198 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
199 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
200 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
201 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
202 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
203 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
204 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
205 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
206
207 pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g;
208 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
209 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
210 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
211 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
212 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
213 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
214 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
215 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
216 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
217
218 pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g;
219 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
220 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
221 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
222 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
223 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
224 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
225 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
226 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
227 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
228
229 fg_corr=81.0f*fgsum-fsum*gsum;
230 den=(81.0f*f2sum-fsum*fsum)*(81.0f*g2sum-gsum*gsum);
231 if(den!=0.0)
232 {
233 if(fg_corr>=0.0) return(fg_corr*fg_corr/den);
234 return(-fg_corr*fg_corr/den);
235 }
236 return(0.0);
237 }
238
db_SignedSquareNormCorr11x11_u(unsigned char ** f_img,unsigned char ** g_img,int x_f,int y_f,int x_g,int y_g)239 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)
240 {
241 unsigned char *pf,*pg;
242 float f,g,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den;
243 int xm_f,xm_g;
244
245 xm_f=x_f-5;
246 xm_g=x_g-5;
247 fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0;
248
249 pf=f_img[y_f-5]+xm_f; pg=g_img[y_g-5]+xm_g;
250 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
251 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
252 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
253 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
254 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
255 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
256 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
257 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
258 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
259 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
260 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
261
262 pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g;
263 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
264 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
265 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
266 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
267 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
268 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
269 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
270 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
271 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
272 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
273 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
274
275 pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g;
276 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
277 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
278 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
279 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
280 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
281 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
282 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
283 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
284 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
285 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
286 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
287
288 pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g;
289 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
290 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
291 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
292 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
293 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
294 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
295 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
296 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
297 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
298 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
299 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
300
301 pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g;
302 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
303 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
304 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
305 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
306 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
307 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
308 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
309 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
310 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
311 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
312 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
313
314 pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g;
315 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
316 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
317 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
318 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
319 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
320 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
321 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
322 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
323 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
324 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
325 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
326
327 pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g;
328 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
329 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
330 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
331 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
332 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
333 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
334 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
335 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
336 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
337 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
338 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
339
340 pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g;
341 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
342 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
343 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
344 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
345 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
346 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
347 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
348 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
349 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
350 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
351 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
352
353 pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g;
354 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
355 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
356 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
357 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
358 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
359 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
360 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
361 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
362 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
363 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
364 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
365
366 pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g;
367 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
368 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
369 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
370 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
371 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
372 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
373 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
374 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
375 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
376 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
377 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
378
379 pf=f_img[y_f+5]+xm_f; pg=g_img[y_g+5]+xm_g;
380 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
381 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
382 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
383 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
384 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
385 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
386 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
387 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
388 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
389 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
390 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
391
392 fg_corr=121.0f*fgsum-fsum*gsum;
393 den=(121.0f*f2sum-fsum*fsum)*(121.0f*g2sum-gsum*gsum);
394 if(den!=0.0)
395 {
396 if(fg_corr>=0.0) return(fg_corr*fg_corr/den);
397 return(-fg_corr*fg_corr/den);
398 }
399 return(0.0);
400 }
401
db_SignedSquareNormCorr11x11_Pre_u(unsigned char ** f_img,int x_f,int y_f,float * sum,float * recip)402 inline void db_SignedSquareNormCorr11x11_Pre_u(unsigned char **f_img,int x_f,int y_f,float *sum,float *recip)
403 {
404 unsigned char *pf;
405 float den;
406 int f,f2sum,fsum;
407 int xm_f;
408
409 xm_f=x_f-5;
410
411 pf=f_img[y_f-5]+xm_f;
412 f= *pf++; f2sum=f*f; fsum=f;
413 f= *pf++; f2sum+=f*f; fsum+=f;
414 f= *pf++; f2sum+=f*f; fsum+=f;
415 f= *pf++; f2sum+=f*f; fsum+=f;
416 f= *pf++; f2sum+=f*f; fsum+=f;
417 f= *pf++; f2sum+=f*f; fsum+=f;
418 f= *pf++; f2sum+=f*f; fsum+=f;
419 f= *pf++; f2sum+=f*f; fsum+=f;
420 f= *pf++; f2sum+=f*f; fsum+=f;
421 f= *pf++; f2sum+=f*f; fsum+=f;
422 f= *pf; f2sum+=f*f; fsum+=f;
423
424 pf=f_img[y_f-4]+xm_f;
425 f= *pf++; f2sum+=f*f; fsum+=f;
426 f= *pf++; f2sum+=f*f; fsum+=f;
427 f= *pf++; f2sum+=f*f; fsum+=f;
428 f= *pf++; f2sum+=f*f; fsum+=f;
429 f= *pf++; f2sum+=f*f; fsum+=f;
430 f= *pf++; f2sum+=f*f; fsum+=f;
431 f= *pf++; f2sum+=f*f; fsum+=f;
432 f= *pf++; f2sum+=f*f; fsum+=f;
433 f= *pf++; f2sum+=f*f; fsum+=f;
434 f= *pf++; f2sum+=f*f; fsum+=f;
435 f= *pf; f2sum+=f*f; fsum+=f;
436
437 pf=f_img[y_f-3]+xm_f;
438 f= *pf++; f2sum+=f*f; fsum+=f;
439 f= *pf++; f2sum+=f*f; fsum+=f;
440 f= *pf++; f2sum+=f*f; fsum+=f;
441 f= *pf++; f2sum+=f*f; fsum+=f;
442 f= *pf++; f2sum+=f*f; fsum+=f;
443 f= *pf++; f2sum+=f*f; fsum+=f;
444 f= *pf++; f2sum+=f*f; fsum+=f;
445 f= *pf++; f2sum+=f*f; fsum+=f;
446 f= *pf++; f2sum+=f*f; fsum+=f;
447 f= *pf++; f2sum+=f*f; fsum+=f;
448 f= *pf; f2sum+=f*f; fsum+=f;
449
450 pf=f_img[y_f-2]+xm_f;
451 f= *pf++; f2sum+=f*f; fsum+=f;
452 f= *pf++; f2sum+=f*f; fsum+=f;
453 f= *pf++; f2sum+=f*f; fsum+=f;
454 f= *pf++; f2sum+=f*f; fsum+=f;
455 f= *pf++; f2sum+=f*f; fsum+=f;
456 f= *pf++; f2sum+=f*f; fsum+=f;
457 f= *pf++; f2sum+=f*f; fsum+=f;
458 f= *pf++; f2sum+=f*f; fsum+=f;
459 f= *pf++; f2sum+=f*f; fsum+=f;
460 f= *pf++; f2sum+=f*f; fsum+=f;
461 f= *pf; f2sum+=f*f; fsum+=f;
462
463 pf=f_img[y_f-1]+xm_f;
464 f= *pf++; f2sum+=f*f; fsum+=f;
465 f= *pf++; f2sum+=f*f; fsum+=f;
466 f= *pf++; f2sum+=f*f; fsum+=f;
467 f= *pf++; f2sum+=f*f; fsum+=f;
468 f= *pf++; f2sum+=f*f; fsum+=f;
469 f= *pf++; f2sum+=f*f; fsum+=f;
470 f= *pf++; f2sum+=f*f; fsum+=f;
471 f= *pf++; f2sum+=f*f; fsum+=f;
472 f= *pf++; f2sum+=f*f; fsum+=f;
473 f= *pf++; f2sum+=f*f; fsum+=f;
474 f= *pf; f2sum+=f*f; fsum+=f;
475
476 pf=f_img[y_f]+xm_f;
477 f= *pf++; f2sum+=f*f; fsum+=f;
478 f= *pf++; f2sum+=f*f; fsum+=f;
479 f= *pf++; f2sum+=f*f; fsum+=f;
480 f= *pf++; f2sum+=f*f; fsum+=f;
481 f= *pf++; f2sum+=f*f; fsum+=f;
482 f= *pf++; f2sum+=f*f; fsum+=f;
483 f= *pf++; f2sum+=f*f; fsum+=f;
484 f= *pf++; f2sum+=f*f; fsum+=f;
485 f= *pf++; f2sum+=f*f; fsum+=f;
486 f= *pf++; f2sum+=f*f; fsum+=f;
487 f= *pf; f2sum+=f*f; fsum+=f;
488
489 pf=f_img[y_f+1]+xm_f;
490 f= *pf++; f2sum+=f*f; fsum+=f;
491 f= *pf++; f2sum+=f*f; fsum+=f;
492 f= *pf++; f2sum+=f*f; fsum+=f;
493 f= *pf++; f2sum+=f*f; fsum+=f;
494 f= *pf++; f2sum+=f*f; fsum+=f;
495 f= *pf++; f2sum+=f*f; fsum+=f;
496 f= *pf++; f2sum+=f*f; fsum+=f;
497 f= *pf++; f2sum+=f*f; fsum+=f;
498 f= *pf++; f2sum+=f*f; fsum+=f;
499 f= *pf++; f2sum+=f*f; fsum+=f;
500 f= *pf; f2sum+=f*f; fsum+=f;
501
502 pf=f_img[y_f+2]+xm_f;
503 f= *pf++; f2sum+=f*f; fsum+=f;
504 f= *pf++; f2sum+=f*f; fsum+=f;
505 f= *pf++; f2sum+=f*f; fsum+=f;
506 f= *pf++; f2sum+=f*f; fsum+=f;
507 f= *pf++; f2sum+=f*f; fsum+=f;
508 f= *pf++; f2sum+=f*f; fsum+=f;
509 f= *pf++; f2sum+=f*f; fsum+=f;
510 f= *pf++; f2sum+=f*f; fsum+=f;
511 f= *pf++; f2sum+=f*f; fsum+=f;
512 f= *pf++; f2sum+=f*f; fsum+=f;
513 f= *pf; f2sum+=f*f; fsum+=f;
514
515 pf=f_img[y_f+3]+xm_f;
516 f= *pf++; f2sum+=f*f; fsum+=f;
517 f= *pf++; f2sum+=f*f; fsum+=f;
518 f= *pf++; f2sum+=f*f; fsum+=f;
519 f= *pf++; f2sum+=f*f; fsum+=f;
520 f= *pf++; f2sum+=f*f; fsum+=f;
521 f= *pf++; f2sum+=f*f; fsum+=f;
522 f= *pf++; f2sum+=f*f; fsum+=f;
523 f= *pf++; f2sum+=f*f; fsum+=f;
524 f= *pf++; f2sum+=f*f; fsum+=f;
525 f= *pf++; f2sum+=f*f; fsum+=f;
526 f= *pf; f2sum+=f*f; fsum+=f;
527
528 pf=f_img[y_f+4]+xm_f;
529 f= *pf++; f2sum+=f*f; fsum+=f;
530 f= *pf++; f2sum+=f*f; fsum+=f;
531 f= *pf++; f2sum+=f*f; fsum+=f;
532 f= *pf++; f2sum+=f*f; fsum+=f;
533 f= *pf++; f2sum+=f*f; fsum+=f;
534 f= *pf++; f2sum+=f*f; fsum+=f;
535 f= *pf++; f2sum+=f*f; fsum+=f;
536 f= *pf++; f2sum+=f*f; fsum+=f;
537 f= *pf++; f2sum+=f*f; fsum+=f;
538 f= *pf++; f2sum+=f*f; fsum+=f;
539 f= *pf; f2sum+=f*f; fsum+=f;
540
541 pf=f_img[y_f+5]+xm_f;
542 f= *pf++; f2sum+=f*f; fsum+=f;
543 f= *pf++; f2sum+=f*f; fsum+=f;
544 f= *pf++; f2sum+=f*f; fsum+=f;
545 f= *pf++; f2sum+=f*f; fsum+=f;
546 f= *pf++; f2sum+=f*f; fsum+=f;
547 f= *pf++; f2sum+=f*f; fsum+=f;
548 f= *pf++; f2sum+=f*f; fsum+=f;
549 f= *pf++; f2sum+=f*f; fsum+=f;
550 f= *pf++; f2sum+=f*f; fsum+=f;
551 f= *pf++; f2sum+=f*f; fsum+=f;
552 f= *pf; f2sum+=f*f; fsum+=f;
553
554 *sum= (float) fsum;
555 den=(121.0f*f2sum-fsum*fsum);
556 *recip=(float)(((den!=0.0)?1.0/den:0.0));
557 }
558
db_SignedSquareNormCorr5x5_PreAlign_u(short * patch,const unsigned char * const * f_img,int x_f,int y_f,float * sum,float * recip)559 inline void db_SignedSquareNormCorr5x5_PreAlign_u(short *patch,const unsigned char * const *f_img,int x_f,int y_f,float *sum,float *recip)
560 {
561 float den;
562 int f2sum,fsum;
563 int xm_f=x_f-2;
564
565 #ifndef DB_USE_SSE2
566 const unsigned char *pf;
567 short f;
568
569 pf=f_img[y_f-2]+xm_f;
570 f= *pf++; f2sum=f*f; fsum=f; (*patch++)=f;
571 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
572 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
573 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
574 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
575
576 pf=f_img[y_f-1]+xm_f;
577 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
578 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
579 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
580 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
581 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
582
583 pf=f_img[y_f]+xm_f;
584 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
585 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
586 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
587 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
588 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
589
590 pf=f_img[y_f+1]+xm_f;
591 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
592 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
593 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
594 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
595 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
596
597 pf=f_img[y_f+2]+xm_f;
598 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
599 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
600 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
601 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
602 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
603 //int xwi;
604 //int ywi;
605 //f2sum=0;
606 //fsum=0;
607 //for (int r=-5;r<=5;r++){
608 // ywi=y_f+r;
609 // for (int c=-5;c<=5;c++){
610 // xwi=x_f+c;
611 // f=f_img[ywi][xwi];
612 // f2sum+=f*f;
613 // fsum+=f;
614 // (*patch++)=f;
615 // }
616 //}
617 (*patch++)=0; (*patch++)=0; (*patch++)=0; (*patch++)=0; (*patch++)=0;
618 (*patch++)=0; (*patch++)=0;
619 #endif /* DB_USE_SSE2 */
620
621 *sum= (float) fsum;
622 den=(25.0f*f2sum-fsum*fsum);
623 *recip= (float)((den!=0.0)?1.0/den:0.0);
624 }
625
db_SignedSquareNormCorr21x21_PreAlign_u(short * patch,const unsigned char * const * f_img,int x_f,int y_f,float * sum,float * recip)626 inline void db_SignedSquareNormCorr21x21_PreAlign_u(short *patch,const unsigned char * const *f_img,int x_f,int y_f,float *sum,float *recip)
627 {
628 float den;
629 int f2sum,fsum;
630 int xm_f=x_f-10;
631 short f;
632
633 int xwi;
634 int ywi;
635 f2sum=0;
636 fsum=0;
637 for (int r=-10;r<=10;r++){
638 ywi=y_f+r;
639 for (int c=-10;c<=10;c++){
640 xwi=x_f+c;
641 f=f_img[ywi][xwi];
642 f2sum+=f*f;
643 fsum+=f;
644 (*patch++)=f;
645 }
646 }
647
648 for(int i=442; i<512; i++)
649 (*patch++)=0;
650
651 *sum= (float) fsum;
652 den=(441.0f*f2sum-fsum*fsum);
653 *recip= (float)((den!=0.0)?1.0/den:0.0);
654
655
656 }
657
658 /* Lay out the image in the patch, computing norm and
659 */
db_SignedSquareNormCorr11x11_PreAlign_u(short * patch,const unsigned char * const * f_img,int x_f,int y_f,float * sum,float * recip)660 inline void db_SignedSquareNormCorr11x11_PreAlign_u(short *patch,const unsigned char * const *f_img,int x_f,int y_f,float *sum,float *recip)
661 {
662 float den;
663 int f2sum,fsum;
664 int xm_f=x_f-5;
665
666 #ifndef DB_USE_SSE2
667 const unsigned char *pf;
668 short f;
669
670 pf=f_img[y_f-5]+xm_f;
671 f= *pf++; f2sum=f*f; fsum=f; (*patch++)=f;
672 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
673 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
674 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
675 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
676 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
677 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
678 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
679 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
680 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
681 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
682
683 pf=f_img[y_f-4]+xm_f;
684 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
685 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
686 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
687 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
688 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
689 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
690 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
691 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
692 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
693 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
694 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
695
696 pf=f_img[y_f-3]+xm_f;
697 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
698 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
699 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
700 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
701 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
702 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
703 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
704 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
705 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
706 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
707 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
708
709 pf=f_img[y_f-2]+xm_f;
710 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
711 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
712 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
713 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
714 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
715 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
716 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
717 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
718 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
719 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
720 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
721
722 pf=f_img[y_f-1]+xm_f;
723 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
724 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
725 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
726 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
727 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
728 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
729 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
730 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
731 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
732 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
733 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
734
735 pf=f_img[y_f]+xm_f;
736 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
737 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
738 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
739 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
740 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
741 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
742 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
743 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
744 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
745 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
746 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
747
748 pf=f_img[y_f+1]+xm_f;
749 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
750 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
751 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
752 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
753 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
754 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
755 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
756 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
757 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
758 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
759 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
760
761 pf=f_img[y_f+2]+xm_f;
762 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
763 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
764 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
765 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
766 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
767 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
768 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
769 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
770 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
771 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
772 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
773
774 pf=f_img[y_f+3]+xm_f;
775 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
776 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
777 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
778 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
779 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
780 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
781 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
782 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
783 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
784 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
785 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
786
787 pf=f_img[y_f+4]+xm_f;
788 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
789 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
790 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
791 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
792 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
793 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
794 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
795 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
796 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
797 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
798 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
799
800 pf=f_img[y_f+5]+xm_f;
801 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
802 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
803 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
804 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
805 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
806 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
807 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
808 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
809 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
810 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
811 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
812
813 //int xwi;
814 //int ywi;
815 //f2sum=0;
816 //fsum=0;
817 //for (int r=-5;r<=5;r++){
818 // ywi=y_f+r;
819 // for (int c=-5;c<=5;c++){
820 // xwi=x_f+c;
821 // f=f_img[ywi][xwi];
822 // f2sum+=f*f;
823 // fsum+=f;
824 // (*patch++)=f;
825 // }
826 //}
827
828 (*patch++)=0; (*patch++)=0; (*patch++)=0; (*patch++)=0; (*patch++)=0;
829 (*patch++)=0; (*patch++)=0;
830 #else
831 const unsigned char *pf0 =f_img[y_f-5]+xm_f;
832 const unsigned char *pf1 =f_img[y_f-4]+xm_f;
833 const unsigned char *pf2 =f_img[y_f-3]+xm_f;
834 const unsigned char *pf3 =f_img[y_f-2]+xm_f;
835 const unsigned char *pf4 =f_img[y_f-1]+xm_f;
836 const unsigned char *pf5 =f_img[y_f ]+xm_f;
837 const unsigned char *pf6 =f_img[y_f+1]+xm_f;
838 const unsigned char *pf7 =f_img[y_f+2]+xm_f;
839 const unsigned char *pf8 =f_img[y_f+3]+xm_f;
840 const unsigned char *pf9 =f_img[y_f+4]+xm_f;
841 const unsigned char *pf10=f_img[y_f+5]+xm_f;
842
843 /* pixel mask */
844 const unsigned char pm[16] = {
845 0xFF,0xFF,
846 0xFF,0xFF,
847 0xFF,0xFF,
848 0,0,0,0,0,
849 0,0,0,0,0};
850 const unsigned char * pm_p = pm;
851
852 _asm
853 {
854 mov ecx,patch /* load patch pointer */
855 mov ebx, pm_p /* load pixel mask pointer */
856 movdqu xmm1,[ebx] /* load pixel mask */
857
858 pxor xmm5,xmm5 /* set xmm5 to 0 accumulator for sum squares */
859 pxor xmm4,xmm4 /* set xmm4 to 0 accumulator for sum */
860 pxor xmm0,xmm0 /* set xmm0 to 0 */
861
862 /* row 0 */
863 mov eax,pf0 /* load image pointer */
864 movdqu xmm7,[eax] /* load 16 pixels */
865 movdqa xmm6,xmm7
866
867 punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/
868 punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/
869
870 pand xmm6,xmm1 /* mask out pixels 12-16 */
871
872 movdqa [ecx+0*22],xmm7 /* move short values to patch */
873 movdqa [ecx+0*22+16],xmm6 /* move short values to patch */
874
875 paddusw xmm4,xmm7 /* accumulate sums */
876 pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */
877 paddd xmm5,xmm7 /* accumulate sum squares */
878
879 paddw xmm4,xmm6 /* accumulate sums */
880 pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */
881 paddd xmm5,xmm6 /* accumulate sum squares */
882
883 /* row 1 */
884 mov eax,pf1 /* load image pointer */
885 movdqu xmm7,[eax] /* load 16 pixels */
886 movdqa xmm6,xmm7
887
888 punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/
889 punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/
890
891 pand xmm6,xmm1 /* mask out pixels 12-16 */
892
893 movdqu [ecx+1*22],xmm7 /* move short values to patch */
894 movdqu [ecx+1*22+16],xmm6 /* move short values to patch */
895
896 paddusw xmm4,xmm7 /* accumulate sums */
897 pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */
898 paddd xmm5,xmm7 /* accumulate sum squares */
899
900 paddw xmm4,xmm6 /* accumulate sums */
901 pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */
902 paddd xmm5,xmm6 /* accumulate sum squares */
903
904 /* row 2 */
905 mov eax,pf2 /* load image pointer */
906 movdqu xmm7,[eax] /* load 16 pixels */
907 movdqa xmm6,xmm7
908
909 punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/
910 punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/
911
912 pand xmm6,xmm1 /* mask out pixels 12-16 */
913
914 movdqu [ecx+2*22],xmm7 /* move short values to patch */
915 movdqu [ecx+2*22+16],xmm6 /* move short values to patch */
916
917 paddusw xmm4,xmm7 /* accumulate sums */
918 pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */
919 paddd xmm5,xmm7 /* accumulate sum squares */
920
921 paddw xmm4,xmm6 /* accumulate sums */
922 pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */
923 paddd xmm5,xmm6 /* accumulate sum squares */
924
925 /* row 3 */
926 mov eax,pf3 /* load image pointer */
927 movdqu xmm7,[eax] /* load 16 pixels */
928 movdqa xmm6,xmm7
929
930 punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/
931 punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/
932
933 pand xmm6,xmm1 /* mask out pixels 12-16 */
934
935 movdqu [ecx+3*22],xmm7 /* move short values to patch */
936 movdqu [ecx+3*22+16],xmm6 /* move short values to patch */
937
938 paddusw xmm4,xmm7 /* accumulate sums */
939 pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */
940 paddd xmm5,xmm7 /* accumulate sum squares */
941
942 paddw xmm4,xmm6 /* accumulate sums */
943 pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */
944 paddd xmm5,xmm6 /* accumulate sum squares */
945
946 /* row 4 */
947 mov eax,pf4 /* load image pointer */
948 movdqu xmm7,[eax] /* load 16 pixels */
949 movdqa xmm6,xmm7
950
951 punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/
952 punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/
953
954 pand xmm6,xmm1 /* mask out pixels 12-16 */
955
956 movdqu [ecx+4*22],xmm7 /* move short values to patch */
957 movdqu [ecx+4*22+16],xmm6 /* move short values to patch */
958
959 paddusw xmm4,xmm7 /* accumulate sums */
960 pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */
961 paddd xmm5,xmm7 /* accumulate sum squares */
962
963 paddw xmm4,xmm6 /* accumulate sums */
964 pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */
965 paddd xmm5,xmm6 /* accumulate sum squares */
966
967 /* row 5 */
968 mov eax,pf5 /* load image pointer */
969 movdqu xmm7,[eax] /* load 16 pixels */
970 movdqa xmm6,xmm7
971
972 punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/
973 punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/
974
975 pand xmm6,xmm1 /* mask out pixels 12-16 */
976
977 movdqu [ecx+5*22],xmm7 /* move short values to patch */
978 movdqu [ecx+5*22+16],xmm6 /* move short values to patch */
979
980 paddusw xmm4,xmm7 /* accumulate sums */
981 pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */
982 paddd xmm5,xmm7 /* accumulate sum squares */
983
984 paddw xmm4,xmm6 /* accumulate sums */
985 pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */
986 paddd xmm5,xmm6 /* accumulate sum squares */
987
988 /* row 6 */
989 mov eax,pf6 /* load image pointer */
990 movdqu xmm7,[eax] /* load 16 pixels */
991 movdqa xmm6,xmm7
992
993 punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/
994 punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/
995
996 pand xmm6,xmm1 /* mask out pixels 12-16 */
997
998 movdqu [ecx+6*22],xmm7 /* move short values to patch */
999 movdqu [ecx+6*22+16],xmm6 /* move short values to patch */
1000
1001 paddusw xmm4,xmm7 /* accumulate sums */
1002 pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */
1003 paddd xmm5,xmm7 /* accumulate sum squares */
1004
1005 paddw xmm4,xmm6 /* accumulate sums */
1006 pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */
1007 paddd xmm5,xmm6 /* accumulate sum squares */
1008
1009 /* row 7 */
1010 mov eax,pf7 /* load image pointer */
1011 movdqu xmm7,[eax] /* load 16 pixels */
1012 movdqa xmm6,xmm7
1013
1014 punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/
1015 punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/
1016
1017 pand xmm6,xmm1 /* mask out pixels 12-16 */
1018
1019 movdqu [ecx+7*22],xmm7 /* move short values to patch */
1020 movdqu [ecx+7*22+16],xmm6 /* move short values to patch */
1021
1022 paddusw xmm4,xmm7 /* accumulate sums */
1023 pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */
1024 paddd xmm5,xmm7 /* accumulate sum squares */
1025
1026 paddw xmm4,xmm6 /* accumulate sums */
1027 pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */
1028 paddd xmm5,xmm6 /* accumulate sum squares */
1029
1030 /* row 8 */
1031 mov eax,pf8 /* load image pointer */
1032 movdqu xmm7,[eax] /* load 16 pixels */
1033 movdqa xmm6,xmm7
1034
1035 punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/
1036 punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/
1037
1038 pand xmm6,xmm1 /* mask out pixels 12-16 */
1039
1040 movdqa [ecx+8*22],xmm7 /* move short values to patch */
1041 movdqa [ecx+8*22+16],xmm6 /* move short values to patch */
1042
1043 paddusw xmm4,xmm7 /* accumulate sums */
1044 pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */
1045 paddd xmm5,xmm7 /* accumulate sum squares */
1046
1047 paddw xmm4,xmm6 /* accumulate sums */
1048 pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */
1049 paddd xmm5,xmm6 /* accumulate sum squares */
1050
1051 /* row 9 */
1052 mov eax,pf9 /* load image pointer */
1053 movdqu xmm7,[eax] /* load 16 pixels */
1054 movdqa xmm6,xmm7
1055
1056 punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/
1057 punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/
1058
1059 pand xmm6,xmm1 /* mask out pixels 12-16 */
1060
1061 movdqu [ecx+9*22],xmm7 /* move short values to patch */
1062 movdqu [ecx+9*22+16],xmm6 /* move short values to patch */
1063
1064 paddusw xmm4,xmm7 /* accumulate sums */
1065 pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */
1066 paddd xmm5,xmm7 /* accumulate sum squares */
1067
1068 paddw xmm4,xmm6 /* accumulate sums */
1069 pmaddwd xmm6,xmm6 /* multiply 16 bit uints into 16 bit uints */
1070 paddd xmm5,xmm6 /* accumulate sum squares */
1071
1072 /* row 10 */
1073 mov eax,pf10 /* load image pointer */
1074 movdqu xmm7,[eax] /* load 16 pixels */
1075 movdqa xmm6,xmm7
1076
1077 punpcklbw xmm7,xmm0 /* unpack low pixels (first 8)*/
1078 punpckhbw xmm6,xmm0 /* unpack high pixels (last 8)*/
1079
1080 pand xmm6,xmm1 /* mask out pixels 12-16 */
1081
1082 movdqu [ecx+10*22],xmm7 /* move short values to patch */
1083 movdqu [ecx+10*22+16],xmm6 /* move short values to patch */
1084
1085 paddusw xmm4,xmm7 /* accumulate sums */
1086 pmaddwd xmm7,xmm7 /* multiply 16 bit ints and add into 32 bit ints */
1087 paddd xmm5,xmm7 /* accumulate sum squares */
1088
1089 paddw xmm4,xmm6 /* accumulate sums */
1090 pmaddwd xmm6,xmm6 /* multiply 16 bit ints and add into 32 bit ints */
1091 paddd xmm5,xmm6 /* accumulate sum squares */
1092
1093 /* add up the sum squares */
1094 movhlps xmm0,xmm5 /* high half to low half */
1095 paddd xmm5,xmm0 /* add high to low */
1096 pshuflw xmm0,xmm5, 0xE /* reshuffle */
1097 paddd xmm5,xmm0 /* add remaining */
1098 movd f2sum,xmm5
1099
1100 /* add up the sum */
1101 movhlps xmm0,xmm4
1102 paddw xmm4,xmm0 /* halves added */
1103 pshuflw xmm0,xmm4,0xE
1104 paddw xmm4,xmm0 /* quarters added */
1105 pshuflw xmm0,xmm4,0x1
1106 paddw xmm4,xmm0 /* eighth added */
1107 movd fsum, xmm4
1108
1109 emms
1110 }
1111
1112 fsum = fsum & 0xFFFF;
1113
1114 patch[126] = 0;
1115 patch[127] = 0;
1116 #endif /* DB_USE_SSE2 */
1117
1118 *sum= (float) fsum;
1119 den=(121.0f*f2sum-fsum*fsum);
1120 *recip= (float)((den!=0.0)?1.0/den:0.0);
1121 }
1122
AffineWarpPointOffset(float & r_w,float & c_w,double Hinv[9],int r,int c)1123 void AffineWarpPointOffset(float &r_w,float &c_w,double Hinv[9],int r,int c)
1124 {
1125 r_w=(float)(Hinv[3]*c+Hinv[4]*r);
1126 c_w=(float)(Hinv[0]*c+Hinv[1]*r);
1127 }
1128
1129
1130
1131 /*!
1132 Prewarp the patches with given affine transform. For a given homogeneous point "x", "H*x" is
1133 the warped point and for any displacement "d" in the warped image resulting in point "y", the
1134 corresponding point in the original image is given by "Hinv*y", which can be simplified for affine H.
1135 If "affine" is 1, then nearest neighbor method is used, else if it is 2, then
1136 bilinear method is used.
1137 */
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)1138 inline void db_SignedSquareNormCorr11x11_PreAlign_AffinePatchWarp_u(short *patch,const unsigned char * const *f_img,
1139 int xi,int yi,float *sum,float *recip,
1140 const double Hinv[9],int affine)
1141 {
1142 float den;
1143 short f;
1144 int f2sum,fsum;
1145
1146 f2sum=0;
1147 fsum=0;
1148
1149 if (affine==1)
1150 {
1151 for (int r=0;r<11;r++){
1152 for (int c=0;c<11;c++){
1153 f=f_img[yi+AffineWarpPoint_NN_LUT_y[r][c]][xi+AffineWarpPoint_NN_LUT_x[r][c]];
1154 f2sum+=f*f;
1155 fsum+=f;
1156 (*patch++)=f;
1157 }
1158 }
1159 }
1160 else if (affine==2)
1161 {
1162 for (int r=0;r<11;r++){
1163 for (int c=0;c<11;c++){
1164 f=db_BilinearInterpolation(yi+AffineWarpPoint_BL_LUT_y[r][c]
1165 ,xi+AffineWarpPoint_BL_LUT_x[r][c],f_img);
1166 f2sum+=f*f;
1167 fsum+=f;
1168 (*patch++)=f;
1169 }
1170 }
1171 }
1172
1173
1174
1175 (*patch++)=0; (*patch++)=0; (*patch++)=0; (*patch++)=0; (*patch++)=0;
1176 (*patch++)=0; (*patch++)=0;
1177
1178 *sum= (float) fsum;
1179 den=(121.0f*f2sum-fsum*fsum);
1180 *recip= (float)((den!=0.0)?1.0/den:0.0);
1181 }
1182
1183
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)1184 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,
1185 float fsum_gsum,float f_recip_g_recip)
1186 {
1187 unsigned char *pf,*pg;
1188 int fgsum;
1189 float fg_corr;
1190 int xm_f,xm_g;
1191
1192 xm_f=x_f-5;
1193 xm_g=x_g-5;
1194
1195 pf=f_img[y_f-5]+xm_f; pg=g_img[y_g-5]+xm_g;
1196 fgsum=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1197 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1198 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1199 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
1200
1201 pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g;
1202 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1203 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1204 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1205 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
1206
1207 pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g;
1208 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1209 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1210 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1211 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
1212
1213 pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g;
1214 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1215 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1216 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1217 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
1218
1219 pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g;
1220 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1221 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1222 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1223 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
1224
1225 pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g;
1226 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1227 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1228 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1229 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
1230
1231 pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g;
1232 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1233 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1234 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1235 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
1236
1237 pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g;
1238 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1239 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1240 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1241 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
1242
1243 pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g;
1244 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1245 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1246 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1247 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
1248
1249 pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g;
1250 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1251 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1252 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1253 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
1254
1255 pf=f_img[y_f+5]+xm_f; pg=g_img[y_g+5]+xm_g;
1256 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1257 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1258 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
1259 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
1260
1261 fg_corr=121.0f*fgsum-fsum_gsum;
1262 if(fg_corr>=0.0) return(fg_corr*fg_corr*f_recip_g_recip);
1263 return(-fg_corr*fg_corr*f_recip_g_recip);
1264 }
1265
db_SignedSquareNormCorr21x21Aligned_Post_s(const short * f_patch,const short * g_patch,float fsum_gsum,float f_recip_g_recip)1266 float db_SignedSquareNormCorr21x21Aligned_Post_s(const short *f_patch,const short *g_patch,float fsum_gsum,float f_recip_g_recip)
1267 {
1268 float fgsum,fg_corr;
1269
1270 fgsum= (float) db_ScalarProduct512_s(f_patch,g_patch);
1271
1272 fg_corr=441.0f*fgsum-fsum_gsum;
1273 if(fg_corr>=0.0) return(fg_corr*fg_corr*f_recip_g_recip);
1274 return(-fg_corr*fg_corr*f_recip_g_recip);
1275 }
1276
1277
db_SignedSquareNormCorr11x11Aligned_Post_s(const short * f_patch,const short * g_patch,float fsum_gsum,float f_recip_g_recip)1278 float db_SignedSquareNormCorr11x11Aligned_Post_s(const short *f_patch,const short *g_patch,float fsum_gsum,float f_recip_g_recip)
1279 {
1280 float fgsum,fg_corr;
1281
1282 fgsum= (float) db_ScalarProduct128_s(f_patch,g_patch);
1283
1284 fg_corr=121.0f*fgsum-fsum_gsum;
1285 if(fg_corr>=0.0) return(fg_corr*fg_corr*f_recip_g_recip);
1286 return(-fg_corr*fg_corr*f_recip_g_recip);
1287 }
1288
db_SignedSquareNormCorr5x5Aligned_Post_s(const short * f_patch,const short * g_patch,float fsum_gsum,float f_recip_g_recip)1289 float db_SignedSquareNormCorr5x5Aligned_Post_s(const short *f_patch,const short *g_patch,float fsum_gsum,float f_recip_g_recip)
1290 {
1291 float fgsum,fg_corr;
1292
1293 fgsum= (float) db_ScalarProduct32_s(f_patch,g_patch);
1294
1295 fg_corr=25.0f*fgsum-fsum_gsum;
1296 if(fg_corr>=0.0) return(fg_corr*fg_corr*f_recip_g_recip);
1297 return(-fg_corr*fg_corr*f_recip_g_recip);
1298 }
1299
1300
db_SignedSquareNormCorr15x15_u(unsigned char ** f_img,unsigned char ** g_img,int x_f,int y_f,int x_g,int y_g)1301 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)
1302 {
1303 unsigned char *pf,*pg;
1304 float f,g,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den;
1305 int xm_f,xm_g;
1306
1307 xm_f=x_f-7;
1308 xm_g=x_g-7;
1309 fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0;
1310
1311 pf=f_img[y_f-7]+xm_f; pg=g_img[y_g-7]+xm_g;
1312 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1313 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1314 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1315 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1316 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1317 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1318 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1319 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1320 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1321 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1322 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1323 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1324 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1325 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1326 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1327
1328 pf=f_img[y_f-6]+xm_f; pg=g_img[y_g-6]+xm_g;
1329 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1330 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1331 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1332 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1333 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1334 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1335 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1336 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1337 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1338 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1339 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1340 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1341 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1342 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1343 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1344
1345 pf=f_img[y_f-5]+xm_f; pg=g_img[y_g-5]+xm_g;
1346 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1347 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1348 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1349 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1350 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1351 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1352 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1353 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1354 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1355 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1356 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1357 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1358 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1359 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1360 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1361
1362 pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g;
1363 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1364 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1365 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1366 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1367 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1368 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1369 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1370 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1371 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1372 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1373 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1374 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1375 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1376 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1377 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1378
1379 pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g;
1380 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1381 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1382 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1383 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1384 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1385 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1386 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1387 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1388 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1389 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1390 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1391 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1392 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1393 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1394 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1395
1396 pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g;
1397 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1398 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1399 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1400 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1401 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1402 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1403 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1404 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1405 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1406 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1407 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1408 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1409 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1410 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1411 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1412
1413 pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g;
1414 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1415 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1416 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1417 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1418 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1419 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1420 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1421 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1422 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1423 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1424 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1425 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1426 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1427 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1428 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1429
1430 pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g;
1431 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1432 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1433 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1434 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1435 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1436 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1437 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1438 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1439 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1440 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1441 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1442 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1443 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1444 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1445 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1446
1447 pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g;
1448 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1449 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1450 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1451 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1452 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1453 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1454 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1455 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1456 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1457 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1458 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1459 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1460 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1461 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1462 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1463
1464 pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g;
1465 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1466 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1467 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1468 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1469 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1470 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1471 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1472 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1473 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1474 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1475 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1476 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1477 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1478 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1479 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1480
1481 pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g;
1482 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1483 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1484 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1485 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1486 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1487 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1488 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1489 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1490 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1491 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1492 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1493 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1494 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1495 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1496 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1497
1498 pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g;
1499 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1500 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1501 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1502 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1503 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1504 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1505 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1506 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1507 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1508 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1509 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1510 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1511 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1512 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1513 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1514
1515 pf=f_img[y_f+5]+xm_f; pg=g_img[y_g+5]+xm_g;
1516 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1517 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1518 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1519 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1520 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1521 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1522 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1523 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1524 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1525 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1526 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1527 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1528 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1529 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1530 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1531
1532 pf=f_img[y_f+6]+xm_f; pg=g_img[y_g+6]+xm_g;
1533 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1534 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1535 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1536 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1537 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1538 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1539 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1540 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1541 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1542 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1543 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1544 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1545 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1546 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1547 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1548
1549 pf=f_img[y_f+7]+xm_f; pg=g_img[y_g+7]+xm_g;
1550 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1551 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1552 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1553 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1554 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1555 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1556 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1557 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1558 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1559 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1560 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1561 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1562 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1563 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1564 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1565
1566 fg_corr=225.0f*fgsum-fsum*gsum;
1567 den=(225.0f*f2sum-fsum*fsum)*(225.0f*g2sum-gsum*gsum);
1568 if(den!=0.0)
1569 {
1570 if(fg_corr>=0.0) return(fg_corr*fg_corr/den);
1571 return(-fg_corr*fg_corr/den);
1572 }
1573 return(0.0);
1574 }
1575
db_SignedSquareNormCorr7x7_f(float ** f_img,float ** g_img,int x_f,int y_f,int x_g,int y_g)1576 inline float db_SignedSquareNormCorr7x7_f(float **f_img,float **g_img,int x_f,int y_f,int x_g,int y_g)
1577 {
1578 float f,g,*pf,*pg,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den;
1579 int xm_f,xm_g;
1580
1581 xm_f=x_f-3;
1582 xm_g=x_g-3;
1583 fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0;
1584
1585 pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g;
1586 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1587 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1588 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1589 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1590 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1591 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1592 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1593
1594 pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g;
1595 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1596 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1597 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1598 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1599 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1600 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1601 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1602
1603 pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g;
1604 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1605 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1606 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1607 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1608 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1609 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1610 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1611
1612 pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g;
1613 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1614 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1615 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1616 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1617 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1618 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1619 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1620
1621 pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g;
1622 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1623 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1624 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1625 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1626 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1627 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1628 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1629
1630 pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g;
1631 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1632 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1633 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1634 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1635 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1636 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1637 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1638
1639 pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g;
1640 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1641 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1642 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1643 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1644 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1645 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1646 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1647
1648 fg_corr=49.0f*fgsum-fsum*gsum;
1649 den=(49.0f*f2sum-fsum*fsum)*(49.0f*g2sum-gsum*gsum);
1650 if(den!=0.0)
1651 {
1652 if(fg_corr>=0.0) return(fg_corr*fg_corr/den);
1653 return(-fg_corr*fg_corr/den);
1654 }
1655 return(0.0);
1656 }
1657
db_SignedSquareNormCorr9x9_f(float ** f_img,float ** g_img,int x_f,int y_f,int x_g,int y_g)1658 inline float db_SignedSquareNormCorr9x9_f(float **f_img,float **g_img,int x_f,int y_f,int x_g,int y_g)
1659 {
1660 float f,g,*pf,*pg,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den;
1661 int xm_f,xm_g;
1662
1663 xm_f=x_f-4;
1664 xm_g=x_g-4;
1665 fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0;
1666
1667 pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g;
1668 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1669 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1670 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1671 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1672 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1673 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1674 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1675 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1676 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1677
1678 pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g;
1679 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1680 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1681 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1682 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1683 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1684 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1685 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1686 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1687 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1688
1689 pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g;
1690 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1691 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1692 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1693 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1694 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1695 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1696 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1697 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1698 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1699
1700 pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g;
1701 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1702 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1703 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1704 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1705 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1706 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1707 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1708 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1709 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1710
1711 pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g;
1712 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1713 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1714 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1715 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1716 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1717 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1718 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1719 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1720 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1721
1722 pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g;
1723 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1724 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1725 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1726 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1727 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1728 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1729 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1730 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1731 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1732
1733 pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g;
1734 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1735 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1736 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1737 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1738 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1739 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1740 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1741 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1742 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1743
1744 pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g;
1745 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1746 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1747 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1748 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1749 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1750 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1751 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1752 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1753 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1754
1755 pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g;
1756 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1757 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1758 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1759 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1760 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1761 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1762 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1763 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1764 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1765
1766 fg_corr=81.0f*fgsum-fsum*gsum;
1767 den=(81.0f*f2sum-fsum*fsum)*(81.0f*g2sum-gsum*gsum);
1768 if(den!=0.0)
1769 {
1770 if(fg_corr>=0.0) return(fg_corr*fg_corr/den);
1771 return(-fg_corr*fg_corr/den);
1772 }
1773 return(0.0);
1774 }
1775
db_SignedSquareNormCorr11x11_f(float ** f_img,float ** g_img,int x_f,int y_f,int x_g,int y_g)1776 inline float db_SignedSquareNormCorr11x11_f(float **f_img,float **g_img,int x_f,int y_f,int x_g,int y_g)
1777 {
1778 float *pf,*pg;
1779 float f,g,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den;
1780 int xm_f,xm_g;
1781
1782 xm_f=x_f-5;
1783 xm_g=x_g-5;
1784 fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0;
1785
1786 pf=f_img[y_f-5]+xm_f; pg=g_img[y_g-5]+xm_g;
1787 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1788 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1789 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1790 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1791 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1792 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1793 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1794 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1795 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1796 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1797 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1798
1799 pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g;
1800 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1801 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1802 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1803 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1804 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1805 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1806 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1807 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1808 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1809 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1810 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1811
1812 pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g;
1813 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1814 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1815 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1816 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1817 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1818 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1819 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1820 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1821 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1822 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1823 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1824
1825 pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g;
1826 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1827 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1828 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1829 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1830 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1831 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1832 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1833 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1834 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1835 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1836 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1837
1838 pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g;
1839 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1840 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1841 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1842 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1843 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1844 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1845 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1846 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1847 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1848 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1849 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1850
1851 pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g;
1852 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1853 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1854 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1855 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1856 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1857 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1858 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1859 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1860 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1861 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1862 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1863
1864 pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g;
1865 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1866 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1867 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1868 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1869 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1870 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1871 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1872 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1873 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1874 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1875 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1876
1877 pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g;
1878 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1879 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1880 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1881 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1882 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1883 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1884 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1885 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1886 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1887 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1888 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1889
1890 pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g;
1891 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1892 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1893 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1894 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1895 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1896 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1897 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1898 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1899 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1900 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1901 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1902
1903 pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g;
1904 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1905 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1906 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1907 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1908 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1909 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1910 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1911 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1912 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1913 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1914 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1915
1916 pf=f_img[y_f+5]+xm_f; pg=g_img[y_g+5]+xm_g;
1917 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1918 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1919 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1920 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1921 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1922 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1923 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1924 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1925 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1926 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1927 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
1928
1929 fg_corr=121.0f*fgsum-fsum*gsum;
1930 den=(121.0f*f2sum-fsum*fsum)*(121.0f*g2sum-gsum*gsum);
1931 if(den!=0.0)
1932 {
1933 if(fg_corr>=0.0) return(fg_corr*fg_corr/den);
1934 return(-fg_corr*fg_corr/den);
1935 }
1936 return(0.0);
1937 }
1938
db_SignedSquareNormCorr11x11_Pre_f(float ** f_img,int x_f,int y_f,float * sum,float * recip)1939 inline void db_SignedSquareNormCorr11x11_Pre_f(float **f_img,int x_f,int y_f,float *sum,float *recip)
1940 {
1941 float *pf,den;
1942 float f,f2sum,fsum;
1943 int xm_f;
1944
1945 xm_f=x_f-5;
1946
1947 pf=f_img[y_f-5]+xm_f;
1948 f= *pf++; f2sum=f*f; fsum=f;
1949 f= *pf++; f2sum+=f*f; fsum+=f;
1950 f= *pf++; f2sum+=f*f; fsum+=f;
1951 f= *pf++; f2sum+=f*f; fsum+=f;
1952 f= *pf++; f2sum+=f*f; fsum+=f;
1953 f= *pf++; f2sum+=f*f; fsum+=f;
1954 f= *pf++; f2sum+=f*f; fsum+=f;
1955 f= *pf++; f2sum+=f*f; fsum+=f;
1956 f= *pf++; f2sum+=f*f; fsum+=f;
1957 f= *pf++; f2sum+=f*f; fsum+=f;
1958 f= *pf; f2sum+=f*f; fsum+=f;
1959
1960 pf=f_img[y_f-4]+xm_f;
1961 f= *pf++; f2sum+=f*f; fsum+=f;
1962 f= *pf++; f2sum+=f*f; fsum+=f;
1963 f= *pf++; f2sum+=f*f; fsum+=f;
1964 f= *pf++; f2sum+=f*f; fsum+=f;
1965 f= *pf++; f2sum+=f*f; fsum+=f;
1966 f= *pf++; f2sum+=f*f; fsum+=f;
1967 f= *pf++; f2sum+=f*f; fsum+=f;
1968 f= *pf++; f2sum+=f*f; fsum+=f;
1969 f= *pf++; f2sum+=f*f; fsum+=f;
1970 f= *pf++; f2sum+=f*f; fsum+=f;
1971 f= *pf; f2sum+=f*f; fsum+=f;
1972
1973 pf=f_img[y_f-3]+xm_f;
1974 f= *pf++; f2sum+=f*f; fsum+=f;
1975 f= *pf++; f2sum+=f*f; fsum+=f;
1976 f= *pf++; f2sum+=f*f; fsum+=f;
1977 f= *pf++; f2sum+=f*f; fsum+=f;
1978 f= *pf++; f2sum+=f*f; fsum+=f;
1979 f= *pf++; f2sum+=f*f; fsum+=f;
1980 f= *pf++; f2sum+=f*f; fsum+=f;
1981 f= *pf++; f2sum+=f*f; fsum+=f;
1982 f= *pf++; f2sum+=f*f; fsum+=f;
1983 f= *pf++; f2sum+=f*f; fsum+=f;
1984 f= *pf; f2sum+=f*f; fsum+=f;
1985
1986 pf=f_img[y_f-2]+xm_f;
1987 f= *pf++; f2sum+=f*f; fsum+=f;
1988 f= *pf++; f2sum+=f*f; fsum+=f;
1989 f= *pf++; f2sum+=f*f; fsum+=f;
1990 f= *pf++; f2sum+=f*f; fsum+=f;
1991 f= *pf++; f2sum+=f*f; fsum+=f;
1992 f= *pf++; f2sum+=f*f; fsum+=f;
1993 f= *pf++; f2sum+=f*f; fsum+=f;
1994 f= *pf++; f2sum+=f*f; fsum+=f;
1995 f= *pf++; f2sum+=f*f; fsum+=f;
1996 f= *pf++; f2sum+=f*f; fsum+=f;
1997 f= *pf; f2sum+=f*f; fsum+=f;
1998
1999 pf=f_img[y_f-1]+xm_f;
2000 f= *pf++; f2sum+=f*f; fsum+=f;
2001 f= *pf++; f2sum+=f*f; fsum+=f;
2002 f= *pf++; f2sum+=f*f; fsum+=f;
2003 f= *pf++; f2sum+=f*f; fsum+=f;
2004 f= *pf++; f2sum+=f*f; fsum+=f;
2005 f= *pf++; f2sum+=f*f; fsum+=f;
2006 f= *pf++; f2sum+=f*f; fsum+=f;
2007 f= *pf++; f2sum+=f*f; fsum+=f;
2008 f= *pf++; f2sum+=f*f; fsum+=f;
2009 f= *pf++; f2sum+=f*f; fsum+=f;
2010 f= *pf; f2sum+=f*f; fsum+=f;
2011
2012 pf=f_img[y_f]+xm_f;
2013 f= *pf++; f2sum+=f*f; fsum+=f;
2014 f= *pf++; f2sum+=f*f; fsum+=f;
2015 f= *pf++; f2sum+=f*f; fsum+=f;
2016 f= *pf++; f2sum+=f*f; fsum+=f;
2017 f= *pf++; f2sum+=f*f; fsum+=f;
2018 f= *pf++; f2sum+=f*f; fsum+=f;
2019 f= *pf++; f2sum+=f*f; fsum+=f;
2020 f= *pf++; f2sum+=f*f; fsum+=f;
2021 f= *pf++; f2sum+=f*f; fsum+=f;
2022 f= *pf++; f2sum+=f*f; fsum+=f;
2023 f= *pf; f2sum+=f*f; fsum+=f;
2024
2025 pf=f_img[y_f+1]+xm_f;
2026 f= *pf++; f2sum+=f*f; fsum+=f;
2027 f= *pf++; f2sum+=f*f; fsum+=f;
2028 f= *pf++; f2sum+=f*f; fsum+=f;
2029 f= *pf++; f2sum+=f*f; fsum+=f;
2030 f= *pf++; f2sum+=f*f; fsum+=f;
2031 f= *pf++; f2sum+=f*f; fsum+=f;
2032 f= *pf++; f2sum+=f*f; fsum+=f;
2033 f= *pf++; f2sum+=f*f; fsum+=f;
2034 f= *pf++; f2sum+=f*f; fsum+=f;
2035 f= *pf++; f2sum+=f*f; fsum+=f;
2036 f= *pf; f2sum+=f*f; fsum+=f;
2037
2038 pf=f_img[y_f+2]+xm_f;
2039 f= *pf++; f2sum+=f*f; fsum+=f;
2040 f= *pf++; f2sum+=f*f; fsum+=f;
2041 f= *pf++; f2sum+=f*f; fsum+=f;
2042 f= *pf++; f2sum+=f*f; fsum+=f;
2043 f= *pf++; f2sum+=f*f; fsum+=f;
2044 f= *pf++; f2sum+=f*f; fsum+=f;
2045 f= *pf++; f2sum+=f*f; fsum+=f;
2046 f= *pf++; f2sum+=f*f; fsum+=f;
2047 f= *pf++; f2sum+=f*f; fsum+=f;
2048 f= *pf++; f2sum+=f*f; fsum+=f;
2049 f= *pf; f2sum+=f*f; fsum+=f;
2050
2051 pf=f_img[y_f+3]+xm_f;
2052 f= *pf++; f2sum+=f*f; fsum+=f;
2053 f= *pf++; f2sum+=f*f; fsum+=f;
2054 f= *pf++; f2sum+=f*f; fsum+=f;
2055 f= *pf++; f2sum+=f*f; fsum+=f;
2056 f= *pf++; f2sum+=f*f; fsum+=f;
2057 f= *pf++; f2sum+=f*f; fsum+=f;
2058 f= *pf++; f2sum+=f*f; fsum+=f;
2059 f= *pf++; f2sum+=f*f; fsum+=f;
2060 f= *pf++; f2sum+=f*f; fsum+=f;
2061 f= *pf++; f2sum+=f*f; fsum+=f;
2062 f= *pf; f2sum+=f*f; fsum+=f;
2063
2064 pf=f_img[y_f+4]+xm_f;
2065 f= *pf++; f2sum+=f*f; fsum+=f;
2066 f= *pf++; f2sum+=f*f; fsum+=f;
2067 f= *pf++; f2sum+=f*f; fsum+=f;
2068 f= *pf++; f2sum+=f*f; fsum+=f;
2069 f= *pf++; f2sum+=f*f; fsum+=f;
2070 f= *pf++; f2sum+=f*f; fsum+=f;
2071 f= *pf++; f2sum+=f*f; fsum+=f;
2072 f= *pf++; f2sum+=f*f; fsum+=f;
2073 f= *pf++; f2sum+=f*f; fsum+=f;
2074 f= *pf++; f2sum+=f*f; fsum+=f;
2075 f= *pf; f2sum+=f*f; fsum+=f;
2076
2077 pf=f_img[y_f+5]+xm_f;
2078 f= *pf++; f2sum+=f*f; fsum+=f;
2079 f= *pf++; f2sum+=f*f; fsum+=f;
2080 f= *pf++; f2sum+=f*f; fsum+=f;
2081 f= *pf++; f2sum+=f*f; fsum+=f;
2082 f= *pf++; f2sum+=f*f; fsum+=f;
2083 f= *pf++; f2sum+=f*f; fsum+=f;
2084 f= *pf++; f2sum+=f*f; fsum+=f;
2085 f= *pf++; f2sum+=f*f; fsum+=f;
2086 f= *pf++; f2sum+=f*f; fsum+=f;
2087 f= *pf++; f2sum+=f*f; fsum+=f;
2088 f= *pf; f2sum+=f*f; fsum+=f;
2089
2090 *sum=fsum;
2091 den=(121.0f*f2sum-fsum*fsum);
2092 *recip= (float) ((den!=0.0)?1.0/den:0.0);
2093 }
2094
db_SignedSquareNormCorr11x11_PreAlign_f(float * patch,const float * const * f_img,int x_f,int y_f,float * sum,float * recip)2095 inline void db_SignedSquareNormCorr11x11_PreAlign_f(float *patch,const float * const *f_img,int x_f,int y_f,float *sum,float *recip)
2096 {
2097 const float *pf;
2098 float den,f,f2sum,fsum;
2099 int xm_f;
2100
2101 xm_f=x_f-5;
2102
2103 pf=f_img[y_f-5]+xm_f;
2104 f= *pf++; f2sum=f*f; fsum=f; (*patch++)=f;
2105 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2106 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2107 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2108 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2109 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2110 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2111 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2112 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2113 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2114 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
2115
2116 pf=f_img[y_f-4]+xm_f;
2117 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2118 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2119 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2120 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2121 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2122 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2123 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2124 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2125 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2126 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2127 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
2128
2129 pf=f_img[y_f-3]+xm_f;
2130 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2131 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2132 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2133 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2134 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2135 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2136 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2137 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2138 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2139 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2140 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
2141
2142 pf=f_img[y_f-2]+xm_f;
2143 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2144 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2145 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2146 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2147 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2148 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2149 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2150 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2151 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2152 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2153 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
2154
2155 pf=f_img[y_f-1]+xm_f;
2156 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2157 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2158 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2159 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2160 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2161 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2162 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2163 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2164 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2165 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2166 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
2167
2168 pf=f_img[y_f]+xm_f;
2169 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2170 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2171 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2172 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2173 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2174 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2175 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2176 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2177 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2178 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2179 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
2180
2181 pf=f_img[y_f+1]+xm_f;
2182 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2183 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2184 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2185 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2186 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2187 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2188 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2189 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2190 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2191 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2192 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
2193
2194 pf=f_img[y_f+2]+xm_f;
2195 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2196 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2197 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2198 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2199 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2200 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2201 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2202 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2203 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2204 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2205 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
2206
2207 pf=f_img[y_f+3]+xm_f;
2208 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2209 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2210 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2211 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2212 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2213 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2214 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2215 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2216 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2217 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2218 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
2219
2220 pf=f_img[y_f+4]+xm_f;
2221 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2222 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2223 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2224 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2225 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2226 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2227 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2228 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2229 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2230 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2231 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
2232
2233 pf=f_img[y_f+5]+xm_f;
2234 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2235 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2236 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2237 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2238 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2239 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2240 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2241 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2242 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2243 f= *pf++; f2sum+=f*f; fsum+=f; (*patch++)=f;
2244 f= *pf; f2sum+=f*f; fsum+=f; (*patch++)=f;
2245
2246 (*patch++)=0.0; (*patch++)=0.0; (*patch++)=0.0; (*patch++)=0.0; (*patch++)=0.0;
2247 (*patch++)=0.0; (*patch++)=0.0;
2248
2249 *sum=fsum;
2250 den=(121.0f*f2sum-fsum*fsum);
2251 *recip= (float) ((den!=0.0)?1.0/den:0.0);
2252 }
2253
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)2254 inline float db_SignedSquareNormCorr11x11_Post_f(float **f_img,float **g_img,int x_f,int y_f,int x_g,int y_g,
2255 float fsum_gsum,float f_recip_g_recip)
2256 {
2257 float *pf,*pg;
2258 float fgsum,fg_corr;
2259 int xm_f,xm_g;
2260
2261 xm_f=x_f-5;
2262 xm_g=x_g-5;
2263
2264 pf=f_img[y_f-5]+xm_f; pg=g_img[y_g-5]+xm_g;
2265 fgsum=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2266 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2267 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2268 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
2269
2270 pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g;
2271 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2272 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2273 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2274 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
2275
2276 pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g;
2277 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2278 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2279 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2280 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
2281
2282 pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g;
2283 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2284 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2285 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2286 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
2287
2288 pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g;
2289 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2290 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2291 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2292 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
2293
2294 pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g;
2295 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2296 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2297 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2298 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
2299
2300 pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g;
2301 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2302 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2303 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2304 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
2305
2306 pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g;
2307 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2308 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2309 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2310 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
2311
2312 pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g;
2313 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2314 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2315 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2316 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
2317
2318 pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g;
2319 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2320 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2321 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2322 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
2323
2324 pf=f_img[y_f+5]+xm_f; pg=g_img[y_g+5]+xm_g;
2325 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2326 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2327 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++); fgsum+=(*pf++)*(*pg++);
2328 fgsum+=(*pf++)*(*pg++); fgsum+=(*pf)*(*pg);
2329
2330 fg_corr=121.0f*fgsum-fsum_gsum;
2331 if(fg_corr>=0.0) return(fg_corr*fg_corr*f_recip_g_recip);
2332 return(-fg_corr*fg_corr*f_recip_g_recip);
2333 }
2334
db_SignedSquareNormCorr11x11Aligned_Post_f(const float * f_patch,const float * g_patch,float fsum_gsum,float f_recip_g_recip)2335 inline float db_SignedSquareNormCorr11x11Aligned_Post_f(const float *f_patch,const float *g_patch,float fsum_gsum,float f_recip_g_recip)
2336 {
2337 float fgsum,fg_corr;
2338
2339 fgsum=db_ScalarProduct128Aligned16_f(f_patch,g_patch);
2340
2341 fg_corr=121.0f*fgsum-fsum_gsum;
2342 if(fg_corr>=0.0) return(fg_corr*fg_corr*f_recip_g_recip);
2343 return(-fg_corr*fg_corr*f_recip_g_recip);
2344 }
2345
db_SignedSquareNormCorr15x15_f(float ** f_img,float ** g_img,int x_f,int y_f,int x_g,int y_g)2346 inline float db_SignedSquareNormCorr15x15_f(float **f_img,float **g_img,int x_f,int y_f,int x_g,int y_g)
2347 {
2348 float *pf,*pg;
2349 float f,g,fgsum,f2sum,g2sum,fsum,gsum,fg_corr,den;
2350 int xm_f,xm_g;
2351
2352 xm_f=x_f-7;
2353 xm_g=x_g-7;
2354 fgsum=0.0; f2sum=0.0; g2sum=0.0; fsum=0.0; gsum=0.0;
2355
2356 pf=f_img[y_f-7]+xm_f; pg=g_img[y_g-7]+xm_g;
2357 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2358 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2359 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2360 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2361 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2362 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2363 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2364 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2365 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2366 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2367 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2368 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2369 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2370 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2371 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2372
2373 pf=f_img[y_f-6]+xm_f; pg=g_img[y_g-6]+xm_g;
2374 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2375 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2376 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2377 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2378 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2379 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2380 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2381 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2382 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2383 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2384 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2385 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2386 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2387 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2388 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2389
2390 pf=f_img[y_f-5]+xm_f; pg=g_img[y_g-5]+xm_g;
2391 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2392 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2393 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2394 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2395 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2396 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2397 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2398 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2399 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2400 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2401 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2402 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2403 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2404 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2405 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2406
2407 pf=f_img[y_f-4]+xm_f; pg=g_img[y_g-4]+xm_g;
2408 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2409 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2410 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2411 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2412 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2413 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2414 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2415 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2416 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2417 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2418 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2419 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2420 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2421 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2422 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2423
2424 pf=f_img[y_f-3]+xm_f; pg=g_img[y_g-3]+xm_g;
2425 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2426 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2427 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2428 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2429 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2430 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2431 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2432 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2433 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2434 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2435 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2436 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2437 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2438 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2439 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2440
2441 pf=f_img[y_f-2]+xm_f; pg=g_img[y_g-2]+xm_g;
2442 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2443 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2444 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2445 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2446 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2447 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2448 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2449 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2450 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2451 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2452 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2453 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2454 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2455 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2456 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2457
2458 pf=f_img[y_f-1]+xm_f; pg=g_img[y_g-1]+xm_g;
2459 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2460 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2461 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2462 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2463 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2464 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2465 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2466 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2467 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2468 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2469 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2470 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2471 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2472 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2473 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2474
2475 pf=f_img[y_f]+xm_f; pg=g_img[y_g]+xm_g;
2476 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2477 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2478 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2479 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2480 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2481 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2482 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2483 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2484 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2485 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2486 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2487 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2488 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2489 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2490 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2491
2492 pf=f_img[y_f+1]+xm_f; pg=g_img[y_g+1]+xm_g;
2493 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2494 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2495 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2496 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2497 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2498 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2499 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2500 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2501 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2502 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2503 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2504 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2505 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2506 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2507 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2508
2509 pf=f_img[y_f+2]+xm_f; pg=g_img[y_g+2]+xm_g;
2510 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2511 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2512 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2513 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2514 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2515 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2516 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2517 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2518 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2519 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2520 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2521 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2522 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2523 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2524 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2525
2526 pf=f_img[y_f+3]+xm_f; pg=g_img[y_g+3]+xm_g;
2527 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2528 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2529 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2530 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2531 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2532 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2533 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2534 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2535 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2536 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2537 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2538 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2539 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2540 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2541 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2542
2543 pf=f_img[y_f+4]+xm_f; pg=g_img[y_g+4]+xm_g;
2544 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2545 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2546 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2547 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2548 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2549 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2550 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2551 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2552 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2553 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2554 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2555 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2556 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2557 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2558 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2559
2560 pf=f_img[y_f+5]+xm_f; pg=g_img[y_g+5]+xm_g;
2561 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2562 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2563 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2564 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2565 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2566 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2567 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2568 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2569 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2570 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2571 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2572 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2573 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2574 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2575 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2576
2577 pf=f_img[y_f+6]+xm_f; pg=g_img[y_g+6]+xm_g;
2578 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2579 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2580 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2581 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2582 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2583 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2584 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2585 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2586 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2587 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2588 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2589 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2590 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2591 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2592 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2593
2594 pf=f_img[y_f+7]+xm_f; pg=g_img[y_g+7]+xm_g;
2595 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2596 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2597 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2598 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2599 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2600 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2601 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2602 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2603 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2604 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2605 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2606 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2607 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2608 f= *pf++; g= *pg++; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2609 f= *pf; g= *pg; fgsum+=f*g; f2sum+=f*f; g2sum+=g*g; fsum+=f; gsum+=g;
2610
2611 fg_corr=225.0f*fgsum-fsum*gsum;
2612 den=(225.0f*f2sum-fsum*fsum)*(225.0f*g2sum-gsum*gsum);
2613 if(den!=0.0)
2614 {
2615 if(fg_corr>=0.0) return(fg_corr*fg_corr/den);
2616 return(-fg_corr*fg_corr/den);
2617 }
2618 return(0.0);
2619 }
2620
db_AllocBuckets_f(int nr_h,int nr_v,int bd)2621 db_Bucket_f** db_AllocBuckets_f(int nr_h,int nr_v,int bd)
2622 {
2623 int i,j;
2624 db_Bucket_f **bp,*b;
2625
2626 b=new db_Bucket_f [(nr_h+2)*(nr_v+2)];
2627 bp=new db_Bucket_f* [(nr_v+2)];
2628 bp=bp+1;
2629 for(i= -1;i<=nr_v;i++)
2630 {
2631 bp[i]=b+1+(nr_h+2)*(i+1);
2632 for(j= -1;j<=nr_h;j++)
2633 {
2634 bp[i][j].ptr=new db_PointInfo_f [bd];
2635 }
2636 }
2637
2638 return(bp);
2639 }
2640
db_AllocBuckets_u(int nr_h,int nr_v,int bd)2641 db_Bucket_u** db_AllocBuckets_u(int nr_h,int nr_v,int bd)
2642 {
2643 int i,j;
2644 db_Bucket_u **bp,*b;
2645
2646 b=new db_Bucket_u [(nr_h+2)*(nr_v+2)];
2647 bp=new db_Bucket_u* [(nr_v+2)];
2648 bp=bp+1;
2649 for(i= -1;i<=nr_v;i++)
2650 {
2651 bp[i]=b+1+(nr_h+2)*(i+1);
2652 for(j= -1;j<=nr_h;j++)
2653 {
2654 bp[i][j].ptr=new db_PointInfo_u [bd];
2655 }
2656 }
2657
2658 return(bp);
2659 }
2660
db_FreeBuckets_f(db_Bucket_f ** bp,int nr_h,int nr_v)2661 void db_FreeBuckets_f(db_Bucket_f **bp,int nr_h,int nr_v)
2662 {
2663 int i,j;
2664
2665 for(i= -1;i<=nr_v;i++) for(j= -1;j<=nr_h;j++)
2666 {
2667 delete [] bp[i][j].ptr;
2668 }
2669 delete [] (bp[-1]-1);
2670 delete [] (bp-1);
2671 }
2672
db_FreeBuckets_u(db_Bucket_u ** bp,int nr_h,int nr_v)2673 void db_FreeBuckets_u(db_Bucket_u **bp,int nr_h,int nr_v)
2674 {
2675 int i,j;
2676
2677 for(i= -1;i<=nr_v;i++) for(j= -1;j<=nr_h;j++)
2678 {
2679 delete [] bp[i][j].ptr;
2680 }
2681 delete [] (bp[-1]-1);
2682 delete [] (bp-1);
2683 }
2684
db_EmptyBuckets_f(db_Bucket_f ** bp,int nr_h,int nr_v)2685 void db_EmptyBuckets_f(db_Bucket_f **bp,int nr_h,int nr_v)
2686 {
2687 int i,j;
2688 for(i= -1;i<=nr_v;i++) for(j= -1;j<=nr_h;j++) bp[i][j].nr=0;
2689 }
2690
db_EmptyBuckets_u(db_Bucket_u ** bp,int nr_h,int nr_v)2691 void db_EmptyBuckets_u(db_Bucket_u **bp,int nr_h,int nr_v)
2692 {
2693 int i,j;
2694 for(i= -1;i<=nr_v;i++) for(j= -1;j<=nr_h;j++) bp[i][j].nr=0;
2695 }
2696
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)2697 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)
2698 {
2699 int i,xi,yi,xpos,ypos,nr;
2700 db_Bucket_f *br;
2701 db_PointInfo_f *pir;
2702
2703 db_EmptyBuckets_f(bp,nr_h,nr_v);
2704 for(i=0;i<nr_corners;i++)
2705 {
2706 xi=(int) x[i];
2707 yi=(int) y[i];
2708 xpos=xi/bw;
2709 ypos=yi/bh;
2710 if(xpos>=0 && xpos<nr_h && ypos>=0 && ypos<nr_v)
2711 {
2712 br=&bp[ypos][xpos];
2713 nr=br->nr;
2714 if(nr<bd)
2715 {
2716 pir=&(br->ptr[nr]);
2717 pir->x=xi;
2718 pir->y=yi;
2719 pir->id=i;
2720 pir->pir=0;
2721 pir->patch=patch_space;
2722 br->nr=nr+1;
2723
2724 db_SignedSquareNormCorr11x11_PreAlign_f(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip));
2725 patch_space+=128;
2726 }
2727 }
2728 }
2729 return(patch_space);
2730 }
2731
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)2732 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)
2733 {
2734 int i,xi,yi,xpos,ypos,nr;
2735 db_Bucket_u *br;
2736 db_PointInfo_u *pir;
2737
2738 db_EmptyBuckets_u(bp,nr_h,nr_v);
2739 for(i=0;i<nr_corners;i++)
2740 {
2741 xi=(int)db_roundi(x[i]);
2742 yi=(int)db_roundi(y[i]);
2743 xpos=xi/bw;
2744 ypos=yi/bh;
2745 if(xpos>=0 && xpos<nr_h && ypos>=0 && ypos<nr_v)
2746 {
2747 br=&bp[ypos][xpos];
2748 nr=br->nr;
2749 if(nr<bd)
2750 {
2751 pir=&(br->ptr[nr]);
2752 pir->x=xi;
2753 pir->y=yi;
2754 pir->id=i;
2755 pir->pir=0;
2756 pir->patch=patch_space;
2757 br->nr=nr+1;
2758
2759 if(use_21)
2760 {
2761 db_SignedSquareNormCorr21x21_PreAlign_u(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip));
2762 patch_space+=512;
2763 }
2764 else
2765 {
2766 if(!use_smaller_matching_window)
2767 {
2768 db_SignedSquareNormCorr11x11_PreAlign_u(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip));
2769 patch_space+=128;
2770 }
2771 else
2772 {
2773 db_SignedSquareNormCorr5x5_PreAlign_u(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip));
2774 patch_space+=32;
2775 }
2776 }
2777 }
2778 }
2779 }
2780 return(patch_space);
2781 }
2782
2783
2784
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])2785 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])
2786 {
2787 int i,xi,yi,xpos,ypos,nr,wxi,wyi;
2788 db_Bucket_f *br;
2789 db_PointInfo_f *pir;
2790 double xd[2],wx[2];
2791
2792 db_EmptyBuckets_f(bp,nr_h,nr_v);
2793 for(i=0;i<nr_corners;i++)
2794 {
2795 xd[0]=x[i];
2796 xd[1]=y[i];
2797 xi=(int) xd[0];
2798 yi=(int) xd[1];
2799 db_ImageHomographyInhomogenous(wx,H,xd);
2800 wxi=(int) wx[0];
2801 wyi=(int) wx[1];
2802
2803 xpos=((wxi+bw)/bw)-1;
2804 ypos=((wyi+bh)/bh)-1;
2805 if(xpos>= -1 && xpos<=nr_h && ypos>= -1 && ypos<=nr_v)
2806 {
2807 br=&bp[ypos][xpos];
2808 nr=br->nr;
2809 if(nr<bd)
2810 {
2811 pir=&(br->ptr[nr]);
2812 pir->x=wxi;
2813 pir->y=wyi;
2814 pir->id=i;
2815 pir->pir=0;
2816 pir->patch=patch_space;
2817 br->nr=nr+1;
2818
2819 db_SignedSquareNormCorr11x11_PreAlign_f(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip));
2820 patch_space+=128;
2821 }
2822 }
2823 }
2824 return(patch_space);
2825 }
2826
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])2827 short* db_FillBucketsPrewarped_u(short *patch_space,const unsigned char * const *f_img,db_Bucket_u **bp,
2828 int bw,int bh,int nr_h,int nr_v,int bd,const double *x,const double *y,
2829 int nr_corners,const double H[9])
2830 {
2831 int i,xi,yi,xpos,ypos,nr,wxi,wyi;
2832 db_Bucket_u *br;
2833 db_PointInfo_u *pir;
2834 double xd[2],wx[2];
2835
2836 db_EmptyBuckets_u(bp,nr_h,nr_v);
2837 for(i=0;i<nr_corners;i++)
2838 {
2839 xd[0]=x[i];
2840 xd[1]=y[i];
2841 xi=(int) db_roundi(xd[0]);
2842 yi=(int) db_roundi(xd[1]);
2843 db_ImageHomographyInhomogenous(wx,H,xd);
2844 wxi=(int) wx[0];
2845 wyi=(int) wx[1];
2846
2847 xpos=((wxi+bw)/bw)-1;
2848 ypos=((wyi+bh)/bh)-1;
2849 if(xpos>= -1 && xpos<=nr_h && ypos>= -1 && ypos<=nr_v)
2850 {
2851 br=&bp[ypos][xpos];
2852 nr=br->nr;
2853 if(nr<bd)
2854 {
2855 pir=&(br->ptr[nr]);
2856 pir->x=wxi;
2857 pir->y=wyi;
2858 pir->id=i;
2859 pir->pir=0;
2860 pir->patch=patch_space;
2861 br->nr=nr+1;
2862
2863 db_SignedSquareNormCorr11x11_PreAlign_u(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip));
2864 patch_space+=128;
2865 }
2866 }
2867 }
2868 return(patch_space);
2869 }
2870
2871
2872
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)2873 short* db_FillBucketsPrewarpedAffine_u(short *patch_space,const unsigned char * const *f_img,db_Bucket_u **bp,
2874 int bw,int bh,int nr_h,int nr_v,int bd,const double *x,const double *y,
2875 int nr_corners,const double H[9],const double Hinv[9],const int warpboundsp[4],
2876 int affine)
2877 {
2878 int i,xi,yi,xpos,ypos,nr,wxi,wyi;
2879 db_Bucket_u *br;
2880 db_PointInfo_u *pir;
2881 double xd[2],wx[2];
2882
2883 db_EmptyBuckets_u(bp,nr_h,nr_v);
2884 for(i=0;i<nr_corners;i++)
2885 {
2886 xd[0]=x[i];
2887 xd[1]=y[i];
2888 xi=(int) db_roundi(xd[0]);
2889 yi=(int) db_roundi(xd[1]);
2890 db_ImageHomographyInhomogenous(wx,H,xd);
2891 wxi=(int) wx[0];
2892 wyi=(int) wx[1];
2893
2894 xpos=((wxi+bw)/bw)-1;
2895 ypos=((wyi+bh)/bh)-1;
2896
2897
2898 if (xpos>= -1 && xpos<=nr_h && ypos>= -1 && ypos<=nr_v)
2899 {
2900 if( xi>warpboundsp[0] && xi<warpboundsp[1] && yi>warpboundsp[2] && yi<warpboundsp[3])
2901 {
2902
2903 br=&bp[ypos][xpos];
2904 nr=br->nr;
2905 if(nr<bd)
2906 {
2907 pir=&(br->ptr[nr]);
2908 pir->x=wxi;
2909 pir->y=wyi;
2910 pir->id=i;
2911 pir->pir=0;
2912 pir->patch=patch_space;
2913 br->nr=nr+1;
2914
2915 db_SignedSquareNormCorr11x11_PreAlign_AffinePatchWarp_u(patch_space,f_img,xi,yi,&(pir->sum),&(pir->recip),Hinv,affine);
2916 patch_space+=128;
2917 }
2918 }
2919 }
2920 }
2921 return(patch_space);
2922 }
2923
2924
2925
db_MatchPointPair_f(db_PointInfo_f * pir_l,db_PointInfo_f * pir_r,unsigned long kA,unsigned long kB)2926 inline void db_MatchPointPair_f(db_PointInfo_f *pir_l,db_PointInfo_f *pir_r,
2927 unsigned long kA,unsigned long kB)
2928 {
2929 int x_l,y_l,x_r,y_r,xm,ym;
2930 double score;
2931
2932 x_l=pir_l->x;
2933 y_l=pir_l->y;
2934 x_r=pir_r->x;
2935 y_r=pir_r->y;
2936 xm=x_l-x_r;
2937 ym=y_l-y_r;
2938 /*Check if disparity is within the maximum disparity
2939 with the formula xm^2*256+ym^2*kA<kB
2940 where kA=256*w^2/h^2
2941 and kB=256*max_disp^2*w^2*/
2942 if(((xm*xm)<<8)+ym*ym*kA<kB)
2943 {
2944 /*Correlate*/
2945 score=db_SignedSquareNormCorr11x11Aligned_Post_f(pir_l->patch,pir_r->patch,
2946 (pir_l->sum)*(pir_r->sum),
2947 (pir_l->recip)*(pir_r->recip));
2948
2949 if((!(pir_l->pir)) || (score>pir_l->s))
2950 {
2951 /*Update left corner*/
2952 pir_l->s=score;
2953 pir_l->pir=pir_r;
2954 }
2955 if((!(pir_r->pir)) || (score>pir_r->s))
2956 {
2957 /*Update right corner*/
2958 pir_r->s=score;
2959 pir_r->pir=pir_l;
2960 }
2961 }
2962 }
2963
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)2964 inline void db_MatchPointPair_u(db_PointInfo_u *pir_l,db_PointInfo_u *pir_r,
2965 unsigned long kA,unsigned long kB, unsigned int rect_window,bool use_smaller_matching_window, int use_21)
2966 {
2967 int xm,ym;
2968 double score;
2969 bool compute_score;
2970
2971
2972 if( rect_window )
2973 compute_score = ((unsigned)db_absi(pir_l->x - pir_r->x)<kA && (unsigned)db_absi(pir_l->y - pir_r->y)<kB);
2974 else
2975 { /*Check if disparity is within the maximum disparity
2976 with the formula xm^2*256+ym^2*kA<kB
2977 where kA=256*w^2/h^2
2978 and kB=256*max_disp^2*w^2*/
2979 xm= pir_l->x - pir_r->x;
2980 ym= pir_l->y - pir_r->y;
2981 compute_score = ((xm*xm)<<8)+ym*ym*kA < kB;
2982 }
2983
2984 if ( compute_score )
2985 {
2986 if(use_21)
2987 {
2988 score=db_SignedSquareNormCorr21x21Aligned_Post_s(pir_l->patch,pir_r->patch,
2989 (pir_l->sum)*(pir_r->sum),
2990 (pir_l->recip)*(pir_r->recip));
2991 }
2992 else
2993 {
2994 /*Correlate*/
2995 if(!use_smaller_matching_window)
2996 {
2997 score=db_SignedSquareNormCorr11x11Aligned_Post_s(pir_l->patch,pir_r->patch,
2998 (pir_l->sum)*(pir_r->sum),
2999 (pir_l->recip)*(pir_r->recip));
3000 }
3001 else
3002 {
3003 score=db_SignedSquareNormCorr5x5Aligned_Post_s(pir_l->patch,pir_r->patch,
3004 (pir_l->sum)*(pir_r->sum),
3005 (pir_l->recip)*(pir_r->recip));
3006 }
3007 }
3008
3009 if((!(pir_l->pir)) || (score>pir_l->s))
3010 {
3011 /*Update left corner*/
3012 pir_l->s=score;
3013 pir_l->pir=pir_r;
3014 }
3015 if((!(pir_r->pir)) || (score>pir_r->s))
3016 {
3017 /*Update right corner*/
3018 pir_r->s=score;
3019 pir_r->pir=pir_l;
3020 }
3021 }
3022 }
3023
db_MatchPointAgainstBucket_f(db_PointInfo_f * pir_l,db_Bucket_f * b_r,unsigned long kA,unsigned long kB)3024 inline void db_MatchPointAgainstBucket_f(db_PointInfo_f *pir_l,db_Bucket_f *b_r,
3025 unsigned long kA,unsigned long kB)
3026 {
3027 int p_r,nr;
3028 db_PointInfo_f *pir_r;
3029
3030 nr=b_r->nr;
3031 pir_r=b_r->ptr;
3032 for(p_r=0;p_r<nr;p_r++) db_MatchPointPair_f(pir_l,pir_r+p_r,kA,kB);
3033 }
3034
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)3035 inline void db_MatchPointAgainstBucket_u(db_PointInfo_u *pir_l,db_Bucket_u *b_r,
3036 unsigned long kA,unsigned long kB,int rect_window, bool use_smaller_matching_window, int use_21)
3037 {
3038 int p_r,nr;
3039 db_PointInfo_u *pir_r;
3040
3041 nr=b_r->nr;
3042 pir_r=b_r->ptr;
3043
3044 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);
3045
3046 }
3047
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)3048 void db_MatchBuckets_f(db_Bucket_f **bp_l,db_Bucket_f **bp_r,int nr_h,int nr_v,
3049 unsigned long kA,unsigned long kB)
3050 {
3051 int i,j,k,a,b,br_nr;
3052 db_Bucket_f *br;
3053 db_PointInfo_f *pir_l;
3054
3055 /*For all buckets*/
3056 for(i=0;i<nr_v;i++) for(j=0;j<nr_h;j++)
3057 {
3058 br=&bp_l[i][j];
3059 br_nr=br->nr;
3060 /*For all points in bucket*/
3061 for(k=0;k<br_nr;k++)
3062 {
3063 pir_l=br->ptr+k;
3064 for(a=i-1;a<=i+1;a++)
3065 {
3066 for(b=j-1;b<=j+1;b++)
3067 {
3068 db_MatchPointAgainstBucket_f(pir_l,&bp_r[a][b],kA,kB);
3069 }
3070 }
3071 }
3072 }
3073 }
3074
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)3075 void db_MatchBuckets_u(db_Bucket_u **bp_l,db_Bucket_u **bp_r,int nr_h,int nr_v,
3076 unsigned long kA,unsigned long kB,int rect_window,bool use_smaller_matching_window, int use_21)
3077 {
3078 int i,j,k,a,b,br_nr;
3079 db_Bucket_u *br;
3080 db_PointInfo_u *pir_l;
3081
3082 /*For all buckets*/
3083 for(i=0;i<nr_v;i++) for(j=0;j<nr_h;j++)
3084 {
3085 br=&bp_l[i][j];
3086 br_nr=br->nr;
3087 /*For all points in bucket*/
3088 for(k=0;k<br_nr;k++)
3089 {
3090 pir_l=br->ptr+k;
3091 for(a=i-1;a<=i+1;a++)
3092 {
3093 for(b=j-1;b<=j+1;b++)
3094 {
3095 db_MatchPointAgainstBucket_u(pir_l,&bp_r[a][b],kA,kB,rect_window,use_smaller_matching_window, use_21);
3096 }
3097 }
3098 }
3099 }
3100 }
3101
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)3102 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)
3103 {
3104 int i,j,k,br_nr;
3105 unsigned long count;
3106 db_Bucket_f *br;
3107 db_PointInfo_f *pir,*pir2;
3108
3109 count=0;
3110 /*For all buckets*/
3111 for(i=0;i<nr_v;i++) for(j=0;j<nr_h;j++)
3112 {
3113 br=&bp_l[i][j];
3114 br_nr=br->nr;
3115 /*For all points in bucket*/
3116 for(k=0;k<br_nr;k++)
3117 {
3118 pir=br->ptr+k;
3119 pir2=pir->pir;
3120 if(pir2)
3121 {
3122 /*This point has a best match*/
3123 if((pir2->pir)==pir)
3124 {
3125 /*We have a mutually consistent match*/
3126 if(count<target)
3127 {
3128 id_l[count]=pir->id;
3129 id_r[count]=pir2->id;
3130 count++;
3131 }
3132 }
3133 }
3134 }
3135 }
3136 *nr_matches=count;
3137 }
3138
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)3139 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)
3140 {
3141 int i,j,k,br_nr;
3142 unsigned long count;
3143 db_Bucket_u *br;
3144 db_PointInfo_u *pir,*pir2;
3145
3146 count=0;
3147 /*For all buckets*/
3148 for(i=0;i<nr_v;i++) for(j=0;j<nr_h;j++)
3149 {
3150 br=&bp_l[i][j];
3151 br_nr=br->nr;
3152 /*For all points in bucket*/
3153 for(k=0;k<br_nr;k++)
3154 {
3155 pir=br->ptr+k;
3156 pir2=pir->pir;
3157 if(pir2)
3158 {
3159 /*This point has a best match*/
3160 if((pir2->pir)==pir)
3161 {
3162 /*We have a mutually consistent match*/
3163 if(count<target)
3164 {
3165 id_l[count]=pir->id;
3166 id_r[count]=pir2->id;
3167 count++;
3168 }
3169 }
3170 }
3171 }
3172 }
3173 *nr_matches=count;
3174 }
3175
db_Matcher_f()3176 db_Matcher_f::db_Matcher_f()
3177 {
3178 m_w=0; m_h=0;
3179 }
3180
~db_Matcher_f()3181 db_Matcher_f::~db_Matcher_f()
3182 {
3183 Clean();
3184 }
3185
Clean()3186 void db_Matcher_f::Clean()
3187 {
3188 if(m_w)
3189 {
3190 /*Free buckets*/
3191 db_FreeBuckets_f(m_bp_l,m_nr_h,m_nr_v);
3192 db_FreeBuckets_f(m_bp_r,m_nr_h,m_nr_v);
3193 /*Free space for patch layouts*/
3194 delete [] m_patch_space;
3195 }
3196 m_w=0; m_h=0;
3197 }
3198
Init(int im_width,int im_height,double max_disparity,int target_nr_corners)3199 unsigned long db_Matcher_f::Init(int im_width,int im_height,double max_disparity,int target_nr_corners)
3200 {
3201 Clean();
3202 m_w=im_width;
3203 m_h=im_height;
3204 m_bw=db_maxi(1,(int) (max_disparity*((double)im_width)));
3205 m_bh=db_maxi(1,(int) (max_disparity*((double)im_height)));
3206 m_nr_h=1+(im_width-1)/m_bw;
3207 m_nr_v=1+(im_height-1)/m_bh;
3208 m_bd=db_maxi(1,(int)(((double)target_nr_corners)*
3209 max_disparity*max_disparity));
3210 m_target=target_nr_corners;
3211 m_kA=(long)(256.0*((double)(m_w*m_w))/((double)(m_h*m_h)));
3212 m_kB=(long)(256.0*max_disparity*max_disparity*((double)(m_w*m_w)));
3213
3214 /*Alloc bucket structure*/
3215 m_bp_l=db_AllocBuckets_f(m_nr_h,m_nr_v,m_bd);
3216 m_bp_r=db_AllocBuckets_f(m_nr_h,m_nr_v,m_bd);
3217
3218 /*Alloc 16byte-aligned space for patch layouts*/
3219 m_patch_space=new float [2*(m_nr_h+2)*(m_nr_v+2)*m_bd*128+16];
3220 m_aligned_patch_space=db_AlignPointer_f(m_patch_space,16);
3221
3222 return(m_target);
3223 }
3224
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])3225 void db_Matcher_f::Match(const float * const *l_img,const float * const *r_img,
3226 const double *x_l,const double *y_l,int nr_l,const double *x_r,const double *y_r,int nr_r,
3227 int *id_l,int *id_r,int *nr_matches,const double H[9])
3228 {
3229 float *ps;
3230
3231 /*Insert the corners into bucket structure*/
3232 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);
3233 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);
3234 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);
3235
3236 /*Compute all the necessary match scores*/
3237 db_MatchBuckets_f(m_bp_l,m_bp_r,m_nr_h,m_nr_v,m_kA,m_kB);
3238
3239 /*Collect the correspondences*/
3240 db_CollectMatches_f(m_bp_l,m_nr_h,m_nr_v,m_target,id_l,id_r,nr_matches);
3241 }
3242
db_Matcher_u()3243 db_Matcher_u::db_Matcher_u()
3244 {
3245 m_w=0; m_h=0;
3246 m_rect_window = 0;
3247 m_bw=m_bh=m_nr_h=m_nr_v=m_bd=m_target=0;
3248 m_bp_l=m_bp_r=0;
3249 m_patch_space=m_aligned_patch_space=0;
3250 }
3251
db_Matcher_u(const db_Matcher_u & cm)3252 db_Matcher_u::db_Matcher_u(const db_Matcher_u& cm)
3253 {
3254 Init(cm.m_w, cm.m_h, cm.m_max_disparity, cm.m_target, cm.m_max_disparity_v);
3255 }
3256
operator =(const db_Matcher_u & cm)3257 db_Matcher_u& db_Matcher_u::operator= (const db_Matcher_u& cm)
3258 {
3259 if ( this == &cm ) return *this;
3260 Init(cm.m_w, cm.m_h, cm.m_max_disparity, cm.m_target, cm.m_max_disparity_v);
3261 return *this;
3262 }
3263
3264
~db_Matcher_u()3265 db_Matcher_u::~db_Matcher_u()
3266 {
3267 Clean();
3268 }
3269
Clean()3270 void db_Matcher_u::Clean()
3271 {
3272 if(m_w)
3273 {
3274 /*Free buckets*/
3275 db_FreeBuckets_u(m_bp_l,m_nr_h,m_nr_v);
3276 db_FreeBuckets_u(m_bp_r,m_nr_h,m_nr_v);
3277 /*Free space for patch layouts*/
3278 delete [] m_patch_space;
3279 }
3280 m_w=0; m_h=0;
3281 }
3282
3283
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)3284 unsigned long db_Matcher_u::Init(int im_width,int im_height,double max_disparity,int target_nr_corners,
3285 double max_disparity_v, bool use_smaller_matching_window, int use_21)
3286 {
3287 Clean();
3288 m_w=im_width;
3289 m_h=im_height;
3290 m_max_disparity=max_disparity;
3291 m_max_disparity_v=max_disparity_v;
3292
3293 if ( max_disparity_v != DB_DEFAULT_NO_DISPARITY )
3294 {
3295 m_rect_window = 1;
3296
3297 m_bw=db_maxi(1,(int)(max_disparity*((double)im_width)));
3298 m_bh=db_maxi(1,(int)(max_disparity_v*((double)im_height)));
3299
3300 m_bd=db_maxi(1,(int)(((double)target_nr_corners)*max_disparity*max_disparity_v));
3301
3302 m_kA=(int)(max_disparity*m_w);
3303 m_kB=(int)(max_disparity_v*m_h);
3304
3305 } else
3306 {
3307 m_bw=(int)db_maxi(1,(int)(max_disparity*((double)im_width)));
3308 m_bh=(int)db_maxi(1,(int)(max_disparity*((double)im_height)));
3309
3310 m_bd=db_maxi(1,(int)(((double)target_nr_corners)*max_disparity*max_disparity));
3311
3312 m_kA=(long)(256.0*((double)(m_w*m_w))/((double)(m_h*m_h)));
3313 m_kB=(long)(256.0*max_disparity*max_disparity*((double)(m_w*m_w)));
3314 }
3315
3316 m_nr_h=1+(im_width-1)/m_bw;
3317 m_nr_v=1+(im_height-1)/m_bh;
3318
3319 m_target=target_nr_corners;
3320
3321 /*Alloc bucket structure*/
3322 m_bp_l=db_AllocBuckets_u(m_nr_h,m_nr_v,m_bd);
3323 m_bp_r=db_AllocBuckets_u(m_nr_h,m_nr_v,m_bd);
3324
3325 m_use_smaller_matching_window = use_smaller_matching_window;
3326 m_use_21 = use_21;
3327
3328 if(m_use_21)
3329 {
3330 /*Alloc 64byte-aligned space for patch layouts*/
3331 m_patch_space=new short [2*(m_nr_h+2)*(m_nr_v+2)*m_bd*512+64];
3332 m_aligned_patch_space=db_AlignPointer_s(m_patch_space,64);
3333 }
3334 else
3335 {
3336 if(!m_use_smaller_matching_window)
3337 {
3338 /*Alloc 16byte-aligned space for patch layouts*/
3339 m_patch_space=new short [2*(m_nr_h+2)*(m_nr_v+2)*m_bd*128+16];
3340 m_aligned_patch_space=db_AlignPointer_s(m_patch_space,16);
3341 }
3342 else
3343 {
3344 /*Alloc 4byte-aligned space for patch layouts*/
3345 m_patch_space=new short [2*(m_nr_h+2)*(m_nr_v+2)*m_bd*32+4];
3346 m_aligned_patch_space=db_AlignPointer_s(m_patch_space,4);
3347 }
3348 }
3349
3350 return(m_target);
3351 }
3352
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)3353 void db_Matcher_u::Match(const unsigned char * const *l_img,const unsigned char * const *r_img,
3354 const double *x_l,const double *y_l,int nr_l,const double *x_r,const double *y_r,int nr_r,
3355 int *id_l,int *id_r,int *nr_matches,const double H[9],int affine)
3356 {
3357 short *ps;
3358
3359 /*Insert the corners into bucket structure*/
3360 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);
3361 if(H==0)
3362 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);
3363 else
3364 {
3365 if (affine)
3366 {
3367 double Hinv[9];
3368 db_InvertAffineTransform(Hinv,H);
3369 float r_w, c_w;
3370 float stretch_x[2];
3371 float stretch_y[2];
3372 AffineWarpPointOffset(r_w,c_w,Hinv, 5,5);
3373 stretch_x[0]=db_absf(c_w);stretch_y[0]=db_absf(r_w);
3374 AffineWarpPointOffset(r_w,c_w,Hinv, 5,-5);
3375 stretch_x[1]=db_absf(c_w);stretch_y[1]=db_absf(r_w);
3376 int max_stretxh_x=(int) (db_maxd(stretch_x[0],stretch_x[1]));
3377 int max_stretxh_y=(int) (db_maxd(stretch_y[0],stretch_y[1]));
3378 int warpbounds[4]={max_stretxh_x,m_w-1-max_stretxh_x,max_stretxh_y,m_h-1-max_stretxh_y};
3379
3380 for (int r=-5;r<=5;r++){
3381 for (int c=-5;c<=5;c++){
3382 AffineWarpPointOffset(r_w,c_w,Hinv,r,c);
3383 AffineWarpPoint_BL_LUT_y[r+5][c+5]=r_w;
3384 AffineWarpPoint_BL_LUT_x[r+5][c+5]=c_w;
3385
3386 AffineWarpPoint_NN_LUT_y[r+5][c+5]=db_roundi(r_w);
3387 AffineWarpPoint_NN_LUT_x[r+5][c+5]=db_roundi(c_w);
3388
3389 }
3390 }
3391
3392 db_FillBucketsPrewarpedAffine_u(ps,r_img,m_bp_r,m_bw,m_bh,m_nr_h,m_nr_v,m_bd,
3393 x_r,y_r,nr_r,H,Hinv,warpbounds,affine);
3394 }
3395 else
3396 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);
3397 }
3398
3399
3400 /*Compute all the necessary match scores*/
3401 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);
3402
3403 /*Collect the correspondences*/
3404 db_CollectMatches_u(m_bp_l,m_nr_h,m_nr_v,m_target,id_l,id_r,nr_matches);
3405 }
3406
IsAllocated()3407 int db_Matcher_u::IsAllocated()
3408 {
3409 return (int)(m_w != 0);
3410 }
3411