git.maemo.org Git - opencv/blob - cv/src/cvinpaint.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 icvers.
  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
  42 /* ////////////////////////////////////////////////////////////////////
  43 //
  44 //  Geometrical transforms on images and matrices: rotation, zoom etc.
  45 //
  46 // */
  47
  48 #include "_cv.h"
  49
  50 #undef CV_MAT_ELEM_PTR_FAST
  51 #define CV_MAT_ELEM_PTR_FAST( mat, row, col, pix_size )  \
  52      ((mat).data.ptr + (size_t)(mat).step*(row) + (pix_size)*(col))
  53
  54 inline float
  55 min4( float a, float b, float c, float d )
  56 {
  57     a = MIN(a,b);
  58     c = MIN(c,d);
  59     return MIN(a,c);
  60 }
  61
  62 #define CV_MAT_3COLOR_ELEM(img,type,y,x,c) CV_MAT_ELEM(img,type,y,(x)*3+(c))
  63 #define KNOWN  0  //known outside narrow band
  64 #define BAND   1  //narrow band (known)
  65 #define INSIDE 2  //unknown
  66 #define CHANGE 3  //servise
  67
  68 typedef struct CvHeapElem
  69 {
  70     float T;
  71     int i,j;
  72     struct CvHeapElem* prev;
  73     struct CvHeapElem* next;
  74 }
  75 CvHeapElem;
  76
  77
  78 class CvPriorityQueueFloat
  79 {
  80 protected:
  81     CvHeapElem *mem,*empty,*head,*tail;
  82     int num,in;
  83
  84 public:
  85     bool Init( const CvMat* f )
  86     {
  87         int i,j;
  88         for( i = num = 0; i < f->rows; i++ )
  89         {
  90             for( j = 0; j < f->cols; j++ )
  91                 num += CV_MAT_ELEM(*f,uchar,i,j)!=0;
  92         }
  93         if (num<=0) return false;
  94         mem = (CvHeapElem*)cvAlloc((num+2)*sizeof(CvHeapElem));
  95         if (mem==NULL) return false;
  96
  97         head       = mem;
  98         head->i    = head->j = -1;
  99         head->prev = NULL;
 100         head->next = mem+1;
 101         head->T    = -FLT_MAX;
 102         empty      = mem+1;
 103         for (i=1; i<=num; i++) {
 104             mem[i].prev   = mem+i-1;
 105             mem[i].next   = mem+i+1;
 106             mem[i].i      = mem[i].i = -1;
 107             mem[i].T      = FLT_MAX;
 108         }
 109         tail       = mem+i;
 110         tail->i    = tail->j = -1;
 111         tail->prev = mem+i-1;
 112         tail->next = NULL;
 113         tail->T    = FLT_MAX;
 114         return true;
 115     }
 116
 117     bool Add(const CvMat* f) {
 118         int i,j;
 119         for (i=0; i<f->rows; i++) {
 120             for (j=0; j<f->cols; j++) {
 121                 if (CV_MAT_ELEM(*f,uchar,i,j)!=0) {
 122                     if (!Push(i,j,0)) return false;
 123                 }
 124             }
 125         }
 126         return true;
 127     }
 128
 129     bool Push(int i, int j, float T) {
 130         CvHeapElem *tmp=empty,*add=empty;
 131         if (empty==tail) return false;
 132         while (tmp->prev->T>T) tmp = tmp->prev;
 133         if (tmp!=empty) {
 134             add->prev->next = add->next;
 135             add->next->prev = add->prev;
 136             empty = add->next;
 137             add->prev = tmp->prev;
 138             add->next = tmp;
 139             add->prev->next = add;
 140             add->next->prev = add;
 141         } else {
 142             empty = empty->next;
 143         }
 144         add->i = i;
 145         add->j = j;
 146         add->T = T;
 147         in++;
 148         //      printf("push i %3d  j %3d  T %12.4e  in %4d\n",i,j,T,in);
 149         return true;
 150     }
 151
 152     bool Pop(int *i, int *j) {
 153         CvHeapElem *tmp=head->next;
 154         if (empty==tmp) return false;
 155         *i = tmp->i;
 156         *j = tmp->j;
 157         tmp->prev->next = tmp->next;
 158         tmp->next->prev = tmp->prev;
 159         tmp->prev = empty->prev;
 160         tmp->next = empty;
 161         tmp->prev->next = tmp;
 162         tmp->next->prev = tmp;
 163         empty = tmp;
 164         in--;
 165         //      printf("pop  i %3d  j %3d  T %12.4e  in %4d\n",tmp->i,tmp->j,tmp->T,in);
 166         return true;
 167     }
 168
 169     bool Pop(int *i, int *j, float *T) {
 170         CvHeapElem *tmp=head->next;
 171         if (empty==tmp) return false;
 172         *i = tmp->i;
 173         *j = tmp->j;
 174         *T = tmp->T;
 175         tmp->prev->next = tmp->next;
 176         tmp->next->prev = tmp->prev;
 177         tmp->prev = empty->prev;
 178         tmp->next = empty;
 179         tmp->prev->next = tmp;
 180         tmp->next->prev = tmp;
 181         empty = tmp;
 182         in--;
 183         //      printf("pop  i %3d  j %3d  T %12.4e  in %4d\n",tmp->i,tmp->j,tmp->T,in);
 184         return true;
 185     }
 186
 187     CvPriorityQueueFloat(void) {
 188         num=in=0;
 189         mem=empty=head=tail=NULL;
 190     }
 191
 192     ~CvPriorityQueueFloat(void)
 193     {
 194         cvFree( &mem );
 195     }
 196 };
 197
 198 inline float VectorScalMult(CvPoint2D32f v1,CvPoint2D32f v2) {
 199    return v1.x*v2.x+v1.y*v2.y;
 200 }
 201
 202 inline float VectorLength(CvPoint2D32f v1) {
 203    return v1.x*v1.x+v1.y*v1.y;
 204 }
 205
 206 ///////////////////////////////////////////////////////////////////////////////////////////
 207 //HEAP::iterator Heap_Iterator;
 208 //HEAP Heap;
 209
 210 float FastMarching_solve(int i1,int j1,int i2,int j2, const CvMat* f, const CvMat* t)
 211 {
 212     double sol, a11, a22, m12;
 213     a11=CV_MAT_ELEM(*t,float,i1,j1);
 214     a22=CV_MAT_ELEM(*t,float,i2,j2);
 215     m12=MIN(a11,a22);
 216
 217     if( CV_MAT_ELEM(*f,uchar,i1,j1) != INSIDE )
 218         if( CV_MAT_ELEM(*f,uchar,i2,j2) != INSIDE )
 219             if( fabs(a11-a22) >= 1.0 )
 220                 sol = 1+m12;
 221             else
 222                 sol = (a11+a22+sqrt((double)(2-(a11-a22)*(a11-a22))))*0.5;
 223         else
 224             sol = 1+a11;
 225     else if( CV_MAT_ELEM(*f,uchar,i2,j2) != INSIDE )
 226         sol = 1+a22;
 227     else
 228         sol = 1+m12;
 229
 230     return (float)sol;
 231 }
 232
 233 /////////////////////////////////////////////////////////////////////////////////////
 234
 235
 236 static void
 237 icvCalcFMM(const CvMat *f, CvMat *t, CvPriorityQueueFloat *Heap, bool negate) {
 238    int i, j, ii = 0, jj = 0, q;
 239    float dist;
 240
 241    while (Heap->Pop(&ii,&jj)) {
 242
 243       unsigned known=(negate)?CHANGE:KNOWN;
 244       CV_MAT_ELEM(*f,uchar,ii,jj) = (uchar)known;
 245
 246       for (q=0; q<4; q++) {
 247          i=0; j=0;
 248          if     (q==0) {i=ii-1; j=jj;}
 249          else if(q==1) {i=ii;   j=jj-1;}
 250          else if(q==2) {i=ii+1; j=jj;}
 251          else {i=ii;   j=jj+1;}
 252          if ((i<=0)||(j<=0)||(i>f->rows)||(j>f->cols)) continue;
 253
 254          if (CV_MAT_ELEM(*f,uchar,i,j)==INSIDE) {
 255             dist = min4(FastMarching_solve(i-1,j,i,j-1,f,t),
 256                         FastMarching_solve(i+1,j,i,j-1,f,t),
 257                         FastMarching_solve(i-1,j,i,j+1,f,t),
 258                         FastMarching_solve(i+1,j,i,j+1,f,t));
 259             CV_MAT_ELEM(*t,float,i,j) = dist;
 260             CV_MAT_ELEM(*f,uchar,i,j) = BAND;
 261             Heap->Push(i,j,dist);
 262          }
 263       }
 264    }
 265
 266    if (negate) {
 267       for (i=0; i<f->rows; i++) {
 268          for(j=0; j<f->cols; j++) {
 269             if (CV_MAT_ELEM(*f,uchar,i,j) == CHANGE) {
 270                CV_MAT_ELEM(*f,uchar,i,j) = KNOWN;
 271                CV_MAT_ELEM(*t,float,i,j) = -CV_MAT_ELEM(*t,float,i,j);
 272             }
 273          }
 274       }
 275    }
 276 }
 277
 278
 279 static void
 280 icvTeleaInpaintFMM(const CvMat *f, CvMat *t, CvMat *out, int range, CvPriorityQueueFloat *Heap ) {
 281    int i = 0, j = 0, ii = 0, jj = 0, k, l, q, color = 0;
 282    float dist;
 283
 284    if (CV_MAT_CN(out->type)==3) {
 285
 286       while (Heap->Pop(&ii,&jj)) {
 287
 288          CV_MAT_ELEM(*f,uchar,ii,jj) = KNOWN;
 289          for(q=0; q<4; q++) {
 290             if     (q==0) {i=ii-1; j=jj;}
 291             else if(q==1) {i=ii;   j=jj-1;}
 292             else if(q==2) {i=ii+1; j=jj;}
 293             else if(q==3) {i=ii;   j=jj+1;}
 294             if ((i<=1)||(j<=1)||(i>t->rows-1)||(j>t->cols-1)) continue;
 295
 296             if (CV_MAT_ELEM(*f,uchar,i,j)==INSIDE) {
 297                dist = min4(FastMarching_solve(i-1,j,i,j-1,f,t),
 298                            FastMarching_solve(i+1,j,i,j-1,f,t),
 299                            FastMarching_solve(i-1,j,i,j+1,f,t),
 300                            FastMarching_solve(i+1,j,i,j+1,f,t));
 301                CV_MAT_ELEM(*t,float,i,j) = dist;
 302
 303                for (color=0; color<=2; color++) {
 304                   CvPoint2D32f gradI,gradT,r;
 305                   float Ia=0,Jx=0,Jy=0,s=1.0e-20f,w,dst,lev,dir,sat;
 306
 307                   if (CV_MAT_ELEM(*f,uchar,i,j+1)!=INSIDE) {
 308                      if (CV_MAT_ELEM(*f,uchar,i,j-1)!=INSIDE) {
 309                         gradT.x=(float)((CV_MAT_ELEM(*t,float,i,j+1)-CV_MAT_ELEM(*t,float,i,j-1)))*0.5f;
 310                      } else {
 311                         gradT.x=(float)((CV_MAT_ELEM(*t,float,i,j+1)-CV_MAT_ELEM(*t,float,i,j)));
 312                      }
 313                   } else {
 314                      if (CV_MAT_ELEM(*f,uchar,i,j-1)!=INSIDE) {
 315                         gradT.x=(float)((CV_MAT_ELEM(*t,float,i,j)-CV_MAT_ELEM(*t,float,i,j-1)));
 316                      } else {
 317                         gradT.x=0;
 318                      }
 319                   }
 320                   if (CV_MAT_ELEM(*f,uchar,i+1,j)!=INSIDE) {
 321                      if (CV_MAT_ELEM(*f,uchar,i-1,j)!=INSIDE) {
 322                         gradT.y=(float)((CV_MAT_ELEM(*t,float,i+1,j)-CV_MAT_ELEM(*t,float,i-1,j)))*0.5f;
 323                      } else {
 324                         gradT.y=(float)((CV_MAT_ELEM(*t,float,i+1,j)-CV_MAT_ELEM(*t,float,i,j)));
 325                      }
 326                   } else {
 327                      if (CV_MAT_ELEM(*f,uchar,i-1,j)!=INSIDE) {
 328                         gradT.y=(float)((CV_MAT_ELEM(*t,float,i,j)-CV_MAT_ELEM(*t,float,i-1,j)));
 329                      } else {
 330                         gradT.y=0;
 331                      }
 332                   }
 333                   for (k=i-range; k<=i+range; k++) {
 334                      int km=k-1+(k==1),kp=k-1-(k==t->rows-2);
 335                      for (l=j-range; l<=j+range; l++) {
 336                         int lm=l-1+(l==1),lp=l-1-(l==t->cols-2);
 337                         if (k>0&&l>0&&k<t->rows-1&&l<t->cols-1) {
 338                            if ((CV_MAT_ELEM(*f,uchar,k,l)!=INSIDE)&&
 339                                ((l-j)*(l-j)+(k-i)*(k-i)<=range*range)) {
 340                               r.y     = (float)(i-k);
 341                               r.x     = (float)(j-l);
 342
 343                               dst = (float)(1./(VectorLength(r)*sqrt((double)VectorLength(r))));
 344                               lev = (float)(1./(1+fabs(CV_MAT_ELEM(*t,float,k,l)-CV_MAT_ELEM(*t,float,i,j))));
 345
 346                               dir=VectorScalMult(r,gradT);
 347                               if (fabs(dir)<=0.01) dir=0.000001f;
 348                               w = (float)fabs(dst*lev*dir);
 349
 350                               if (CV_MAT_ELEM(*f,uchar,k,l+1)!=INSIDE) {
 351                                  if (CV_MAT_ELEM(*f,uchar,k,l-1)!=INSIDE) {
 352                                     gradI.x=(float)((CV_MAT_3COLOR_ELEM(*out,uchar,km,lp+1,color)-CV_MAT_3COLOR_ELEM(*out,uchar,km,lm-1,color)))*2.0f;
 353                                  } else {
 354                                     gradI.x=(float)((CV_MAT_3COLOR_ELEM(*out,uchar,km,lp+1,color)-CV_MAT_3COLOR_ELEM(*out,uchar,km,lm,color)));
 355                                  }
 356                               } else {
 357                                  if (CV_MAT_ELEM(*f,uchar,k,l-1)!=INSIDE) {
 358                                     gradI.x=(float)((CV_MAT_3COLOR_ELEM(*out,uchar,km,lp,color)-CV_MAT_3COLOR_ELEM(*out,uchar,km,lm-1,color)));
 359                                  } else {
 360                                     gradI.x=0;
 361                                  }
 362                               }
 363                               if (CV_MAT_ELEM(*f,uchar,k+1,l)!=INSIDE) {
 364                                  if (CV_MAT_ELEM(*f,uchar,k-1,l)!=INSIDE) {
 365                                     gradI.y=(float)((CV_MAT_3COLOR_ELEM(*out,uchar,kp+1,lm,color)-CV_MAT_3COLOR_ELEM(*out,uchar,km-1,lm,color)))*2.0f;
 366                                  } else {
 367                                     gradI.y=(float)((CV_MAT_3COLOR_ELEM(*out,uchar,kp+1,lm,color)-CV_MAT_3COLOR_ELEM(*out,uchar,km,lm,color)));
 368                                  }
 369                               } else {
 370                                  if (CV_MAT_ELEM(*f,uchar,k-1,l)!=INSIDE) {
 371                                     gradI.y=(float)((CV_MAT_3COLOR_ELEM(*out,uchar,kp,lm,color)-CV_MAT_3COLOR_ELEM(*out,uchar,km-1,lm,color)));
 372                                  } else {
 373                                     gradI.y=0;
 374                                  }
 375                               }
 376                               Ia += (float)w * (float)(CV_MAT_3COLOR_ELEM(*out,uchar,km,lm,color));
 377                               Jx -= (float)w * (float)(gradI.x*r.x);
 378                               Jy -= (float)w * (float)(gradI.y*r.y);
 379                               s  += w;
 380                            }
 381                         }
 382                      }
 383                   }
 384                   sat = (float)((Ia/s+(Jx+Jy)/(sqrt(Jx*Jx+Jy*Jy)+1.0e-20f)+0.5f));
 385                   {
 386                   int isat = cvRound(sat);
 387                   CV_MAT_3COLOR_ELEM(*out,uchar,i-1,j-1,color) = CV_CAST_8U(isat);
 388                   }
 389                }
 390
 391                CV_MAT_ELEM(*f,uchar,i,j) = BAND;
 392                Heap->Push(i,j,dist);
 393             }
 394          }
 395       }
 396
 397    } else if (CV_MAT_CN(out->type)==1) {
 398
 399       while (Heap->Pop(&ii,&jj)) {
 400
 401          CV_MAT_ELEM(*f,uchar,ii,jj) = KNOWN;
 402          for(q=0; q<4; q++) {
 403             if     (q==0) {i=ii-1; j=jj;}
 404             else if(q==1) {i=ii;   j=jj-1;}
 405             else if(q==2) {i=ii+1; j=jj;}
 406             else if(q==3) {i=ii;   j=jj+1;}
 407             if ((i<=1)||(j<=1)||(i>t->rows-1)||(j>t->cols-1)) continue;
 408
 409             if (CV_MAT_ELEM(*f,uchar,i,j)==INSIDE) {
 410                dist = min4(FastMarching_solve(i-1,j,i,j-1,f,t),
 411                            FastMarching_solve(i+1,j,i,j-1,f,t),
 412                            FastMarching_solve(i-1,j,i,j+1,f,t),
 413                            FastMarching_solve(i+1,j,i,j+1,f,t));
 414                CV_MAT_ELEM(*t,float,i,j) = dist;
 415
 416                for (color=0; color<=0; color++) {
 417                   CvPoint2D32f gradI,gradT,r;
 418                   float Ia=0,Jx=0,Jy=0,s=1.0e-20f,w,dst,lev,dir,sat;
 419
 420                   if (CV_MAT_ELEM(*f,uchar,i,j+1)!=INSIDE) {
 421                      if (CV_MAT_ELEM(*f,uchar,i,j-1)!=INSIDE) {
 422                         gradT.x=(float)((CV_MAT_ELEM(*t,float,i,j+1)-CV_MAT_ELEM(*t,float,i,j-1)))*0.5f;
 423                      } else {
 424                         gradT.x=(float)((CV_MAT_ELEM(*t,float,i,j+1)-CV_MAT_ELEM(*t,float,i,j)));
 425                      }
 426                   } else {
 427                      if (CV_MAT_ELEM(*f,uchar,i,j-1)!=INSIDE) {
 428                         gradT.x=(float)((CV_MAT_ELEM(*t,float,i,j)-CV_MAT_ELEM(*t,float,i,j-1)));
 429                      } else {
 430                         gradT.x=0;
 431                      }
 432                   }
 433                   if (CV_MAT_ELEM(*f,uchar,i+1,j)!=INSIDE) {
 434                      if (CV_MAT_ELEM(*f,uchar,i-1,j)!=INSIDE) {
 435                         gradT.y=(float)((CV_MAT_ELEM(*t,float,i+1,j)-CV_MAT_ELEM(*t,float,i-1,j)))*0.5f;
 436                      } else {
 437                         gradT.y=(float)((CV_MAT_ELEM(*t,float,i+1,j)-CV_MAT_ELEM(*t,float,i,j)));
 438                      }
 439                   } else {
 440                      if (CV_MAT_ELEM(*f,uchar,i-1,j)!=INSIDE) {
 441                         gradT.y=(float)((CV_MAT_ELEM(*t,float,i,j)-CV_MAT_ELEM(*t,float,i-1,j)));
 442                      } else {
 443                         gradT.y=0;
 444                      }
 445                   }
 446                   for (k=i-range; k<=i+range; k++) {
 447                      int km=k-1+(k==1),kp=k-1-(k==t->rows-2);
 448                      for (l=j-range; l<=j+range; l++) {
 449                         int lm=l-1+(l==1),lp=l-1-(l==t->cols-2);
 450                         if (k>0&&l>0&&k<t->rows-1&&l<t->cols-1) {
 451                            if ((CV_MAT_ELEM(*f,uchar,k,l)!=INSIDE)&&
 452                                ((l-j)*(l-j)+(k-i)*(k-i)<=range*range)) {
 453                               r.y     = (float)(i-k);
 454                               r.x     = (float)(j-l);
 455
 456                               dst = (float)(1./(VectorLength(r)*sqrt(VectorLength(r))));
 457                               lev = (float)(1./(1+fabs(CV_MAT_ELEM(*t,float,k,l)-CV_MAT_ELEM(*t,float,i,j))));
 458
 459                               dir=VectorScalMult(r,gradT);
 460                               if (fabs(dir)<=0.01) dir=0.000001f;
 461                               w = (float)fabs(dst*lev*dir);
 462
 463                               if (CV_MAT_ELEM(*f,uchar,k,l+1)!=INSIDE) {
 464                                  if (CV_MAT_ELEM(*f,uchar,k,l-1)!=INSIDE) {
 465                                     gradI.x=(float)((CV_MAT_ELEM(*out,uchar,km,lp+1)-CV_MAT_ELEM(*out,uchar,km,lm-1)))*2.0f;
 466                                  } else {
 467                                     gradI.x=(float)((CV_MAT_ELEM(*out,uchar,km,lp+1)-CV_MAT_ELEM(*out,uchar,km,lm)));
 468                                  }
 469                               } else {
 470                                  if (CV_MAT_ELEM(*f,uchar,k,l-1)!=INSIDE) {
 471                                     gradI.x=(float)((CV_MAT_ELEM(*out,uchar,km,lp)-CV_MAT_ELEM(*out,uchar,km,lm-1)));
 472                                  } else {
 473                                     gradI.x=0;
 474                                  }
 475                               }
 476                               if (CV_MAT_ELEM(*f,uchar,k+1,l)!=INSIDE) {
 477                                  if (CV_MAT_ELEM(*f,uchar,k-1,l)!=INSIDE) {
 478                                     gradI.y=(float)((CV_MAT_ELEM(*out,uchar,kp+1,lm)-CV_MAT_ELEM(*out,uchar,km-1,lm)))*2.0f;
 479                                  } else {
 480                                     gradI.y=(float)((CV_MAT_ELEM(*out,uchar,kp+1,lm)-CV_MAT_ELEM(*out,uchar,km,lm)));
 481                                  }
 482                               } else {
 483                                  if (CV_MAT_ELEM(*f,uchar,k-1,l)!=INSIDE) {
 484                                     gradI.y=(float)((CV_MAT_ELEM(*out,uchar,kp,lm)-CV_MAT_ELEM(*out,uchar,km-1,lm)));
 485                                  } else {
 486                                     gradI.y=0;
 487                                  }
 488                               }
 489                               Ia += (float)w * (float)(CV_MAT_ELEM(*out,uchar,km,lm));
 490                               Jx -= (float)w * (float)(gradI.x*r.x);
 491                               Jy -= (float)w * (float)(gradI.y*r.y);
 492                               s  += w;
 493                            }
 494                         }
 495                      }
 496                   }
 497                   sat = (float)((Ia/s+(Jx+Jy)/(sqrt(Jx*Jx+Jy*Jy)+1.0e-20f)+0.5f));
 498                   {
 499                   int isat = cvRound(sat);
 500                   CV_MAT_ELEM(*out,uchar,i-1,j-1) = CV_CAST_8U(isat);
 501                   }
 502                }
 503
 504                CV_MAT_ELEM(*f,uchar,i,j) = BAND;
 505                Heap->Push(i,j,dist);
 506             }
 507          }
 508       }
 509    }
 510 }
 511
 512
 513 static void
 514 icvNSInpaintFMM(const CvMat *f, CvMat *t, CvMat *out, int range, CvPriorityQueueFloat *Heap) {
 515    int i = 0, j = 0, ii = 0, jj = 0, k, l, q, color = 0;
 516    float dist;
 517
 518    if (CV_MAT_CN(out->type)==3) {
 519
 520       while (Heap->Pop(&ii,&jj)) {
 521
 522          CV_MAT_ELEM(*f,uchar,ii,jj) = KNOWN;
 523          for(q=0; q<4; q++) {
 524             if     (q==0) {i=ii-1; j=jj;}
 525             else if(q==1) {i=ii;   j=jj-1;}
 526             else if(q==2) {i=ii+1; j=jj;}
 527             else if(q==3) {i=ii;   j=jj+1;}
 528             if ((i<=1)||(j<=1)||(i>t->rows-1)||(j>t->cols-1)) continue;
 529
 530             if (CV_MAT_ELEM(*f,uchar,i,j)==INSIDE) {
 531                dist = min4(FastMarching_solve(i-1,j,i,j-1,f,t),
 532                            FastMarching_solve(i+1,j,i,j-1,f,t),
 533                            FastMarching_solve(i-1,j,i,j+1,f,t),
 534                            FastMarching_solve(i+1,j,i,j+1,f,t));
 535                CV_MAT_ELEM(*t,float,i,j) = dist;
 536
 537                for (color=0; color<=2; color++) {
 538                   CvPoint2D32f gradI,r;
 539                   float Ia=0,s=1.0e-20f,w,dst,dir;
 540
 541                   for (k=i-range; k<=i+range; k++) {
 542                      int km=k-1+(k==1),kp=k-1-(k==f->rows-2);
 543                      for (l=j-range; l<=j+range; l++) {
 544                         int lm=l-1+(l==1),lp=l-1-(l==f->cols-2);
 545                         if (k>0&&l>0&&k<f->rows-1&&l<f->cols-1) {
 546                            if ((CV_MAT_ELEM(*f,uchar,k,l)!=INSIDE)&&
 547                                ((l-j)*(l-j)+(k-i)*(k-i)<=range*range)) {
 548                               r.y=(float)(k-i);
 549                               r.x=(float)(l-j);
 550
 551                               dst = 1/(VectorLength(r)*VectorLength(r)+1);
 552
 553                               if (CV_MAT_ELEM(*f,uchar,k+1,l)!=INSIDE) {
 554                                  if (CV_MAT_ELEM(*f,uchar,k-1,l)!=INSIDE) {
 555                                     gradI.x=(float)(abs(CV_MAT_3COLOR_ELEM(*out,uchar,kp+1,lm,color)-CV_MAT_3COLOR_ELEM(*out,uchar,kp,lm,color))+
 556                                                     abs(CV_MAT_3COLOR_ELEM(*out,uchar,kp,lm,color)-CV_MAT_3COLOR_ELEM(*out,uchar,km-1,lm,color)));
 557                                  } else {
 558                                     gradI.x=(float)(abs(CV_MAT_3COLOR_ELEM(*out,uchar,kp+1,lm,color)-CV_MAT_3COLOR_ELEM(*out,uchar,kp,lm,color)))*2.0f;
 559                                  }
 560                               } else {
 561                                  if (CV_MAT_ELEM(*f,uchar,k-1,l)!=INSIDE) {
 562                                     gradI.x=(float)(abs(CV_MAT_3COLOR_ELEM(*out,uchar,kp,lm,color)-CV_MAT_3COLOR_ELEM(*out,uchar,km-1,lm,color)))*2.0f;
 563                                  } else {
 564                                     gradI.x=0;
 565                                  }
 566                               }
 567                               if (CV_MAT_ELEM(*f,uchar,k,l+1)!=INSIDE) {
 568                                  if (CV_MAT_ELEM(*f,uchar,k,l-1)!=INSIDE) {
 569                                     gradI.y=(float)(abs(CV_MAT_3COLOR_ELEM(*out,uchar,km,lp+1,color)-CV_MAT_3COLOR_ELEM(*out,uchar,km,lm,color))+
 570                                                     abs(CV_MAT_3COLOR_ELEM(*out,uchar,km,lm,color)-CV_MAT_3COLOR_ELEM(*out,uchar,km,lm-1,color)));
 571                                  } else {
 572                                     gradI.y=(float)(abs(CV_MAT_3COLOR_ELEM(*out,uchar,km,lp+1,color)-CV_MAT_3COLOR_ELEM(*out,uchar,km,lm,color)))*2.0f;
 573                                  }
 574                               } else {
 575                                  if (CV_MAT_ELEM(*f,uchar,k,l-1)!=INSIDE) {
 576                                     gradI.y=(float)(abs(CV_MAT_3COLOR_ELEM(*out,uchar,km,lm,color)-CV_MAT_3COLOR_ELEM(*out,uchar,km,lm-1,color)))*2.0f;
 577                                  } else {
 578                                     gradI.y=0;
 579                                  }
 580                               }
 581
 582                               gradI.x=-gradI.x;
 583                               dir=VectorScalMult(r,gradI);
 584
 585                               if (fabs(dir)<=0.01) {
 586                                  dir=0.000001f;
 587                               } else {
 588                                  dir = (float)fabs(VectorScalMult(r,gradI)/sqrt(VectorLength(r)*VectorLength(gradI)));
 589                               }
 590                               w = dst*dir;
 591                               Ia += (float)w * (float)(CV_MAT_3COLOR_ELEM(*out,uchar,km,lm,color));
 592                               s  += w;
 593                            }
 594                         }
 595                      }
 596                   }
 597                   {
 598                   int out_val = cvRound((double)Ia/s);
 599                   CV_MAT_3COLOR_ELEM(*out,uchar,i-1,j-1,color) = CV_CAST_8U(out_val);
 600                   }
 601                }
 602
 603                CV_MAT_ELEM(*f,uchar,i,j) = BAND;
 604                Heap->Push(i,j,dist);
 605             }
 606          }
 607       }
 608
 609    } else if (CV_MAT_CN(out->type)==1) {
 610
 611       while (Heap->Pop(&ii,&jj)) {
 612
 613          CV_MAT_ELEM(*f,uchar,ii,jj) = KNOWN;
 614          for(q=0; q<4; q++) {
 615             if     (q==0) {i=ii-1; j=jj;}
 616             else if(q==1) {i=ii;   j=jj-1;}
 617             else if(q==2) {i=ii+1; j=jj;}
 618             else if(q==3) {i=ii;   j=jj+1;}
 619             if ((i<=1)||(j<=1)||(i>t->rows-1)||(j>t->cols-1)) continue;
 620
 621             if (CV_MAT_ELEM(*f,uchar,i,j)==INSIDE) {
 622                dist = min4(FastMarching_solve(i-1,j,i,j-1,f,t),
 623                            FastMarching_solve(i+1,j,i,j-1,f,t),
 624                            FastMarching_solve(i-1,j,i,j+1,f,t),
 625                            FastMarching_solve(i+1,j,i,j+1,f,t));
 626                CV_MAT_ELEM(*t,float,i,j) = dist;
 627
 628                {
 629                   CvPoint2D32f gradI,r;
 630                   float Ia=0,s=1.0e-20f,w,dst,dir;
 631
 632                   for (k=i-range; k<=i+range; k++) {
 633                      int km=k-1+(k==1),kp=k-1-(k==t->rows-2);
 634                      for (l=j-range; l<=j+range; l++) {
 635                         int lm=l-1+(l==1),lp=l-1-(l==t->cols-2);
 636                         if (k>0&&l>0&&k<t->rows-1&&l<t->cols-1) {
 637                            if ((CV_MAT_ELEM(*f,uchar,k,l)!=INSIDE)&&
 638                                ((l-j)*(l-j)+(k-i)*(k-i)<=range*range)) {
 639                               r.y=(float)(i-k);
 640                               r.x=(float)(j-l);
 641
 642                               dst = 1/(VectorLength(r)*VectorLength(r)+1);
 643
 644                               if (CV_MAT_ELEM(*f,uchar,k+1,l)!=INSIDE) {
 645                                  if (CV_MAT_ELEM(*f,uchar,k-1,l)!=INSIDE) {
 646                                     gradI.x=(float)(abs(CV_MAT_ELEM(*out,uchar,kp+1,lm)-CV_MAT_ELEM(*out,uchar,kp,lm))+
 647                                                     abs(CV_MAT_ELEM(*out,uchar,kp,lm)-CV_MAT_ELEM(*out,uchar,km-1,lm)));
 648                                  } else {
 649                                     gradI.x=(float)(abs(CV_MAT_ELEM(*out,uchar,kp+1,lm)-CV_MAT_ELEM(*out,uchar,kp,lm)))*2.0f;
 650                                  }
 651                               } else {
 652                                  if (CV_MAT_ELEM(*f,uchar,k-1,l)!=INSIDE) {
 653                                     gradI.x=(float)(abs(CV_MAT_ELEM(*out,uchar,kp,lm)-CV_MAT_ELEM(*out,uchar,km-1,lm)))*2.0f;
 654                                  } else {
 655                                     gradI.x=0;
 656                                  }
 657                               }
 658                               if (CV_MAT_ELEM(*f,uchar,k,l+1)!=INSIDE) {
 659                                  if (CV_MAT_ELEM(*f,uchar,k,l-1)!=INSIDE) {
 660                                     gradI.y=(float)(abs(CV_MAT_ELEM(*out,uchar,km,lp+1)-CV_MAT_ELEM(*out,uchar,km,lm))+
 661                                                     abs(CV_MAT_ELEM(*out,uchar,km,lm)-CV_MAT_ELEM(*out,uchar,km,lm-1)));
 662                                  } else {
 663                                     gradI.y=(float)(abs(CV_MAT_ELEM(*out,uchar,km,lp+1)-CV_MAT_ELEM(*out,uchar,km,lm)))*2.0f;
 664                                  }
 665                               } else {
 666                                  if (CV_MAT_ELEM(*f,uchar,k,l-1)!=INSIDE) {
 667                                     gradI.y=(float)(abs(CV_MAT_ELEM(*out,uchar,km,lm)-CV_MAT_ELEM(*out,uchar,km,lm-1)))*2.0f;
 668                                  } else {
 669                                     gradI.y=0;
 670                                  }
 671                               }
 672
 673                               gradI.x=-gradI.x;
 674                               dir=VectorScalMult(r,gradI);
 675
 676                               if (fabs(dir)<=0.01) {
 677                                  dir=0.000001f;
 678                               } else {
 679                                  dir = (float)fabs(VectorScalMult(r,gradI)/sqrt(VectorLength(r)*VectorLength(gradI)));
 680                               }
 681                               w = dst*dir;
 682                               Ia += (float)w * (float)(CV_MAT_ELEM(*out,uchar,km,lm));
 683                               s  += w;
 684                            }
 685                         }
 686                      }
 687                   }
 688                   {
 689                   int out_val = cvRound((double)Ia/s);
 690                   CV_MAT_ELEM(*out,uchar,i-1,j-1) = CV_CAST_8U(out_val);
 691                   }
 692                }
 693
 694                CV_MAT_ELEM(*f,uchar,i,j) = BAND;
 695                Heap->Push(i,j,dist);
 696             }
 697          }
 698       }
 699
 700    }
 701 }
 702
 703 #define SET_BORDER1_C1(image,type,value) {\
 704       int i,j;\
 705       for(j=0; j<image->cols; j++) {\
 706          CV_MAT_ELEM(*image,type,0,j) = value;\
 707       }\
 708       for (i=1; i<image->rows-1; i++) {\
 709          CV_MAT_ELEM(*image,type,i,0) = CV_MAT_ELEM(*image,type,i,image->cols-1) = value;\
 710       }\
 711       for(j=0; j<image->cols; j++) {\
 712          CV_MAT_ELEM(*image,type,erows-1,j) = value;\
 713       }\
 714    }
 715
 716 #define COPY_MASK_BORDER1_C1(src,dst,type) {\
 717       int i,j;\
 718       for (i=0; i<src->rows; i++) {\
 719          for(j=0; j<src->cols; j++) {\
 720             if (CV_MAT_ELEM(*src,type,i,j)!=0)\
 721                CV_MAT_ELEM(*dst,type,i+1,j+1) = INSIDE;\
 722          }\
 723       }\
 724    }
 725
 726
 727 CV_IMPL void
 728 cvInpaint( const CvArr* _input_img, const CvArr* _inpaint_mask, CvArr* _output_img,
 729            double inpaintRange, int flags )
 730 {
 731     CvMat *mask = 0, *band = 0, *f = 0, *t = 0, *out = 0;
 732     CvPriorityQueueFloat *Heap = 0, *Out = 0;
 733     IplConvKernel *el_cross = 0, *el_range = 0;
 734
 735     CV_FUNCNAME( "cvInpaint" );
 736
 737     __BEGIN__;
 738
 739     CvMat input_hdr, mask_hdr, output_hdr;
 740     CvMat* input_img, *inpaint_mask, *output_img;
 741     int range=cvRound(inpaintRange);
 742     int erows, ecols;
 743
 744     CV_CALL( input_img = cvGetMat( _input_img, &input_hdr ));
 745     CV_CALL( inpaint_mask = cvGetMat( _inpaint_mask, &mask_hdr ));
 746     CV_CALL( output_img = cvGetMat( _output_img, &output_hdr ));
 747
 748     if( !CV_ARE_SIZES_EQ(input_img,output_img) || !CV_ARE_SIZES_EQ(input_img,inpaint_mask))
 749         CV_ERROR( CV_StsUnmatchedSizes, "All the input and output images must have the same size" );
 750
 751     if( CV_MAT_TYPE(input_img->type) != CV_8UC1 &&
 752         CV_MAT_TYPE(input_img->type) != CV_8UC3 ||
 753         !CV_ARE_TYPES_EQ(input_img,output_img) )
 754         CV_ERROR( CV_StsUnsupportedFormat,
 755         "Only 8-bit 1-channel and 3-channel input/output images are supported" );
 756
 757     if( CV_MAT_TYPE(inpaint_mask->type) != CV_8UC1 )
 758         CV_ERROR( CV_StsUnsupportedFormat, "The mask must be 8-bit 1-channel image" );
 759
 760     range = MAX(range,1);
 761     range = MIN(range,100);
 762
 763     ecols = input_img->cols + 2;
 764     erows = input_img->rows + 2;
 765
 766     CV_CALL( f = cvCreateMat(erows, ecols, CV_8UC1));
 767     CV_CALL( t = cvCreateMat(erows, ecols, CV_32FC1));
 768     CV_CALL( band = cvCreateMat(erows, ecols, CV_8UC1));
 769     CV_CALL( mask = cvCreateMat(erows, ecols, CV_8UC1));
 770     CV_CALL( el_cross = cvCreateStructuringElementEx(3,3,1,1,CV_SHAPE_CROSS,NULL));
 771
 772     cvCopy( input_img, output_img );
 773     cvSet(mask,cvScalar(KNOWN,0,0,0));
 774     COPY_MASK_BORDER1_C1(inpaint_mask,mask,uchar);
 775     SET_BORDER1_C1(mask,uchar,0);
 776     cvSet(f,cvScalar(KNOWN,0,0,0));
 777     cvSet(t,cvScalar(1.0e6f,0,0,0));
 778     cvDilate(mask,band,el_cross,1);   // image with narrow band
 779     Heap=new CvPriorityQueueFloat;
 780     if (!Heap->Init(band))
 781         EXIT;
 782     cvSub(band,mask,band,NULL);
 783     SET_BORDER1_C1(band,uchar,0);
 784     if (!Heap->Add(band))
 785         EXIT;
 786     cvSet(f,cvScalar(BAND,0,0,0),band);
 787     cvSet(f,cvScalar(INSIDE,0,0,0),mask);
 788     cvSet(t,cvScalar(0,0,0,0),band);
 789
 790     if( flags == CV_INPAINT_TELEA )
 791     {
 792         CV_CALL( out = cvCreateMat(erows, ecols, CV_8UC1));
 793         CV_CALL( el_range = cvCreateStructuringElementEx(2*range+1,2*range+1,
 794             range,range,CV_SHAPE_RECT,NULL));
 795         cvDilate(mask,out,el_range,1);
 796         cvSub(out,mask,out,NULL);
 797         Out=new CvPriorityQueueFloat;
 798         if (!Out->Init(out))
 799             EXIT;
 800         if (!Out->Add(band))
 801             EXIT;
 802         cvSub(out,band,out,NULL);
 803         SET_BORDER1_C1(out,uchar,0);
 804         icvCalcFMM(out,t,Out,true);
 805         icvTeleaInpaintFMM(mask,t,output_img,range,Heap);
 806     }
 807     else
 808         icvNSInpaintFMM(mask,t,output_img,range,Heap);
 809
 810     __END__;
 811
 812     delete Out;
 813     delete Heap;
 814     cvReleaseStructuringElement(&el_cross);
 815     cvReleaseStructuringElement(&el_range);
 816     cvReleaseMat(&out);
 817     cvReleaseMat(&mask);
 818     cvReleaseMat(&band);
 819     cvReleaseMat(&t);
 820     cvReleaseMat(&f);
 821 }