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 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 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 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
