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 54inline float 55min4( 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 68typedef struct CvHeapElem 69{ 70 float T; 71 int i,j; 72 struct CvHeapElem* prev; 73 struct CvHeapElem* next; 74} 75CvHeapElem; 76 77 78class CvPriorityQueueFloat 79{ 80protected: 81 CvHeapElem *mem,*empty,*head,*tail; 82 int num,in; 83 84public: 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 = -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 198inline float VectorScalMult(CvPoint2D32f v1,CvPoint2D32f v2) { 199 return v1.x*v2.x+v1.y*v2.y; 200} 201 202inline 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 210float 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 236static void 237icvCalcFMM(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 279static void 280icvTeleaInpaintFMM(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 513static void 514icvNSInpaintFMM(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 727CV_IMPL void 728cvInpaint( 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} 822