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
49CvStatus
50icvDynamicCorrespond( 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
323static CvStatus
324icvDynamicCorrespondMulti( 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*/
386CV_IMPL void
387cvDynamicCorrespondMulti( 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