git.maemo.org Git - opencv/blob - cvaux/src/cvcorrespond.cpp

   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 }