1b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis/*M/////////////////////////////////////////////////////////////////////////////////////// 2f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// 3f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// 5f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// By downloading, copying, installing or using the software you agree to this license. 6f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// If you do not agree to this license, do not download, install, 7f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// copy or use the software. 8f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// 9f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// 10f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// Intel License Agreement 11f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// For Open Source Computer Vision Library 12f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// 13f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// Copyright (C) 2000, Intel Corporation, all rights reserved. 14f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// Third party copyrights are property of their respective icvers. 15f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// 16f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// Redistribution and use in source and binary forms, with or without modification, 17f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// are permitted provided that the following conditions are met: 18b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis// 1955fc873017f10f6f566b182b70f6fc22aefa3464Chandler Carruth// * Redistribution's of source code must retain the above copyright notice, 2055fc873017f10f6f566b182b70f6fc22aefa3464Chandler Carruth// this list of conditions and the following disclaimer. 2155fc873017f10f6f566b182b70f6fc22aefa3464Chandler Carruth// 22ec8605f1d7ec846dbf51047bfd5c56d32d1ff91cArgyrios Kyrtzidis// * Redistribution's in binary form must reproduce the above copyright notice, 23b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis// this list of conditions and the following disclaimer in the documentation 24b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis// and/or other materials provided with the distribution. 2518c66fdc3c4008d335885695fe36fb5353c5f672Ted Kremenek// 26a620bd8fb8c6f2080898e4aecf77b46e7e1a8f16Ted Kremenek// * The name of Intel Corporation may not be used to endorse or promote products 27a93d0f280693b8418bc88cf7a8c93325f7fcf4c6Benjamin Kramer// derived from this software without specific prior written permission. 28f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// 29f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// This software is provided by the copyright holders and contributors "as is" and 309ef6537a894c33003359b1f9b9676e9178e028b7Ted Kremenek// any express or implied warranties, including, but not limited to, the implied 31f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek// warranties of merchantability and fitness for a particular purpose are disclaimed. 3225a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis// In no event shall the Intel Corporation or contributors be liable for any direct, 3325a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis// indirect, incidental, special, exemplary, or consequential damages 341eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump// (including, but not limited to, procurement of substitute goods or services; 35b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis// loss of use, data, or profits; or business interruption) however caused 36b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis// and on any theory of liability, whether in contract, strict liability, 37b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis// or tort (including negligence or otherwise) arising in any way out of 381eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump// the use of this software, even if advised of the possibility of such damage. 39b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis// 40b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis//M*/ 41ec8605f1d7ec846dbf51047bfd5c56d32d1ff91cArgyrios Kyrtzidis 42b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis/* //////////////////////////////////////////////////////////////////// 43cfdf9b4edf1172728be97d1ae2d95171975f812bTed Kremenek// 44b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis// Geometrical transforms on images and matrices: rotation, zoom etc. 456bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines// 461eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump// */ 47b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 48b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis#include "_cv.h" 49b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 50b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis#undef CV_MAT_ELEM_PTR_FAST 511eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump#define CV_MAT_ELEM_PTR_FAST( mat, row, col, pix_size ) \ 52b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis ((mat).data.ptr + (size_t)(mat).step*(row) + (pix_size)*(col)) 53b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 54b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidisinline float 55b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidismin4( float a, float b, float c, float d ) 56b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis{ 57651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines a = MIN(a,b); 58b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis c = MIN(c,d); 591eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump return MIN(a,c); 60b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis} 61b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 621eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump#define CV_MAT_3COLOR_ELEM(img,type,y,x,c) CV_MAT_ELEM(img,type,y,(x)*3+(c)) 63b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis#define KNOWN 0 //known outside narrow band 64651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines#define BAND 1 //narrow band (known) 65651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines#define INSIDE 2 //unknown 66b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis#define CHANGE 3 //servise 67b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 68b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidistypedef struct CvHeapElem 69b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis{ 70f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek float T; 71b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis int i,j; 72b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis struct CvHeapElem* prev; 73b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis struct CvHeapElem* next; 74b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis} 75590dd8e0959d8df5621827768987c4792b74fc06Anna ZaksCvHeapElem; 76590dd8e0959d8df5621827768987c4792b74fc06Anna Zaks 77651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines 78590dd8e0959d8df5621827768987c4792b74fc06Anna Zaksclass CvPriorityQueueFloat 79b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis{ 80cfdf9b4edf1172728be97d1ae2d95171975f812bTed Kremenekprotected: 81f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek CvHeapElem *mem,*empty,*head,*tail; 82b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis int num,in; 83b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 84b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidispublic: 851eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump bool Init( const CvMat* f ) 86b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis { 87b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis int i,j; 88ec8605f1d7ec846dbf51047bfd5c56d32d1ff91cArgyrios Kyrtzidis for( i = num = 0; i < f->rows; i++ ) 89b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis { 90f45d18c08b549644dd5f3d3ad731b8e4d09be730Ted Kremenek for( j = 0; j < f->cols; j++ ) 91b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis num += CV_MAT_ELEM(*f,uchar,i,j)!=0; 926bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines } 93cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenek if (num<=0) return false; 94b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis mem = (CvHeapElem*)cvAlloc((num+2)*sizeof(CvHeapElem)); 95b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis if (mem==NULL) return false; 96b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 97b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis head = mem; 981eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump head->i = head->j = -1; 99b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis head->prev = NULL; 100b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis head->next = mem+1; 101b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis head->T = -FLT_MAX; 10210620eb5164e31208fcbf0437cd79ae535ed0559Sean Hunt empty = mem+1; 103b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis for (i=1; i<=num; i++) { 104651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines mem[i].prev = mem+i-1; 105cfdf9b4edf1172728be97d1ae2d95171975f812bTed Kremenek mem[i].next = mem+i+1; 1061eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump mem[i].i = -1; 107b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis mem[i].T = FLT_MAX; 108b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis } 1091eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump tail = mem+i; 110b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis tail->i = tail->j = -1; 111651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines tail->prev = mem+i-1; 112651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines tail->next = NULL; 113b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis tail->T = FLT_MAX; 114b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis return true; 115b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis } 116b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 117b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis bool Add(const CvMat* f) { 118b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis int i,j; 119b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis for (i=0; i<f->rows; i++) { 120b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis for (j=0; j<f->cols; j++) { 121b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis if (CV_MAT_ELEM(*f,uchar,i,j)!=0) { 122590dd8e0959d8df5621827768987c4792b74fc06Anna Zaks if (!Push(i,j,0)) return false; 123590dd8e0959d8df5621827768987c4792b74fc06Anna Zaks } 124651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines } 125590dd8e0959d8df5621827768987c4792b74fc06Anna Zaks } 126cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenek return true; 127cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenek } 128cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenek 129b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis bool Push(int i, int j, float T) { 130b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis CvHeapElem *tmp=empty,*add=empty; 131b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis if (empty==tail) return false; 1321eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump while (tmp->prev->T>T) tmp = tmp->prev; 133b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis if (tmp!=empty) { 1341eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump add->prev->next = add->next; 135b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis add->next->prev = add->prev; 136b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis empty = add->next; 137651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines add->prev = tmp->prev; 138651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines add->next = tmp; 139651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines add->prev->next = add; 140b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis add->next->prev = add; 1411eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump } else { 142b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis empty = empty->next; 143b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis } 144651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines add->i = i; 145651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines add->j = j; 146651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines add->T = T; 147b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis in++; 148b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis // printf("push i %3d j %3d T %12.4e in %4d\n",i,j,T,in); 149b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis return true; 150b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis } 151b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 152b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis bool Pop(int *i, int *j) { 153ec8605f1d7ec846dbf51047bfd5c56d32d1ff91cArgyrios Kyrtzidis CvHeapElem *tmp=head->next; 154b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis if (empty==tmp) return false; 155b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis *i = tmp->i; 1566bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines *j = tmp->j; 1576bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines tmp->prev->next = tmp->next; 158b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis tmp->next->prev = tmp->prev; 159b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis tmp->prev = empty->prev; 1606bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines tmp->next = empty; 161b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis tmp->prev->next = tmp; 162b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis tmp->next->prev = tmp; 163390909c89c98ab1807e15e033a72e975f866fb23Anna Zaks empty = tmp; 164390909c89c98ab1807e15e033a72e975f866fb23Anna Zaks in--; 165b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis // printf("pop i %3d j %3d T %12.4e in %4d\n",tmp->i,tmp->j,tmp->T,in); 166b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis return true; 167b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis } 168b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 169b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis bool Pop(int *i, int *j, float *T) { 170466224fd068a0a0084968a7f521a690a51c3b226Jordan Rose CvHeapElem *tmp=head->next; 171466224fd068a0a0084968a7f521a690a51c3b226Jordan Rose if (empty==tmp) return false; 172cfdf9b4edf1172728be97d1ae2d95171975f812bTed Kremenek *i = tmp->i; 173b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis *j = tmp->j; 1748bef8238181a30e52dea380789a7e2d760eac532Ted Kremenek *T = tmp->T; 175b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis tmp->prev->next = tmp->next; 176b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis tmp->next->prev = tmp->prev; 177b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis tmp->prev = empty->prev; 178b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis tmp->next = empty; 179b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis tmp->prev->next = tmp; 180cfdf9b4edf1172728be97d1ae2d95171975f812bTed Kremenek tmp->next->prev = tmp; 181b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis empty = tmp; 1828bef8238181a30e52dea380789a7e2d760eac532Ted Kremenek in--; 183b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis // printf("pop i %3d j %3d T %12.4e in %4d\n",tmp->i,tmp->j,tmp->T,in); 184b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis return true; 1850bd6b110e908892d4b5c8671a9f435a1d72ad16aAnna Zaks } 186b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 1871eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump CvPriorityQueueFloat(void) { 1884ba48c43410a8ad4f32d1d3f684c7d297513e0a1Argyrios Kyrtzidis num=in=0; 1894ba48c43410a8ad4f32d1d3f684c7d297513e0a1Argyrios Kyrtzidis mem=empty=head=tail=NULL; 19039ac1876f6f9a1a8e0070f0df61036c7ba05202bAnna Zaks } 191dc84cd5efdd3430efb22546b4ac656aa0540b210David Blaikie 19225a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis ~CvPriorityQueueFloat(void) 19325a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis { 1944ba48c43410a8ad4f32d1d3f684c7d297513e0a1Argyrios Kyrtzidis cvFree( &mem ); 1954ba48c43410a8ad4f32d1d3f684c7d297513e0a1Argyrios Kyrtzidis } 1964ba48c43410a8ad4f32d1d3f684c7d297513e0a1Argyrios Kyrtzidis}; 1974ba48c43410a8ad4f32d1d3f684c7d297513e0a1Argyrios Kyrtzidis 19825a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidisinline float VectorScalMult(CvPoint2D32f v1,CvPoint2D32f v2) { 19925a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis return v1.x*v2.x+v1.y*v2.y; 20025a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis} 20125a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis 20225a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidisinline float VectorLength(CvPoint2D32f v1) { 203b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis return v1.x*v1.x+v1.y*v1.y; 204390909c89c98ab1807e15e033a72e975f866fb23Anna Zaks} 205b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 206b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis/////////////////////////////////////////////////////////////////////////////////////////// 207b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis//HEAP::iterator Heap_Iterator; 2085251abea41b446c26e3239c8dd6c7edea6fc335dDavid Blaikie//HEAP Heap; 209b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 210cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenekfloat FastMarching_solve(int i1,int j1,int i2,int j2, const CvMat* f, const CvMat* t) 21125a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis{ 2128bef8238181a30e52dea380789a7e2d760eac532Ted Kremenek double sol, a11, a22, m12; 213b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis a11=CV_MAT_ELEM(*t,float,i1,j1); 214b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis a22=CV_MAT_ELEM(*t,float,i2,j2); 215b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis m12=MIN(a11,a22); 216b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 217b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis if( CV_MAT_ELEM(*f,uchar,i1,j1) != INSIDE ) 218b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis if( CV_MAT_ELEM(*f,uchar,i2,j2) != INSIDE ) 21925a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis if( fabs(a11-a22) >= 1.0 ) 2204ba48c43410a8ad4f32d1d3f684c7d297513e0a1Argyrios Kyrtzidis sol = 1+m12; 22125a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis else 22225a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis sol = (a11+a22+sqrt((double)(2-(a11-a22)*(a11-a22))))*0.5; 223b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis else 224b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis sol = 1+a11; 22525a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis else if( CV_MAT_ELEM(*f,uchar,i2,j2) != INSIDE ) 226b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis sol = 1+a22; 22725a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis else 228b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis sol = 1+m12; 22925a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis 2305251abea41b446c26e3239c8dd6c7edea6fc335dDavid Blaikie return (float)sol; 231b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis} 232b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 233cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenek///////////////////////////////////////////////////////////////////////////////////// 23425a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis 2355251abea41b446c26e3239c8dd6c7edea6fc335dDavid Blaikie 236b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidisstatic void 237cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted KremenekicvCalcFMM(const CvMat *f, CvMat *t, CvPriorityQueueFloat *Heap, bool negate) { 238cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenek int i, j, ii = 0, jj = 0, q; 239cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenek float dist; 240b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 241b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis while (Heap->Pop(&ii,&jj)) { 242b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 2431eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump unsigned known=(negate)?CHANGE:KNOWN; 244b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis CV_MAT_ELEM(*f,uchar,ii,jj) = (uchar)known; 2458bef8238181a30e52dea380789a7e2d760eac532Ted Kremenek 246b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis for (q=0; q<4; q++) { 2471eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump i=0; j=0; 248b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis if (q==0) {i=ii-1; j=jj;} 249b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis else if(q==1) {i=ii; j=jj-1;} 250b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis else if(q==2) {i=ii+1; j=jj;} 251b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis else {i=ii; j=jj+1;} 2521eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump if ((i<=0)||(j<=0)||(i>f->rows)||(j>f->cols)) continue; 253b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis 254b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis if (CV_MAT_ELEM(*f,uchar,i,j)==INSIDE) { 2551eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump dist = min4(FastMarching_solve(i-1,j,i,j-1,f,t), 256b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis FastMarching_solve(i+1,j,i,j-1,f,t), 257cfa88f893915ceb8ae4ce2f17c46c24a4d67502fDmitri Gribenko FastMarching_solve(i-1,j,i,j+1,f,t), 258a620bd8fb8c6f2080898e4aecf77b46e7e1a8f16Ted Kremenek FastMarching_solve(i+1,j,i,j+1,f,t)); 259b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis CV_MAT_ELEM(*t,float,i,j) = dist; 260a620bd8fb8c6f2080898e4aecf77b46e7e1a8f16Ted Kremenek CV_MAT_ELEM(*f,uchar,i,j) = BAND; 261a620bd8fb8c6f2080898e4aecf77b46e7e1a8f16Ted Kremenek Heap->Push(i,j,dist); 262a620bd8fb8c6f2080898e4aecf77b46e7e1a8f16Ted Kremenek } 263a620bd8fb8c6f2080898e4aecf77b46e7e1a8f16Ted Kremenek } 264b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis } 265a620bd8fb8c6f2080898e4aecf77b46e7e1a8f16Ted Kremenek 266cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenek if (negate) { 2676bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines for (i=0; i<f->rows; i++) { 2686bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines for(j=0; j<f->cols; j++) { 2696bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines if (CV_MAT_ELEM(*f,uchar,i,j) == CHANGE) { 2706bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines CV_MAT_ELEM(*f,uchar,i,j) = KNOWN; 2716bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines CV_MAT_ELEM(*t,float,i,j) = -CV_MAT_ELEM(*t,float,i,j); 2726bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines } 2736bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines } 2746bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines } 2756bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines } 2766bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines} 2776bcf27bb9a4b5c3f79cb44c0e4654a6d7619ad89Stephen Hines 278651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines 279785950e59424dca7ce0081bebf13c0acd2c4fff6Jordan Rosestatic void 280b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios KyrtzidisicvTeleaInpaintFMM(const CvMat *f, CvMat *t, CvMat *out, int range, CvPriorityQueueFloat *Heap ) { 281b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis int i = 0, j = 0, ii = 0, jj = 0, k, l, q, color = 0; 28225a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis float dist; 2831eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump 28425a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis if (CV_MAT_CN(out->type)==3) { 28514108da7f7fc059772711e4ffee1322a27b152a7Steve Naroff 28614108da7f7fc059772711e4ffee1322a27b152a7Steve Naroff while (Heap->Pop(&ii,&jj)) { 2871eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump 28814108da7f7fc059772711e4ffee1322a27b152a7Steve Naroff CV_MAT_ELEM(*f,uchar,ii,jj) = KNOWN; 289183700f494ec9b6701b6efe82bcb25f4c79ba561John McCall for(q=0; q<4; q++) { 29014108da7f7fc059772711e4ffee1322a27b152a7Steve Naroff if (q==0) {i=ii-1; j=jj;} 29114108da7f7fc059772711e4ffee1322a27b152a7Steve Naroff else if(q==1) {i=ii; j=jj-1;} 29214108da7f7fc059772711e4ffee1322a27b152a7Steve Naroff else if(q==2) {i=ii+1; j=jj;} 2931eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump else if(q==3) {i=ii; j=jj+1;} 29414108da7f7fc059772711e4ffee1322a27b152a7Steve Naroff if ((i<=1)||(j<=1)||(i>t->rows-1)||(j>t->cols-1)) continue; 2951eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump 29614108da7f7fc059772711e4ffee1322a27b152a7Steve Naroff if (CV_MAT_ELEM(*f,uchar,i,j)==INSIDE) { 29714108da7f7fc059772711e4ffee1322a27b152a7Steve Naroff dist = min4(FastMarching_solve(i-1,j,i,j-1,f,t), 29814108da7f7fc059772711e4ffee1322a27b152a7Steve Naroff FastMarching_solve(i+1,j,i,j-1,f,t), 2991eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump FastMarching_solve(i-1,j,i,j+1,f,t), 30014108da7f7fc059772711e4ffee1322a27b152a7Steve Naroff FastMarching_solve(i+1,j,i,j+1,f,t)); 301cfdf9b4edf1172728be97d1ae2d95171975f812bTed Kremenek CV_MAT_ELEM(*t,float,i,j) = dist; 3027360fda1efd88fd28ca2882579676dbd8569c181Ted Kremenek 30325a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis for (color=0; color<=2; color++) { 30425a792b0361d80337c75a14320f5be1b210066dcArgyrios Kyrtzidis CvPoint2D32f gradI,gradT,r; 305cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenek float Ia=0,Jx=0,Jy=0,s=1.0e-20f,w,dst,lev,dir,sat; 3061eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump 307183700f494ec9b6701b6efe82bcb25f4c79ba561John McCall if (CV_MAT_ELEM(*f,uchar,i,j+1)!=INSIDE) { 308cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenek if (CV_MAT_ELEM(*f,uchar,i,j-1)!=INSIDE) { 309cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenek gradT.x=(float)((CV_MAT_ELEM(*t,float,i,j+1)-CV_MAT_ELEM(*t,float,i,j-1)))*0.5f; 310cf118d41f7930a18dce97416ef7834a62642f587Ted Kremenek } else { 311cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenek gradT.x=(float)((CV_MAT_ELEM(*t,float,i,j+1)-CV_MAT_ELEM(*t,float,i,j))); 312cc9ac41ac06d9511fbc8ad2914ef6bd6f99aa247Ted Kremenek } 313b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis } else { 314b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis if (CV_MAT_ELEM(*f,uchar,i,j-1)!=INSIDE) { 315651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines gradT.x=(float)((CV_MAT_ELEM(*t,float,i,j)-CV_MAT_ELEM(*t,float,i,j-1))); 316651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines } else { 317b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis gradT.x=0; 318b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis } 3191eb4433ac451dc16f4133a88af2d002ac26c58efMike Stump } 320b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis if (CV_MAT_ELEM(*f,uchar,i+1,j)!=INSIDE) { 321b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis if (CV_MAT_ELEM(*f,uchar,i-1,j)!=INSIDE) { 322651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines gradT.y=(float)((CV_MAT_ELEM(*t,float,i+1,j)-CV_MAT_ELEM(*t,float,i-1,j)))*0.5f; 323651f13cea278ec967336033dd032faef0e9fc2ecStephen Hines } else { 324b3d74da3e1620c9a7a378afb5f244e4987e6713eArgyrios Kyrtzidis gradT.y=(float)((CV_MAT_ELEM(*t,float,i+1,j)-CV_MAT_ELEM(*t,float,i,j))); 3257360fda1efd88fd28ca2882579676dbd8569c181Ted Kremenek } 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