1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
21 //
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
25 //
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41 #include "_cvaux.h"
42 #include "_cvvm.h"
43 #include <stdlib.h>
44 #include <assert.h>
45
46
47 /*======================================================================================*/
48
49 CvStatus
icvDynamicCorrespond(int * first,int first_runs,int * second,int second_runs,int * first_corr,int * second_corr)50 icvDynamicCorrespond( int *first, /* first sequence of runs */
51 /* s0|w0|s1|w1|...|s(n-1)|w(n-1)|sn */
52 int first_runs, /* number of runs */
53 int *second, /* second sequence of runs */
54 int second_runs, int *first_corr, /* s0'|e0'|s1'|e1'|... */
55 int *second_corr )
56 {
57
58 float Pd, Fi, S;
59 float Occlusion;
60 float *costTable;
61 uchar *matchEdges;
62 int prev;
63 int curr;
64 int baseIndex;
65 int i, j;
66 int i_1, j_1;
67 int n;
68 int l_beg, r_beg, l_end, r_end, l_len, r_len;
69 int first_curr;
70 int second_curr;
71 int l_color, r_color;
72 int len_color;
73 float cost, cost1;
74 float min1, min2, min3;
75 float cmin;
76 uchar cpath;
77 int row_size;
78
79 /* Test arguments for errors */
80
81 if( (first == 0) ||
82 (first_runs < 1) ||
83 (second == 0) || (second_runs < 1) || (first_corr == 0) || (second_corr == 0) )
84
85 return CV_BADFACTOR_ERR;
86
87
88 Pd = 0.95f;
89 Fi = (float) CV_PI;
90 S = 1;
91
92 Occlusion = (float) log( Pd * Fi / ((1 - Pd) * sqrt( fabs( (CV_PI * 2) * (1. / S) ))));
93
94 costTable = (float *)cvAlloc( (first_runs + 1) * (second_runs + 1) * sizeof( float ));
95
96 if( costTable == 0 )
97 return CV_OUTOFMEM_ERR;
98
99 matchEdges = (uchar *)cvAlloc( (first_runs + 1) * (second_runs + 1) * sizeof( uchar ));
100
101 if( matchEdges == 0 )
102 {
103 cvFree( &costTable );
104 return CV_OUTOFMEM_ERR;
105 }
106
107 row_size = first_runs + 1;
108
109 /* ============= Fill costTable ============= */
110
111 costTable[0] = 0.0f;
112
113 /* Fill upper line in the cost Table */
114
115 prev = first[0];
116 curr = 2;
117
118 for( n = 0; n < first_runs; n++ )
119 {
120
121 l_end = first[curr];
122 curr += 2;
123 costTable[n + 1] = costTable[n] + Occlusion * (l_end - prev);
124 prev = l_end;
125
126 } /* for */
127
128 /* Fill lefter line in the cost Table */
129
130 prev = second[0];
131 curr = 2;
132 baseIndex = 0;
133
134 for( n = 0; n < second_runs; n++ )
135 {
136
137 l_end = second[curr];
138 curr += 2;
139 costTable[baseIndex + row_size] = costTable[baseIndex] + Occlusion * (l_end - prev);
140 baseIndex += row_size;
141 prev = l_end;
142
143 } /* for */
144
145 /* Count costs in the all rest cells */
146
147 first_curr = 0;
148 second_curr = 0;
149
150 for( i = 1; i <= first_runs; i++ )
151 {
152 for( j = 1; j <= second_runs; j++ )
153 {
154
155 first_curr = (i - 1) * 2;
156 second_curr = (j - 1) * 2;
157
158 l_beg = first[first_curr];
159 first_curr++;
160 l_color = first[first_curr];
161 first_curr++;
162 l_end = first[first_curr];
163 l_len = l_end - l_beg + 1;
164
165 r_beg = second[second_curr];
166 second_curr++;
167 r_color = second[second_curr];
168 second_curr++;
169 r_end = second[second_curr];
170 r_len = r_end - r_beg + 1;
171
172 i_1 = i - 1;
173 j_1 = j - 1;
174
175 if( r_len == l_len )
176 {
177 cost = 0;
178 }
179 else
180 {
181
182 if( r_len > l_len )
183 {
184 cost = (float) (r_len * r_len - l_len * l_len) * (1 / (r_len * l_len));
185 }
186 else
187 {
188 cost = (float) (l_len * l_len - r_len * r_len) * (1 / (r_len * l_len));
189 }
190 } /* if */
191
192 len_color = r_color - l_color;
193
194 cost1 = (float) ((len_color * len_color) >> 2);
195
196 min2 = costTable[i_1 + j * row_size] + Occlusion * l_len;
197
198 min3 = costTable[i + j_1 * row_size] + Occlusion * r_len;
199
200 min1 = costTable[i_1 + j_1 * row_size] + cost + (float) cost1;
201
202 if( min1 < min2 )
203 {
204
205 if( min1 < min3 )
206 {
207 cmin = min1;
208 cpath = 1;
209 }
210 else
211 {
212 cmin = min3;
213 cpath = 3;
214 } /* if */
215
216 }
217 else
218 {
219
220 if( min2 < min3 )
221 {
222 cmin = min2;
223 cpath = 2;
224 }
225 else
226 {
227 cmin = min3;
228 cpath = 3;
229 } /* if */
230
231 } /* if */
232
233 costTable[i + j * row_size] = cmin;
234 matchEdges[i + j * row_size] = cpath;
235 } /* for */
236 } /* for */
237
238 /* =========== Reconstruct the Path =========== */
239
240 i = first_runs;
241 j = second_runs;
242
243 first_curr = i * 2 - 2;
244 second_curr = j * 2 - 2;
245
246
247 while( i > 0 && j > 0 )
248 {
249
250 /* Connect begins */
251 switch (matchEdges[i + j * row_size])
252 {
253
254 case 1: /* to diagonal */
255
256 first_corr[first_curr] = second[second_curr];
257 first_corr[first_curr + 1] = second[second_curr + 2];
258 second_corr[second_curr] = first[first_curr];
259 second_corr[second_curr + 1] = first[first_curr + 2];
260
261 first_curr -= 2;
262 second_curr -= 2;
263 i--;
264 j--;
265
266 break;
267
268 case 2: /* to left */
269
270 first_corr[first_curr] = second[second_curr + 2];
271 first_corr[first_curr + 1] = second[second_curr + 2];
272
273 first_curr -= 2;
274 i--;
275
276 break;
277
278 case 3: /* to up */
279
280 second_corr[second_curr] = first[first_curr + 2];
281 second_corr[second_curr + 1] = first[first_curr + 2];
282
283 second_curr -= 2;
284 j--;
285
286 break;
287 } /* switch */
288 } /* while */
289
290 /* construct rest of horisontal path if its need */
291 while( i > 0 )
292 {
293
294 first_corr[first_curr] = second[second_curr + 2]; /* connect to begin */
295 first_corr[first_curr + 1] = second[second_curr + 2]; /* connect to begin */
296
297 first_curr -= 2;
298 i--;
299
300 } /* while */
301
302 /* construct rest of vertical path if its need */
303 while( j > 0 )
304 {
305
306 second_corr[second_curr] = first[first_curr + 2];
307 second_corr[second_curr + 1] = first[first_curr + 2];
308
309 second_curr -= 2;
310 j--;
311
312 } /* while */
313
314 cvFree( &costTable );
315 cvFree( &matchEdges );
316
317 return CV_NO_ERR;
318 } /* icvDynamicCorrespond */
319
320
321 /*======================================================================================*/
322
323 static CvStatus
icvDynamicCorrespondMulti(int lines,int * first,int * first_runs,int * second,int * second_runs,int * first_corr,int * second_corr)324 icvDynamicCorrespondMulti( int lines, /* number of scanlines */
325 int *first, /* s0|w0|s1|w1|...s(n-1)|w(n-1)|sn */
326 int *first_runs, /* numbers of runs */
327 int *second, int *second_runs, int *first_corr, /* s0'|e0'|s1'|e1'|... */
328 int *second_corr )
329 {
330 CvStatus error;
331
332 int currFirst;
333 int currSecond;
334 int currFirstCorr;
335 int currSecondCorr;
336 int n;
337
338 /* Test errors */
339
340 if( (lines < 1) ||
341 (first == 0) ||
342 (first_runs == 0) ||
343 (second == 0) || (second_runs == 0) || (first_corr == 0) || (second_corr == 0) )
344 return CV_BADFACTOR_ERR;
345
346 currFirst = 0;
347 currSecond = 0;
348 currFirstCorr = 0;
349 currSecondCorr = 0;
350
351 for( n = 0; n < lines; n++ )
352 {
353
354 error = icvDynamicCorrespond( &(first[currFirst]),
355 first_runs[n],
356 &(second[currSecond]),
357 second_runs[n],
358 &(first_corr[currFirstCorr]),
359 &(second_corr[currSecondCorr]) );
360
361 if( error != CV_NO_ERR )
362 return error;
363
364 currFirst += first_runs[n] * 2 + 1;
365 currSecond += second_runs[n] * 2 + 1;
366 currFirstCorr += first_runs[n] * 2;
367 currSecondCorr += second_runs[n] * 2;
368
369 }
370
371 return CV_NO_ERR;
372
373 } /* icvDynamicCorrespondMulti */
374
375
376 /*======================================================================================*/
377
378 /*F///////////////////////////////////////////////////////////////////////////////////////
379 // Name: cvDynamicCorrespondMulti
380 // Purpose: The functions
381 // Context:
382 // Parameters:
383 //
384 // Notes:
385 //F*/
386 CV_IMPL void
cvDynamicCorrespondMulti(int lines,int * first,int * first_runs,int * second,int * second_runs,int * first_corr,int * second_corr)387 cvDynamicCorrespondMulti( int lines, /* number of scanlines */
388 int *first, /* s0|w0|s1|w1|...s(n-1)|w(n-1)|sn */
389 int *first_runs, /* numbers of runs */
390 int *second, int *second_runs, int *first_corr, /* s0'|e0'|s1'|e1'|... */
391 int *second_corr )
392 {
393 CV_FUNCNAME( "cvDynamicCorrespondMulti" );
394 __BEGIN__;
395
396 IPPI_CALL( icvDynamicCorrespondMulti( lines, /* number of scanlines */
397 first, /* s0|w0|s1|w1|...s(n-1)|w(n-1)|sn */
398 first_runs, /* numbers of runs */
399 second, second_runs, first_corr, /* s0'|e0'|s1'|e1'|... */
400 second_corr ));
401 __CLEANUP__;
402 __END__;
403 }
404