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
min4(float a,float b,float c,float d)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:
Init(const CvMat * f)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
Add(const CvMat * f)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
Push(int i,int j,float T)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
Pop(int * i,int * j)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
Pop(int * i,int * j,float * T)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
CvPriorityQueueFloat(void)187 CvPriorityQueueFloat(void) {
188 num=in=0;
189 mem=empty=head=tail=NULL;
190 }
191
~CvPriorityQueueFloat(void)192 ~CvPriorityQueueFloat(void)
193 {
194 cvFree( &mem );
195 }
196 };
197
VectorScalMult(CvPoint2D32f v1,CvPoint2D32f v2)198 inline float VectorScalMult(CvPoint2D32f v1,CvPoint2D32f v2) {
199 return v1.x*v2.x+v1.y*v2.y;
200 }
201
VectorLength(CvPoint2D32f v1)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
FastMarching_solve(int i1,int j1,int i2,int j2,const CvMat * f,const CvMat * t)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
icvCalcFMM(const CvMat * f,CvMat * t,CvPriorityQueueFloat * Heap,bool negate)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
icvTeleaInpaintFMM(const CvMat * f,CvMat * t,CvMat * out,int range,CvPriorityQueueFloat * Heap)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
icvNSInpaintFMM(const CvMat * f,CvMat * t,CvMat * out,int range,CvPriorityQueueFloat * Heap)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
cvInpaint(const CvArr * _input_img,const CvArr * _inpaint_mask,CvArr * _output_img,double inpaintRange,int flags)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 }
822