int camDrawTextBitmap(CamImage *image, char *text, int x, int y, CamBitmapFont *font)
{
CamImage imx=*image;
CamROI roi;
char *s=text;
int n;
CAM_CHECK_ARGS(camDrawTextBitmapFont,image->imageData!=NULL);
CAM_CHECK_ARGS(camDrawTextBitmapFont,image->nChannels>=3);
roi.coi=0;
imx.roi=&roi;
for (;*s;s++) {
n=*s-font->first_char;
if (n>=0) {
if (n>=font->nb_chars) {
n-='a'-'A';
}
if ((n>=0)&&(n<font->nb_chars)) {
roi.xOffset=x;
roi.yOffset=y;
roi.width=font->letters[n].width;
roi.height=font->letters[n].height;
camCopy(&font->letters[n],&imx);
x+=font->letters[n].width;
}
} else x+=font->height/2;
}
return 0;
}
// Binary images processing
// camMonadicArithm1U : source is n bits, dest is one bit
int camMonadicArithm1U(CamImage *source, CamImage *dest, CamArithmParams *params)
{
int x,y;
int width,height;
CAM_PIXEL *srcptr,*cpsrcptr;
CAM_BIT_BLOCK *dstptr,*cpdstptr;
CAM_BIT_BLOCK bit_block,eol_block;
int sol_offset,bit_offset;
int c1=params->c1;
int c2=params->c2;
int c3=params->c3;
CamInternalROIPolicyStruct iROI;
CAM_CHECK_ARGS(camMonadicArithm,(source->depth&CAM_DEPTH_MASK)<=(sizeof(CAM_PIXEL)*8));
CAM_CHECK_ARGS(camMonadicArithm,(source->depth&CAM_DEPTH_MASK)>=8);
// ROI (Region Of Interest) management
CAM_CHECK(camMonadicArithm,camInternalROIPolicy(source, dest, &iROI, 0));
srcptr=(CAM_PIXEL*)iROI.srcptr;
width=iROI.srcroi.width;
height=iROI.srcroi.height;
if (dest->roi) {
sol_offset=dest->roi->xOffset%CAM_BIT_BLOCK_SIZE;
dstptr=(CAM_BIT_BLOCK*)(dest->imageData+dest->roi->yOffset*dest->widthStep)+(dest->roi->xOffset/CAM_BIT_BLOCK_SIZE);
} else {
sol_offset=0;
dstptr=(CAM_BIT_BLOCK*)dest->imageData;
}
switch(params->operation) {
case CAM_ARITHM_ABS:
camError("camMonadicArithm","CAM_ARITHM_ABS operation is not possible on binary images");
break;
case CAM_ARITHM_SELECT:
if (c2) {
FOREACH_PIXEL {
STORE_BIT((int)(*srcptr==c1));
} END_FOREACH_PIXEL
} else {
FOREACH_PIXEL {
STORE_BIT((int)(*srcptr!=c1));
} END_FOREACH_PIXEL
}
break;
case CAM_ARITHM_THRESHOLD:
if (c2) {
FOREACH_PIXEL {
STORE_BIT((int)(*srcptr<c1));
} END_FOREACH_PIXEL
} else {
int camYUV2RGB(CamImage* source, CamImage *dest)
{
unsigned char *srcptr,*dstptr;
int x,y;
int y1,u,v;
int c1,c2,c3,c4;
int width, height;
CamInternalROIPolicyStruct iROI;
DECLARE_MASK_MANAGEMENT;
CAM_CHECK_ARGS2(camYUV2RGB,source->imageData!=NULL,"source image is not allocated");
if (dest->imageData==NULL) {
// Automatic allocation
camAllocateRGBImage(dest,source->width,source->height);
}
CAM_CHECK(camYUV2RGB,camInternalROIPolicy(source, dest, &iROI, CAM_MASK_SUPPORT | CAM_IGNORE_COI_MISMATCH));
CAM_CHECK_ARGS(camYUV2RGB,source->nChannels==3);
CAM_CHECK_ARGS(camYUV2RGB,dest->nChannels==3);
CAM_CHECK_ARGS(camYUV2RGB,dest->dataOrder==CAM_DATA_ORDER_PIXEL);
CAM_CHECK_ARGS(camYUV2RGB,source->depth==CAM_DEPTH_8U);
CAM_CHECK_ARGS(camYUV2RGB,dest->depth==CAM_DEPTH_8U);
CAM_CHECK_ARGS(camYUV2RGB,(*((int*)source->colorModel)==*((int*)"YUV")));
CAM_CHECK_ARGS(camYUV2RGB,(*((int*)dest->colorModel)==*((int*)"RGB")));
CAM_CHECK_ARGS(camYUV2RGB,(*((int*)dest->channelSeq)==*((int*)"RGB")));
width=iROI.srcroi.width;
height=iROI.srcroi.height;
INIT_MASK_MANAGEMENT;
// Initialize tables if need be
if (LUTYUV2RGB[0]==-1) camInitLUTYUV2RGB();
if (source->dataOrder==CAM_DATA_ORDER_PIXEL) {
for (y = 0; y < height; y ++) {
srcptr=(unsigned char*)(iROI.srcptr+y*source->widthStep);
dstptr=(unsigned char*)(iROI.dstptr+y*dest->widthStep);
BEGIN_MASK_MANAGEMENT(
srcptr+=startx*3;
dstptr+=startx*3;
)
for (x=startx;x<endx;x++) {
y1 = to76309[*srcptr++];
u = *srcptr++;
v = *srcptr++;
c1 = LUTYUV2RGB[v];
c2 = GU[u];
c3 = GV[v];
c4 = BU[u];
*dstptr++ = clip[384+((y1 + c1)>>16)];
*dstptr++ = clip[384+((y1 - c2 - c3)>>16)];
*dstptr++ = clip[384+((y1 + c4)>>16)];
}
END_MASK_MANAGEMENT;
}
} else {
int camKeypointsMatchingKdTree(CamKeypoints *target, CamFPKdTreeNode *kdTreeRoot, CamKeypointsMatches *matches, int explore)
{
int i, c, model, best, nbModels = 0;
int bestDistance, secondBestDistance;
CamKeypoint *point, *bestMatch;
int results[MAX_NB_MODELS], best4Target[2048];
CAM_CHECK_ARGS(camKeypointsMatchingKdTree, target->nbPoints <= 2048);
CAM_CHECK_ARGS(camKeypointsMatchingKdTree, matches->allocated != 0);
for (model = 0; model < MAX_NB_MODELS; model++) results[model] = 0;
matches->nbMatches = 0;
matches->nbOutliers = 0;
for (c = 0; c < target->nbPoints; c++) {
point = target->keypoint[c];
bestMatch = camFindKeypointKdTree(point, kdTreeRoot, explore, &bestDistance, &secondBestDistance);
//printf("%d %d %d\n", bestMatch->set->id, bestDistance, secondBestDistance);
// Final test...
if (secondBestDistance == -1 || bestDistance < 0.8 * secondBestDistance) {
if (bestMatch->set->id >= 0 && bestMatch->set->id < MAX_NB_MODELS) {
results[bestMatch->set->id]++;
if (bestMatch->set->id + 1 > nbModels) nbModels = bestMatch->set->id + 1;
}
matches->pairs[matches->nbMatches].p1 = bestMatch;
matches->pairs[matches->nbMatches].p2 = point;
matches->pairs[matches->nbMatches].mark = bestDistance;
best4Target[matches->nbMatches] = bestMatch->set->id;
matches->nbMatches++;
if (matches->nbMatches == matches->allocated) break;
}
}
best = 0;
for (i = 1; i < nbModels; i++) {
if (results[i] > results[best]) best = i;
}
c = 0;
for (i = 0; i < matches->nbMatches; i++) {
if (best4Target[i] == best) {
matches->pairs[c++] = matches->pairs[i];
}
}
matches->nbMatches = results[best];
return best;
}
int camKeypointsMatching(CamKeypoints *target, CamKeypoints **models, int nbM, CamKeypointsMatches *matches)
{
int i, c, model, best, nbModels = 0;
int distance, bestDistance, secondBestDistance, bestModel, secondBestModel;
CamKeypoint *point, *bestMatch;
#define MAX_NB_MODELS 256
int results[MAX_NB_MODELS], best4Target[2048];
CAM_CHECK_ARGS(camKeypointsMatching, nbModels <= MAX_NB_MODELS);
CAM_CHECK_ARGS(camKeypointsMatching, target->nbPoints <= 2048);
CAM_CHECK_ARGS(camKeypointsMatching, matches->allocated != 0);
for (model = 0; model < MAX_NB_MODELS; model++) results[model] = 0;
matches->nbMatches = 0;
matches->nbOutliers = 0;
for (c = 0; c < target->nbPoints; c++) {
point = target->keypoint[c];
bestDistance = -1; bestModel = 0;
for (model = 0; model < nbM; model++) {
for (i = 0; i < models[model]->nbPoints; i++) {
distance = camKeypointsDistance(models[model]->keypoint[i], point);
if (bestDistance == -1 || distance <= bestDistance) {
bestMatch = models[model]->keypoint[i];
secondBestDistance = bestDistance;
secondBestModel = bestModel;
bestDistance = distance;
bestModel = model;
} else if (secondBestDistance == -1 || distance <= secondBestDistance) {
secondBestDistance = distance;
secondBestModel = model;
}
}
}
//printf("%d %d %d\n", bestMatch->set->id, bestDistance, secondBestDistance);
/*
if (models[bestModel]->id == models[secondBestModel]->id) {
// Accept the point whatever it is
secondBestDistance = 2 * bestDistance;
}
*/
/*
secondBestDistance = -1; secondBestModel = 0;
// We have to scan again for the second best
for (model = 0; model < nbM; model++) {
if (models[model]->id == models[bestModel]->id) continue; // Skip the best model
for (i = 0; i < models[model]->nbPoints; i++) {
distance = camKeypointsDistance(models[model]->keypoint[i], point);
if (secondBestDistance == -1 || distance <= secondBestDistance) {
secondBestDistance = distance;
secondBestModel = model;
}
}
}
}
*/
// Final test...
if (bestDistance < 0.8 * secondBestDistance) {
if (bestMatch->set->id >= 0 && bestMatch->set->id < MAX_NB_MODELS) {
results[bestMatch->set->id]++;
if (bestMatch->set->id + 1 > nbModels) nbModels = bestMatch->set->id + 1;
}
matches->pairs[matches->nbMatches].p1 = bestMatch;
matches->pairs[matches->nbMatches].p2 = point;
matches->pairs[matches->nbMatches].mark = bestDistance;
best4Target[matches->nbMatches] = bestMatch->set->id;
matches->nbMatches++;
if (matches->nbMatches == matches->allocated) break;
}
}
best = 0;
for (i = 1; i < nbModels; i++) {
if (results[i] > results[best]) best = i;
}
c = 0;
for (i = 0; i < matches->nbMatches; i++) {
if (best4Target[i] == best) {
matches->pairs[c++] = matches->pairs[i];
}
}
matches->nbMatches = results[best];
return best;
}
int camCopy(CamImage* source, CamImage* dest)
{
int x,y,i,n,c,special=0;
int width,height;
CAM_PIXEL *srcptr,*cpsrcptr;
CAM_PIXEL_DST *dstptr,*cpdstptr;
CamROI roi,roi2,*tmp,*tmp2;
#ifdef CAM_SATURATE
int valpix;
int valmin=0, valmax=255;
#endif
CamRun *run;
int startx,endx;
CamInternalROIPolicyStruct iROI;
if (dest->imageData) {
// Special cases :
// Grey to color conversion : source is one channel, and dest is several channels (3 or 4)
if (((source->nChannels==1)||((source->roi)&&(source->roi->coi)))
&&(dest->nChannels!=1)&&((dest->roi==NULL)||(dest->roi->coi==0))) special=1;
// 3 to 4 channels conversion : RGB->RGBA. Adding an alpha channel
else if ((source->nChannels==3)&&(dest->nChannels==4)&&
((source->roi==NULL)||(source->roi->coi==0))&&
((dest->roi==NULL)||(dest->roi->coi==0))) special=2;
// 4 to 3 channels conversion : RGBA -> RGB. Removing an alpha channel
else if ((source->nChannels==4)&&(dest->nChannels==3)&&
((source->roi==NULL)||(source->roi->coi==0))&&
((dest->roi==NULL)||(dest->roi->coi==0))) special=2;
}
if (special) {
tmp=dest->roi;
if (dest->roi) {
roi=*tmp;
} else {
camSetMaxROI(&roi,dest);
}
dest->roi=&roi;
roi.coi=1;
n=dest->nChannels;
if (n==4) n=3; // Doesn't copy to the alpha channel
if (special==2) {
tmp2=source->roi;
if (source->roi) {
roi2=*tmp2;
} else {
camSetMaxROI(&roi2,source);
}
source->roi=&roi2;
roi2.coi=1;
}
} else n=1;
for (c=0;c<n;c++) {
// ROI (Region Of Interest) management
CAM_CHECK(camCopy,camInternalROIPolicy(source, dest, &iROI, 1));
CAM_CHECK_ARGS(camCopy,(source->depth&CAM_DEPTH_MASK)<=(sizeof(CAM_PIXEL)*8));
CAM_CHECK_ARGS(camCopy,(source->depth&CAM_DEPTH_MASK)>=8);
CAM_CHECK_ARGS(camCopy,(dest->depth&CAM_DEPTH_MASK)<=(sizeof(CAM_PIXEL_DST)*8));
CAM_CHECK_ARGS(camCopy,(dest->depth&CAM_DEPTH_MASK)>=8);
CAM_CHECK_ARGS(camCopy,(source->depth&CAM_DEPTH_SIGN)==(dest->depth&CAM_DEPTH_SIGN));
width=iROI.srcroi.width;
height=iROI.srcroi.height;
srcptr=(CAM_PIXEL*)iROI.srcptr;
dstptr=(CAM_PIXEL_DST*)iROI.dstptr;
INIT_MASK_MANAGEMENT;
if (source->dataOrder==CAM_DATA_ORDER_PIXEL) {
if (dest->dataOrder==CAM_DATA_ORDER_PIXEL) {
for (y=0;y<height;y++) {
cpsrcptr=srcptr; cpdstptr=dstptr;
BEGIN_MASK_MANAGEMENT( \
srcptr=cpsrcptr+startx*iROI.srcinc;\
dstptr=cpdstptr+startx*iROI.dstinc;\
)
#if CAM_FAST==8
if ((iROI.srcinc==iROI.dstinc)&&(iROI.srcinc==iROI.nChannels)) {
memcpy(dstptr,srcptr,(endx-startx)*iROI.nChannels);
} else
#else
#if CAM_FAST==16
if ((iROI.srcinc==iROI.dstinc)&&(iROI.srcinc==iROI.nChannels)) {
memcpy(dstptr,srcptr,((endx-startx)*iROI.nChannels)<<1);
} else
#endif
#endif
{
for (x=startx;x<endx;x++) {
for (i=0;i<iROI.nChannels;i++) {
#ifdef CAM_SATURATE
valpix=*(srcptr+i);
if (valpix<valmin) valpix=valmin;
else if (valpix>valmax) valpix=valmax;
*(dstptr+i)=valpix;
#else
*(dstptr+i)=(CAM_PIXEL_DST)*(srcptr+i);
#endif
}
srcptr+=iROI.srcinc;
dstptr+=iROI.dstinc;
} }
END_MASK_MANAGEMENT;
srcptr=(CAM_PIXEL*)(((char*)cpsrcptr)+source->widthStep);
dstptr=(CAM_PIXEL_DST*)(((char*)cpdstptr)+dest->widthStep);
}
} else {
for (y=0;y<height;y++) {
cpsrcptr=srcptr; cpdstptr=dstptr;
BEGIN_MASK_MANAGEMENT( \
srcptr=cpsrcptr+startx*iROI.srcinc;\
dstptr=cpdstptr+startx; )
for (x=startx;x<endx;x++) {
for (i=0;i<iROI.nChannels;i++) {
#ifdef CAM_SATURATE
valpix=*(srcptr+i);
if (valpix<valmin) valpix=valmin;
else if (valpix>valmax) valpix=valmax;
*(dstptr+i*iROI.dstpinc)=valpix;
#else
*(dstptr+i*iROI.dstpinc)=(CAM_PIXEL_DST)*(srcptr+i);
#endif
}
srcptr+=iROI.srcinc;
dstptr++;
}
END_MASK_MANAGEMENT;
srcptr=(CAM_PIXEL*)(((char*)cpsrcptr)+source->widthStep);
dstptr=(CAM_PIXEL_DST*)(((char*)cpdstptr)+dest->widthStep);
}
}
// This function is buggy, since it doesn't fully implement a super sampling
// Still being worked on...
int camWarpingSuperSampling(CamImage *source, CamImage *dest, CamWarpingParams *params)
{
int x,y,width,height;
int xl,yl,xr,yr; // Position on the left and right side (source image)
int incxl,incyl,incxr,incyr; // Increment on the left and right sides (source image)
int dxl,dyl,dxr,dyr; // Delta for the left and right sides
int xp,yp; // Position of the current point
int xpp,ypp; // Position of the previous point
int incxp,incyp; // Increment for the current point
int xpl[CAM_MAX_SCANLINE+1],ypl[CAM_MAX_SCANLINE+1]; // (x,y) points for the previous scanline
CAM_PIXEL scanline[CAM_MAX_SCANLINE+1],valpix1,valpixp;
CAM_PIXEL *dstptr,*srcptr1,*cpdstptr;
int xptr1,yptr1=0xdead; // Special value. Generally inaccessible.
int result,xx,yy;
int perspective=params->perspective;
int zul,zbl,zur,zbr,cl,cr,zl,zr; // Projection parameters
int w1,w2,w3,w4,sum; // Weights for linear "square" interpolation
CamPoint I;
CAM_CHECK_ARGS(camWarpingSuperSampling,source->nChannels==1);
CAM_CHECK_ARGS(camWarpingSuperSampling,dest->nChannels==1);
CAM_CHECK_ARGS(camWarpingSuperSampling,(source->depth&CAM_DEPTH_MASK)<=(sizeof(CAM_PIXEL)*8));
CAM_CHECK_ARGS(camWarpingSuperSampling,(source->depth&CAM_DEPTH_MASK)>=8);
CAM_CHECK_ARGS(camWarpingSuperSampling,(dest->depth&CAM_DEPTH_MASK)<=(sizeof(CAM_PIXEL)*8));
CAM_CHECK_ARGS(camWarpingSuperSampling,(dest->depth&CAM_DEPTH_MASK)>=8);
// ROI (Region Of Interest) management
if (dest->roi) {
dstptr=(CAM_PIXEL*)(dest->imageData+dest->roi->yOffset*dest->widthStep+dest->roi->xOffset*sizeof(CAM_PIXEL));
width=dest->roi->width;
height=dest->roi->height;
} else {
dstptr=(CAM_PIXEL*)dest->imageData;
width=dest->width;
height=dest->height;
}
xl=params->p[0].x;
yl=params->p[0].y;
xr=params->p[1].x;
yr=params->p[1].y;
dxl=params->p[3].x-xl;
dyl=params->p[3].y-yl;
dxr=params->p[2].x-xr;
dyr=params->p[2].y-yr;
// perspective warping or not?
if (perspective) {
if (!camIntersectionSegments(params->p,&I)) {
perspective=0;
} else {
// 32 bits fixed point
zul=(int)((((CAM_INT64)1)<<48)/(params->p[0].y-I.y));
zur=(int)((((CAM_INT64)1)<<48)/(params->p[1].y-I.y));
zbl=(int)((((CAM_INT64)1)<<48)/(params->p[3].y-I.y));
zbr=(int)((((CAM_INT64)1)<<48)/(params->p[2].y-I.y));
cl=(zul-zbl)/height; // 32 bits fixed point
cr=(zur-zbr)/height; // Should be positive valued
}
}
if (!perspective) {
incxl=dxl/height;
incyl=dyl/height;
incxr=dxr/height;
incyr=dyr/height;
} else {
incxl=(int)((((CAM_INT64)dxl)<<16)/dyl);
incxr=(int)((((CAM_INT64)dxr)<<16)/dyr);
}
// First scan-line
// Let's go across each horizontal pixel
xp=xl; yp=yl;
incxp=(xr-xl)/width;
incyp=(yr-yl)/width;
for (x=0;x<=width;x++,xp+=incxp,yp+=incyp) {
xpl[x]=xp;
ypl[x]=yp;
// Let's retrieve the pixel at that position in the source image
xx=xp>>16;
yy=yp>>16;
GET_PIXEL(xx,yy,1);
scanline[x]=valpix1;
}
// For all destination pixels
// Let's go across all the lines
for (y=0;y<height;y++) {
if (perspective) {
zl=zul-(int)(((CAM_INT64)y)*cl); // 32 bits fixed points
zr=zur-(int)(((CAM_INT64)y)*cr);
yl=I.y+(int)((((CAM_INT64)1)<<48)/zl);
yr=I.y+(int)((((CAM_INT64)1)<<48)/zr);
xl=params->p[0].x+(int)(((CAM_INT64)incxl)*(yl-params->p[0].y)>>16);
xr=params->p[1].x+(int)(((CAM_INT64)incxr)*(yr-params->p[1].y)>>16);
} else {
// Go to the next line
xl+=incxl;
xr+=incxr;
yl+=incyl;
yr+=incyr;
}
cpdstptr=dstptr;
xp=xl; yp=yl;
incxp=(xr-xl)/width;
incyp=(yr-yl)/width;
// Let's retrieve the value of the first pixel in the source image
xx=xp>>16;
yy=yp>>16;
GET_PIXEL(xx,yy,1);
// And then across each horizontal pixel
for (x=0;x<width;x++,dstptr++) {
// Manage the positions of pixels
xpp=xp;
ypp=yp;
xp+=incxp;
yp+=incyp;
// Manage the values of pixels
valpixp=valpix1;
// Let's retrieve the value of the current pixel in the source image
xx=xp>>16;
yy=yp>>16;
GET_PIXEL(xx,yy,1);
// OK. Now we do have our four source points
// These are pl[x], pl[x+1], pp and p
// (pl = previous line, pp = previous point, p = actual point)
// ((x,y) access trough xpl and ypl variables for pl)
// Their pixel values are respectively
// scanline[x], scanline[x+1], valpixp and valpix1
// We can compute the interpolation between them
switch (params->interpolation) {
case 1: // linear "square" interpolation (slow but nice for resampling)
w1=((0xffff-(xpl[x]&0xffff))>>8)*((0xffff-(ypl[x]&0xffff))>>8);
w2=((xpl[x+1]&0xffff)>>8)*((0xffff-(ypl[x+1]&0xffff))>>8);
w3=((0xffff-(xpp&0xffff))>>8)*((ypp&0xffff)>>8);
w4=((xp&0xffff)>>8)*((yp&0xffff)>>8);
sum=w1+w2+w3+w4;
if (sum) {
result=(((int)scanline[x])*w1+
((int)scanline[x+1])*w2+
((int)valpixp)*w3+
((int)valpix1)*w4)/(w1+w2+w3+w4);
break;
}
case 0: // Very crude (but fast) interpolation scheme
default:
result=((int)scanline[x]+
(int)scanline[x+1]+
(int)valpixp+
(int)valpix1)>>2;
break;
}
// Write the result to the destination image
*dstptr=(CAM_PIXEL)result;
// Go to the next point
// Copy to pl (previous line) in order to prepare for the next horizontal scan
xpl[x]=xpp;
ypl[x]=ypp;
// And copy the value of the previous pixel into the scanline buffer
scanline[x]=valpixp;
}
// Copy to pl (previous line) to prepare for the next horizontal scan
xpl[x]=xp;
ypl[x]=yp;
// And copy the value of the last pixel into the scanline buffer
scanline[x]=valpix1;
// Move the destination pointer
dstptr=(CAM_PIXEL*)(((char*)cpdstptr)+dest->widthStep);
}
int camDilateCircle7(CamImage *source, CamImage *dest)
#endif // CAM_MM_DO_EROSION
#endif // CAM_MM_GRADIENT
#endif // CAM_MM_FINDLOCALMAXIMA
#endif // CAM_MM_OPT_CIRCLE7
{
int i,x,y,ntb,nlb;
int width,height;
int top,left;
int firsttime;
int acc=0;
CamInternalROIPolicyStruct iROI;
#ifdef CAM_MM_DO_EROSION
int tabero[9];
int valero[2];
#endif // CAM_MM_DO_EROSION
#ifdef CAM_MM_DO_DILATION
int tabdil[9];
int valdil[2];
#endif // CAM_MM_DO_DILATION
CAM_PIXEL *srcptr,*dstptr,*tmpptr,*cpsrcptr,*cpdstptr;
unsigned CAM_PIXEL *linesPtr[CAM_MM_NEIGHB];
CAM_PIXEL linesBuffer[CAM_MM_NEIGHB][CAM_MAX_SCANLINE+CAM_MM_NEIGHB-1];
DECLARE_MASK_MANAGEMENT;
#ifdef CAM_MM_FINDLOCALMAXIMA
int valcenter, xp, yp, zp, found;
#ifdef CAM_MM_OPT_CIRCLE7
const int lines2test = 5;
const int start2test[] = {2, 1, 0, 0, 0};
const int end2test[] = {4, 5, 4, 2, 1};
#else
#ifdef CAM_MM_OPT_CIRCLE5
const int lines2test = 4;
const int start2test[] = {1, 0, 0, 0};
const int end2test[] = {3, 3, 1, 0};
#else
const int lines2test = 2;
const int start2test[] = {0, 0};
const int end2test[] = {2, 0};
#endif // CAM_MM_OPT_CIRCLE5
#endif // CAM_MM_OPT_CIRCLE7
// ROI (Region Of Interest) management
CAM_CHECK(camFindLocalMaxima, camInternalROIPolicy(source, NULL, &iROI, 1));
CAM_CHECK_ARGS(camFindLocalMaxima, iROI.nChannels==1);
CAM_CHECK_ARGS2(camFindLocalMaxima, points != NULL, "points destination parameter should not be set to NULL");
#else
CamMorphoMathsKernel params;
CAM_CHECK(camMorphoMathsKernel,camInternalROIPolicy(source, dest, &iROI, 1));
CAM_CHECK_ARGS(camMorphoMathsKernel,(source->depth==dest->depth));
CAM_CHECK_ARGS(camMorphoMathsKernel,iROI.nChannels==1);
if (source->depth == CAM_DEPTH_1U) {
// Binary processing
#ifdef CAM_MM_DO_EROSION
#ifdef CAM_MM_OPT_SQUARE3
for (y=0;y<3;y++) {
for (x=0;x<3;x++) {
params.erosionStructElt[y][x]=1;
}
}
return camErode3x3(source,dest,¶ms);
#endif
#ifdef CAM_MM_OPT_CIRCLE5
for (y=0;y<5;y++) {
for (x=0;x<5;x++) {
params.erosionStructElt[y][x]=1;
}
}
params.erosionStructElt[0][0]=0;
params.erosionStructElt[0][4]=0;
params.erosionStructElt[4][0]=0;
params.erosionStructElt[4][4]=0;
return camErode5x5(source,dest,¶ms);
#endif
#ifdef CAM_MM_OPT_CIRCLE7
for (y=0;y<7;y++) {
for (x=0;x<7;x++) {
params.erosionStructElt[y][x]=camCircle7StructElt[y][x];
}
}
return camErode7x7(source,dest,¶ms);
#endif
#else
#ifdef CAM_MM_OPT_SQUARE3
for (y=0;y<3;y++) {
for (x=0;x<3;x++) {
params.dilationStructElt[y][x]=1;
}
}
return camDilate3x3(source,dest,¶ms);
#endif
#ifdef CAM_MM_OPT_CIRCLE5
for (y=0;y<5;y++) {
for (x=0;x<5;x++) {
params.dilationStructElt[y][x]=1;
}
}
params.dilationStructElt[0][0]=0;
params.dilationStructElt[0][4]=0;
params.dilationStructElt[4][0]=0;
params.dilationStructElt[4][4]=0;
return camDilate5x5(source,dest,¶ms);
#endif
#ifdef CAM_MM_OPT_CIRCLE7
for (y=0;y<7;y++) {
for (x=0;x<7;x++) {
params.dilationStructElt[y][x]=camCircle7StructElt[y][x];
}
}
return camDilate7x7(source,dest,¶ms);
#endif
#endif // CAM_MM_DO_EROSION
}
#endif // CAM_MM_FINDLOCALMAXIMA
CAM_CHECK_ARGS(camMorphoMathsKernel,(source->depth&CAM_DEPTH_MASK)<=(sizeof(CAM_PIXEL)*8));
CAM_CHECK_ARGS(camMorphoMathsKernel,(source->depth&CAM_DEPTH_MASK)>=8);
#ifndef CAM_MM_FINDLOCALMAXIMA
CAM_CHECK_ARGS(camMorphoMathsKernel,(dest->depth&CAM_DEPTH_MASK)<=(sizeof(CAM_PIXEL)*8));
CAM_CHECK_ARGS(camMorphoMathsKernel,(dest->depth&CAM_DEPTH_MASK)>=8);
#endif
width=iROI.srcroi.width;
height=iROI.srcroi.height;
if (source->roi) {
left=iROI.srcroi.xOffset;
top=iROI.srcroi.yOffset;
nlb=left; if (nlb>CAM_MM_NEIGHB/2) nlb=CAM_MM_NEIGHB/2;
ntb=top; if (ntb>CAM_MM_NEIGHB/2) ntb=CAM_MM_NEIGHB/2;
srcptr=(CAM_PIXEL*)(source->imageData+iROI.srcchoffset+(top-ntb)*source->widthStep)+(left-nlb);
} else {
srcptr=(CAM_PIXEL*)(source->imageData+iROI.srcchoffset);
left=0;
top=0;
}
dstptr=(CAM_PIXEL*)iROI.dstptr;
CAM_CHECK_ARGS(camMorphoMathsKernel,(width>CAM_MM_NEIGHB/2));
CAM_CHECK_ARGS(camMorphoMathsKernel,(height>CAM_MM_NEIGHB/2));
// Mask management
INIT_MASK_MANAGEMENT;
// Initialize algorithm
for (i=0;i<CAM_MM_NEIGHB;i++) {
linesPtr[i]=linesBuffer[i];
}
// Initialize neighbourhood
// Fill the top lines
for (y=0;y+top<CAM_MM_NEIGHB/2;y++) {
// Out of frame : fill with border color
if (source->borderMode[CAM_SIDE_TOP_INDEX]==CAM_BORDER_REPLICATE) {
cpsrcptr=srcptr;
for (x=0;x<CAM_MM_NEIGHB/2;x++) {
linesPtr[y][x]=*srcptr;
}
for (;x<width+CAM_MM_NEIGHB/2;x++) {
linesPtr[y][x]=*srcptr;
srcptr+=iROI.srcinc;
}
for (;x<width+CAM_MM_NEIGHB-1;x++) {
linesPtr[y][x]=*(srcptr-iROI.srcinc);
}
srcptr=cpsrcptr;
} else {
for (x=0;x<width+CAM_MM_NEIGHB-1;x++) {
linesPtr[y][x]=source->borderConst[CAM_SIDE_TOP_INDEX];
}
}
}
// Fill the next lines with image pixels
for (;y<CAM_MM_NEIGHB-1;y++) {
cpsrcptr=srcptr;
for (x=0;x<CAM_MM_NEIGHB/2;x++) {
if (left+x-CAM_MM_NEIGHB/2<0) {
// Out of frame : fill with border color
if (source->borderMode[CAM_SIDE_LEFT_INDEX]==CAM_BORDER_REPLICATE) {
linesPtr[y][x]=*srcptr;
} else {
linesPtr[y][x]=source->borderConst[CAM_SIDE_LEFT_INDEX];
}
} else {
// Get border pixels from the source frame
linesPtr[y][x]=*srcptr;
srcptr+=iROI.srcinc;
}
}
for (;x<width+CAM_MM_NEIGHB/2;x++) {
linesPtr[y][x]=*srcptr;
srcptr+=iROI.srcinc;
}
for (;x<width+CAM_MM_NEIGHB-1;x++) {
if (left+x-CAM_MM_NEIGHB/2<source->width) {
// Get border pixels from the source frame
linesPtr[y][x]=*srcptr;
srcptr+=iROI.srcinc;
} else {
// Out of frame : fill with border color
if (source->borderMode[CAM_SIDE_RIGHT_INDEX]==CAM_BORDER_REPLICATE) {
linesPtr[y][x]=*(srcptr-iROI.srcinc);
} else {
linesPtr[y][x]=source->borderConst[CAM_SIDE_RIGHT_INDEX];
}
}
}
srcptr=(CAM_PIXEL*)(((char*)cpsrcptr)+source->widthStep);
}
// Now process the whole image
// This is the main loop
for (y=0;y<height;y++) {
firsttime=1;
cpsrcptr=srcptr;
cpdstptr=dstptr;
// Start a new line
// Have we reached the bottom of the frame ?
if (top+y+CAM_MM_NEIGHB/2>=source->height) {
if (source->borderMode[CAM_SIDE_BOTTOM_INDEX]==CAM_BORDER_REPLICATE) {
// Go up one line (in order to stay on the last line)
cpsrcptr=(CAM_PIXEL*)(((char*)cpsrcptr)-source->widthStep);
srcptr=cpsrcptr;
for (x=0;x<CAM_MM_NEIGHB/2;x++) {
linesPtr[CAM_MM_NEIGHB-1][x]=*srcptr;
}
for (;x<width+CAM_MM_NEIGHB/2;x++) {
linesPtr[CAM_MM_NEIGHB-1][x]=*srcptr;
srcptr+=iROI.srcinc;
}
for (;x<width+CAM_MM_NEIGHB-1;x++) {
linesPtr[CAM_MM_NEIGHB-1][x]=*(srcptr-iROI.srcinc);
}
} else {
for (x=0;x<width+CAM_MM_NEIGHB-1;x++) {
linesPtr[CAM_MM_NEIGHB-1][x]=source->borderConst[CAM_SIDE_BOTTOM_INDEX];
}
}
} else {
for (x=0;x<CAM_MM_NEIGHB/2;x++) {
if (left+x-CAM_MM_NEIGHB/2<0) {
// Out of frame : fill with border color
if (source->borderMode[CAM_SIDE_LEFT_INDEX]==CAM_BORDER_REPLICATE) {
linesPtr[CAM_MM_NEIGHB-1][x]=*srcptr;
} else {
linesPtr[CAM_MM_NEIGHB-1][x]=source->borderConst[CAM_SIDE_LEFT_INDEX];
}
} else {
// Get border pixels from the source frame
linesPtr[CAM_MM_NEIGHB-1][x]=*srcptr;
srcptr+=iROI.srcinc;
}
}
// Fast transfer with memcpy
if (iROI.srcinc==1) {
memcpy(&linesPtr[CAM_MM_NEIGHB-1][x],srcptr,(source->width-left+CAM_MM_NEIGHB/2-x)<<(sizeof(CAM_PIXEL)-1));
x=source->width-left+CAM_MM_NEIGHB/2;
} else {
for (;x<source->width-left+CAM_MM_NEIGHB/2;x++) {
// Get a pixel from the source frame
linesPtr[CAM_MM_NEIGHB-1][x]=*srcptr;
srcptr+=iROI.srcinc;
}
}
for (;x<width+CAM_MM_NEIGHB-1;x++) {
// Out of frame : fill with border color
if (source->borderMode[CAM_SIDE_RIGHT_INDEX]==CAM_BORDER_REPLICATE) {
linesPtr[CAM_MM_NEIGHB-1][x]=*(srcptr+(source->width-1)*iROI.srcinc);
} else {
linesPtr[CAM_MM_NEIGHB-1][x]=source->borderConst[CAM_SIDE_RIGHT_INDEX];
}
}
}
if (source->depth & CAM_DEPTH_SIGN) {
#define linesPtr ((signed CAM_PIXEL**) linesPtr)
BEGIN_MASK_MANAGEMENT(dstptr=cpdstptr+startx*iROI.dstinc;)
// Process all the pixels in the line
for (x=startx+CAM_MM_NEIGHB-1;x<endx+CAM_MM_NEIGHB-1;x++) {
#ifdef CAM_MM_DO_EROSION
{
#define tabX tabero
#define valX valero
#define XOP <
#ifdef CAM_MM_OPT_CIRCLE7
#include "cam_morphomaths_code_circle7.c"
#else
#ifdef CAM_MM_OPT_CIRCLE5
#include "cam_morphomaths_code_circle5.c"
#else
#include "cam_morphomaths_code_square3.c"
#endif // CAM_MM_OPT_CIRCLE5
#endif // CAM_MM_OPT_CIRCLE7
#undef tabX
#undef valX
#undef XOP
}
#endif // CAM_MM_DO_EROSION
#ifdef CAM_MM_DO_DILATION
{
#define tabX tabdil
#define valX valdil
#define XOP >
#ifdef CAM_MM_OPT_CIRCLE7
#include "cam_morphomaths_code_circle7.c"
#else
#ifdef CAM_MM_OPT_CIRCLE5
#include "cam_morphomaths_code_circle5.c"
#else
#include "cam_morphomaths_code_square3.c"
#endif // CAM_MM_OPT_CIRCLE5
#endif // CAM_MM_OPT_CIRCLE7
#undef tabX
#undef valX
#undef XOP
}
#endif // CAM_MM_DO_DILATION
firsttime=0;
#ifdef CAM_MM_FINDLOCALMAXIMA
valcenter = linesPtr[CAM_MM_NEIGHB / 2][x - CAM_MM_NEIGHB + 1 + CAM_MM_NEIGHB / 2];
if (valdil[0] == valcenter && valcenter >= threshold && valcenter != 0) {
// Check that it is the only point with this maximum (in the upper part)
for (yp = 0, zp = 0, found = 0; yp < lines2test; yp++) {
for (xp = start2test[yp]; xp <= end2test[yp]; xp++) {
if (linesPtr[yp][x - CAM_MM_NEIGHB + 1 + xp] == valcenter) {
found = 1;
break;
}
}
}
if (!found) {
// Keep this point : this is a local maximum
if (points->nbPoints < points->allocated) {
points->keypoint[points->nbPoints]->x = x - CAM_MM_NEIGHB + 1;
points->keypoint[points->nbPoints]->y = y;
points->keypoint[points->nbPoints]->scale = 0;
points->keypoint[points->nbPoints]->angle = 0;
points->keypoint[points->nbPoints]->value = valcenter;
points->nbPoints++;
acc += valdil[0];
} else {
camInternalROIPolicyExit(&iROI);
return 0;
}
}
}
#else
#ifdef CAM_MM_GRADIENT
acc+=(*dstptr=valdil[0]-valero[0]);
#else
#ifdef CAM_MM_DO_EROSION
acc+=(*dstptr=valero[0]);
#else
acc+=(*dstptr=valdil[0]);
#endif
#endif // CAM_MM_GRADIENT
dstptr+=iROI.dstinc;
#endif // CAM_MM_FINDLOCALMAXIMA
}
END_MASK_MANAGEMENT;
#undef linesPtr
} else {
int camFillColor(CamImage *image, int x, int y, int fillcolor, int tolerance)
{
int first=0,last=0;
int i,j,d,xp,yp;
CAM_PIXEL *ptr,*ptrx;
const int nx[4]={-1,0,+1,0},ny[4]={0,-1,0,+1};
CAM_PIXEL pcolor[4],initcolor[4]; // 4 is the maximum number of channels
CamInternalROIPolicyStruct iROI;
int acc=1;
int queuex[FIFO_SIZE];
int queuey[FIFO_SIZE];
// ROI (Region Of Interest) management
CAM_CHECK(camFillColor,camInternalROIPolicy(image, NULL, &iROI, 0));
CAM_CHECK_ARGS(camFillColor, ((iROI.nChannels==1)||(image->dataOrder==CAM_DATA_ORDER_PIXEL)));
if ((x>=iROI.srcroi.xOffset)&&(y>=iROI.srcroi.yOffset)&&(x<iROI.srcroi.xOffset+iROI.srcroi.width)&&(y<iROI.srcroi.yOffset+iROI.srcroi.height)) {
for (i=0;i<iROI.nChannels;i++) {
pcolor[i]=(fillcolor>>(i*8))&0xff;
}
ptr=ptrx=(CAM_PIXEL*)(image->imageData+iROI.srcchoffset+y*image->widthStep)+x*iROI.srcinc;
if (tolerance>=0) {
for (i=0;i<iROI.nChannels;i++) {
initcolor[i]=*ptrx++;
}
FIFO_ADD(x,y);
for (ptrx=ptr,i=0;i<iROI.nChannels;i++,ptrx++) {
*ptrx=pcolor[i];
}
while (!FIFO_EMPTY()) {
x=queuex[first];
y=queuey[first];
FIFO_NEXT();
for (j=0;j<4;j++) {
xp=x+nx[j];
yp=y+ny[j];
if ((xp>=iROI.srcroi.xOffset)&&(yp>=iROI.srcroi.yOffset)&&(xp<iROI.srcroi.xOffset+iROI.srcroi.width)&&(yp<iROI.srcroi.yOffset+iROI.srcroi.height)) {
// Get the color at (xp,yp)
ptr=ptrx=(CAM_PIXEL*)(image->imageData+iROI.srcchoffset+yp*image->widthStep)+xp*iROI.srcinc;
// Is it the same color as the initial color?
// Compute distance between colors
d=0;
for (i=0;i<iROI.nChannels;i++,ptrx++) {
if (*ptrx>initcolor[i]) d+=*ptrx-initcolor[i];
else d+=initcolor[i]-*ptrx;
}
if (d<=tolerance) {
// Yes, then this pixel should be repainted and added to the queue
FIFO_ADD(xp,yp);
for (ptrx=ptr,i=0;i<iROI.nChannels;i++,ptrx++) {
*ptrx=pcolor[i];
}
acc++;
}
}
}
}
} else {
FIFO_ADD(x,y);
for (ptrx=ptr,i=0;i<iROI.nChannels;i++,ptrx++) {
*ptrx=pcolor[i];
}
while (!FIFO_EMPTY()) {
x=queuex[first];
y=queuey[first];
FIFO_NEXT();
for (j=0;j<4;j++) {
xp=x+nx[j];
yp=y+ny[j];
if ((xp>=iROI.srcroi.xOffset)&&(yp>=iROI.srcroi.yOffset)&&(xp<iROI.srcroi.xOffset+iROI.srcroi.width)&&(yp<iROI.srcroi.yOffset+iROI.srcroi.height)) {
// Get the color at (xp,yp)
ptr=ptrx=(CAM_PIXEL*)(image->imageData+iROI.srcchoffset+yp*image->widthStep)+xp*iROI.srcinc;
for (i=0;i<iROI.nChannels;i++,ptrx++) if (*ptrx!=pcolor[i]) break;
// Is it the same color as the fill color?
if (i!=iROI.nChannels) {
// Yes, then this pixel should be repainted and added to the queue
FIFO_ADD(xp,yp);
for (ptrx=ptr,i=0;i<iROI.nChannels;i++,ptrx++) {
*ptrx=pcolor[i];
}
acc++;
}
}
}
}
}
}
return acc;
}
int camDrawText16s(CamImage *image, char *text, int x, int y, int cwidth, int cheight, int orientation, int color)
{
static const int characters_table[][16]={
{1,1,1,1,1,1,1,1,0,0,1,0,0,0,1,0}, /* 0 */
{1,0,0,0,1,1,0,0,0,1,0,0,0,1,0,0}, /* {0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0}, */
{1,1,1,0,1,1,1,0,0,0,0,1,0,0,0,1},
{1,1,1,1,1,1,0,0,0,0,0,1,0,0,0,0},
{0,0,0,0,0,0,0,1,0,1,0,1,0,1,0,1},
{1,1,0,1,1,1,0,1,0,0,0,1,0,0,0,1},
{1,1,0,1,1,1,1,1,0,0,0,1,0,0,0,1},
{1,1,0,0,0,0,0,0,0,0,1,0,0,0,1,0},
{1,1,1,1,1,1,1,1,0,0,0,1,0,0,0,1},
{1,1,1,1,1,1,0,1,0,0,0,1,0,0,0,1}, /* 9 */
{1,1,1,1,0,0,1,1,0,0,0,1,0,0,0,1}, /* A */
{1,1,1,1,1,1,0,0,0,1,0,1,0,1,0,0},
{1,1,0,0,1,1,1,1,0,0,0,0,0,0,0,0},
{1,1,1,1,1,1,0,0,0,1,0,0,0,1,0,0},
{1,1,0,0,1,1,1,1,0,0,0,0,0,0,0,1},
{1,1,0,0,0,0,1,1,0,0,0,0,0,0,0,1},
{1,1,0,1,1,1,1,1,0,0,0,1,0,0,0,0},
{0,0,1,1,0,0,1,1,0,0,0,1,0,0,0,1},
{0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,0},
{0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,1,1,0,0,1,0,1,0,0,1},
{0,0,0,0,1,1,1,1,0,0,0,0,0,0,0,0},
{0,0,1,1,0,0,1,1,1,0,1,0,0,0,0,0},
{0,0,1,1,0,0,1,1,1,0,0,0,1,0,0,0},
{1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0},
{1,1,1,0,0,0,1,1,0,0,0,1,0,0,0,1},
{1,1,1,1,1,1,1,1,0,0,0,0,1,0,0,0},
{1,1,1,0,0,0,1,1,0,0,0,1,1,0,0,1},
{1,1,0,1,1,1,0,1,0,0,0,1,0,0,0,1},
{1,1,0,0,0,0,0,0,0,1,0,0,0,1,0,0},
{0,0,1,1,1,1,1,1,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,1,1,0,0,1,0,0,0,1,0},
{0,0,1,1,0,0,1,1,0,0,0,0,1,0,1,0},
{0,0,0,0,0,0,0,0,1,0,1,0,1,0,1,0},
{0,0,0,0,0,0,0,0,1,0,1,0,0,1,0,0},
{1,1,0,0,1,1,0,0,0,0,1,0,0,0,1,0}, /* Z */
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}};
static const int segments[16][4]={
{0,0,1,0},{1,0,2,0},{2,0,2,1},{2,1,2,2},
{1,2,2,2},{0,2,1,2},{0,1,0,2},{0,0,0,1},
{0,0,1,1},{1,0,1,1},{2,0,1,1},{2,1,1,1},
{2,2,1,1},{1,2,1,1},{0,2,1,1},{0,1,1,1}
};
int i,j,l;
int xp[3],yp[3];
const int *character;
int special_character;
char carac;
CAM_CHECK_ARGS(camDrawText16s,image->imageData!=NULL);
if (orientation) {
xp[0]=0; xp[1]=2*cwidth/5; xp[2]=4*cwidth/5;
yp[0]=cheight; yp[1]=cheight-cheight/3; yp[2]=cheight-2*cheight/3;
} else {
xp[0]=0; xp[1]=cwidth/3; xp[2]=2*cwidth/3;
yp[0]=0; yp[1]=2*cheight/5; yp[2]=4*cheight/5;
}
l=strlen(text);
for (i=0;i<l;i++) {
if (orientation) {
carac=text[l-1-i];
} else {
carac=text[i];
}
special_character=0;
if ((carac>='0')&&(carac<='9')) {
character=characters_table[carac-'0'];
} else if ((carac>='A')&&(carac<='Z')) {
character=characters_table[carac-'A'+10];
} else if ((carac>='a')&&(carac<='z')) {
character=characters_table[carac-'a'+10];
} else if (carac==' ') {
/* Space character */
special_character=1;
} else if (carac==':') {
special_character=1;
if (orientation) {
camDrawLine(image,x+xp[1],y+yp[1],x+xp[1],y+yp[1],color);
camDrawLine(image,x+xp[2],y+yp[1],x+xp[2],y+yp[1],color);
} else {
camDrawLine(image,x+xp[1],y+yp[1],x+xp[1],y+yp[1],color);
camDrawLine(image,x+xp[1],y+yp[2],x+xp[1],y+yp[2],color);
}
} else if (carac=='.') {
special_character=1;
if (orientation) {
camDrawLine(image,x+xp[2],y+yp[1],x+xp[2],y+yp[1],color);
} else {
camDrawLine(image,x+xp[1],y+yp[2],x+xp[1],y+yp[2],color);
}
} else if (carac=='=') {
special_character=1;
if (orientation) {
camDrawLine(image,x+xp[1],y+yp[0],x+xp[1],y+yp[2],color);
camDrawLine(image,x+xp[2],y+yp[0],x+xp[2],y+yp[2],color);
} else {
camDrawLine(image,x+xp[0],y+yp[1],x+xp[2],y+yp[1],color);
camDrawLine(image,x+xp[0],y+yp[2],x+xp[2],y+yp[2],color);
}
} else if (carac=='-') {
special_character=1;
if (orientation) {
camDrawLine(image,x+xp[1],y+yp[0],x+xp[1],y+yp[2],color);
} else {
camDrawLine(image,x+xp[0],y+yp[1],x+xp[2],y+yp[1],color);
}
}
if (!special_character) {
if (orientation) {
for (j=0;j<16;j++) {
if (character[j]) {
camDrawLine(image,x+xp[segments[j][1]],y+yp[segments[j][0]],x+xp[segments[j][3]],y+yp[segments[j][2]],color);
}
}
} else {
for (j=0;j<16;j++) {
if (character[j]) {
camDrawLine(image,x+xp[segments[j][0]],y+yp[segments[j][1]],x+xp[segments[j][2]],y+yp[segments[j][3]],color);
}
}
}
}
if (orientation) {
y+=cheight;
} else {
x+=cwidth;
}
}
return 1;
}
int camAccumulateLine(CamImage *image, int x1, int y1, int x2, int y2, int accumulator)
{
int dx, dy, xincr, yincr, x, y, xp, yp, i, j;
int inc, error, correction, acc, temp, ptrincr, runl, runl2;
CAM_PIXEL *ptrX;
CamInternalROIPolicyStruct iROI;
#ifdef CAM_DEBUG
int valmax=(1<<(sizeof(CAM_PIXEL)*8))-1;
int value;
#endif
// ROI (Region Of Interest) management
CAM_CHECK(camAccumulateLine,camInternalROIPolicy(image, NULL, &iROI, 0));
CAM_CHECK_ARGS(camAccumulateLine,iROI.nChannels==1);
CAM_CHECK_ARGS2(camAccumulateLine,(image->depth&CAM_DEPTH_SIGN)==0,"Supports only unsigned images");
/* Clipping */
if (x1<x2) {
x=x1; y=y1; xp=x2; yp=y2;
} else {
x=x2; y=y2; xp=x1; yp=y1;
}
if (x<iROI.srcroi.xOffset) {
if (x==xp) return 1;
y=y+(y-yp)*(iROI.srcroi.xOffset-x)/(x-xp);
x=iROI.srcroi.xOffset;
if (xp<iROI.srcroi.xOffset) return 1; /* The line is completely outside the ROI */
}
if (xp>=(iROI.srcroi.xOffset+iROI.srcroi.width)) {
if (x==xp) return 1;
yp=y+(y-yp)*((iROI.srcroi.xOffset+iROI.srcroi.width)-x-1)/(x-xp);
xp=(iROI.srcroi.xOffset+iROI.srcroi.width)-1;
if (x>=(iROI.srcroi.xOffset+iROI.srcroi.width)) return 1; /* The line is completely outside the ROI */
}
if (y<yp) {
x1=x; y1=y; x2=xp; y2=yp;
} else {
x1=xp; y1=yp; x2=x; y2=y;
}
if (y1<iROI.srcroi.yOffset) {
if (y1==y2) return 1;
x1=x1+(x1-x2)*(iROI.srcroi.yOffset-y1)/(y1-y2);
y1=iROI.srcroi.yOffset;
if (y2<iROI.srcroi.yOffset) return 1; /* The line is completely outside the ROI */
}
if (y2>=(iROI.srcroi.yOffset+iROI.srcroi.height)) {
if (y1==y2) return 1;
x2=x1+(x1-x2)*((iROI.srcroi.yOffset+iROI.srcroi.height)-y1-1)/(y1-y2);
y2=(iROI.srcroi.yOffset+iROI.srcroi.height)-1;
if (y1>=(iROI.srcroi.yOffset+iROI.srcroi.height)) return 1; /* The line is completely outside the ROI */
}
#define LINE
#ifdef CAM_DEBUG
#define SETPIXEL value=*ptrX; value+=accumulator; if ((value<0)||(value>valmax)) {camError("camAccumulateLane","Saturation"); return 0;} else *ptrX=(CAM_PIXEL)value;
#else
#define SETPIXEL *ptrX+=accumulator;
#endif
#define INITPOINTERS ptrX=((CAM_PIXEL*)(image->imageData+iROI.srcchoffset+y*image->widthStep))+x*iROI.srcinc
#include "cam_draw_code.c"
#undef INITPOINTERS
#undef SETPIXEL
#undef LINE
return 1;
}
int camIntegralImage(CamImage *src, CamImage *dest)
{
int x, y;
int width, height;
CAM_PIXEL *srcptr, *srctmpptr;
unsigned long *dstptr, *dsttmpptr;
CamInternalROIPolicyStruct iROI;
unsigned long val;
#ifdef CAM_VECTORIZE
int i;
union i4vector v1, v2;
#endif
CAM_CHECK_ARGS2(camIntegralImage, src->imageData != NULL, "source image is not allocated");
if (dest->imageData==NULL) {
// Automatic allocation
camAllocateImage(dest, src->width, src->height, CAM_DEPTH_32U);
}
CAM_CHECK(camIntegralImage, camInternalROIPolicy(src, dest, &iROI, 0));
CAM_CHECK_ARGS(camIntegralImage, iROI.nChannels == 1);
CAM_CHECK_ARGS(camIntegralImage, (src->depth & CAM_DEPTH_MASK) >= 8);
CAM_CHECK_ARGS(camIntegralImage, (src->depth & CAM_DEPTH_MASK) <= (sizeof(CAM_PIXEL) * 8));
CAM_CHECK_ARGS(camIntegralImage, (dest->depth & CAM_DEPTH_MASK) == 32);
CAM_CHECK_ARGS(camIntegralImage, !(src->depth & CAM_DEPTH_SIGN));
/*
// One pass algorithm
width = iROI.srcroi.width;
height = iROI.srcroi.height;
srcptr = (CAM_PIXEL*)iROI.srcptr;
dstptr = (unsigned long*)iROI.dstptr;
srctmpptr = srcptr;
dsttmpptr = dstptr;
val = 0;
for (x = 0; x < width; x++, srcptr += iROI.srcinc, dstptr++) {
val += *srcptr;
*dstptr = val;
}
srcptr = (CAM_PIXEL*)(((char*)srctmpptr) + src->widthStep);
dstptr = (unsigned long*)(((char*)dsttmpptr) + dest->widthStep);
for (y = 1; y < height; y++) {
srctmpptr = srcptr;
dsttmpptr = dstptr;
val = 0;
for (x = 0; x < width; x++, srcptr += iROI.srcinc, dstptr++) {
val += *srcptr;
*dstptr = val + *(unsigned long*)(((char*)dstptr) - dest->widthStep);
}
srcptr = (CAM_PIXEL*)(((char*)srctmpptr) + src->widthStep);
dstptr = (unsigned long*)(((char*)dsttmpptr) + dest->widthStep);
}
*/
// Two passes algorithm
// First pass
width = iROI.srcroi.width;
height = iROI.srcroi.height;
srcptr = (CAM_PIXEL*)iROI.srcptr;
dstptr = (unsigned long*)iROI.dstptr;
for (y = 0; y < height; y++) {
srctmpptr = srcptr;
dsttmpptr = dstptr;
val = 0;
for (x = 0; x < width; x++, srcptr += iROI.srcinc, dstptr++) {
val += *srcptr;
*dstptr = val;
}
srcptr = (CAM_PIXEL*)(((char*)srctmpptr) + src->widthStep);
dstptr = (unsigned long*)(((char*)dsttmpptr) + dest->widthStep);
}
#ifndef CAM_VECTORIZE
// Second pass
dstptr = (unsigned long*)iROI.dstptr;
dstptr = (unsigned long*)(((char*)dstptr) + dest->widthStep);
for (y = 1; y < height; y++) {
dsttmpptr = dstptr;
for (x = 0; x < width; x++, dstptr++) {
*dstptr += *(unsigned long*)(((char*)dstptr) - dest->widthStep);
}
dstptr = (unsigned long*)(((char*)dsttmpptr) + dest->widthStep);
}
#else
// Second pass / SSE2 vectorized
dstptr = (unsigned long*)iROI.dstptr;
dstptr = (unsigned long*)(((char*)dstptr) + dest->widthStep);
for (y = 1; y < height; y++) {
dsttmpptr = dstptr;
for (x = 0; x < width; x+=4) {
// Add 4 pixels at once
for (i = 0; i != 4; i++, dstptr++) {
v1.i[i] = *dstptr;
v2.i[i] = *(unsigned long*)(((char*)dstptr) - dest->widthStep);
}
v1.v = v1.v + v2.v;
dstptr -= (iROI.dstinc<<2);
for (i = 0; i != 4; i++, dstptr++) *dstptr = v1.i[i];
}
for (; x < width; x++, dstptr += iROI.dstinc) {
*dstptr += *(unsigned long*)(((char*)dstptr) - dest->widthStep);
}
dstptr = (unsigned long*)(((char*)dsttmpptr) + dest->widthStep);
}
#endif
camInternalROIPolicyExit(&iROI);
return 1;
}
int camMedianFilter(CamImage *source, CamImage *dest)
{
int i,j,x,y,xp,yp;
int width,height;
int value,result;
int left,top;
CAM_PIXEL *srcptr,*dstptr,*tmpptr,*cpsrcptr,*cpdstptr;
CAM_PIXEL *linesPtr[CAM_MF_NEIGHB];
CAM_PIXEL linesBuffer[CAM_MF_NEIGHB][CAM_MAX_SCANLINE+CAM_MF_NEIGHB-1];
// Data for sorting pixels in the neighbourhood
int values[CAM_MF_NEIGHB*CAM_MF_NEIGHB];
int next[CAM_MF_NEIGHB*CAM_MF_NEIGHB];
int first,last,nb,prev;
CamInternalROIPolicyStruct iROI;
// ROI (Region Of Interest) management
CAM_CHECK(camMedianFilter,camInternalROIPolicy(source, dest, &iROI, 1));
CAM_CHECK_ARGS(camMedianFilter,iROI.nChannels==1);
CAM_CHECK_ARGS(camMedianFilter,(source->depth&CAM_DEPTH_MASK)<=(sizeof(CAM_PIXEL)*8));
CAM_CHECK_ARGS(camMedianFilter,(source->depth&CAM_DEPTH_MASK)>=8);
CAM_CHECK_ARGS(camMedianFilter,(dest->depth&CAM_DEPTH_MASK)<=(sizeof(CAM_PIXEL)*8));
CAM_CHECK_ARGS(camMedianFilter,(dest->depth&CAM_DEPTH_MASK)>=8);
width=iROI.srcroi.width;
height=iROI.srcroi.height;
if (source->roi) {
left=iROI.srcroi.xOffset;
top=iROI.srcroi.yOffset;
i=left; if (i>CAM_MF_NEIGHB/2) i=CAM_MF_NEIGHB/2;
j=top; if (j>CAM_MF_NEIGHB/2) j=CAM_MF_NEIGHB/2;
srcptr=(CAM_PIXEL*)(source->imageData+iROI.srcchoffset+(top-j)*source->widthStep)+(left-i);
} else {
srcptr=(CAM_PIXEL*)(source->imageData+iROI.srcchoffset);
left=0;
top=0;
}
dstptr=(CAM_PIXEL*)iROI.dstptr;
CAM_CHECK_ARGS(camMedianFilter,(width>CAM_MF_NEIGHB/2));
CAM_CHECK_ARGS(camMedianFilter,(height>CAM_MF_NEIGHB/2));
// Initialize algorithm
for (i=0;i<CAM_MF_NEIGHB;i++) {
linesPtr[i]=linesBuffer[i];
}
// Initialize neighbourhood
// Fill the top lines
for (y=0;y+top<CAM_MF_NEIGHB/2;y++) {
// Out of frame : fill with border color
if (source->borderMode[CAM_SIDE_TOP_INDEX]==CAM_BORDER_REPLICATE) {
cpsrcptr=srcptr;
for (x=0;x<CAM_MF_NEIGHB/2;x++) {
linesPtr[y][x]=*srcptr;
}
for (;x<width+CAM_MF_NEIGHB/2;x++) {
linesPtr[y][x]=*srcptr;
srcptr+=iROI.srcinc;
}
for (;x<width+CAM_MF_NEIGHB-1;x++) {
linesPtr[y][x]=*(srcptr-iROI.srcinc);
}
srcptr=cpsrcptr;
} else {
for (x=0;x<width+CAM_MF_NEIGHB-1;x++) {
linesPtr[y][x]=source->borderConst[CAM_SIDE_TOP_INDEX];
}
}
}
// Fill the next lines with image pixels
for (;y<CAM_MF_NEIGHB-1;y++) {
cpsrcptr=srcptr;
for (x=0;x<CAM_MF_NEIGHB/2;x++) {
if (left+x-CAM_MF_NEIGHB/2<0) {
// Out of frame : fill with border color
if (source->borderMode[CAM_SIDE_LEFT_INDEX]==CAM_BORDER_REPLICATE) {
linesPtr[y][x]=*srcptr;
} else {
linesPtr[y][x]=source->borderConst[CAM_SIDE_LEFT_INDEX];
}
} else {
// Get border pixels from the source frame
linesPtr[y][x]=*srcptr;
srcptr+=iROI.srcinc;
}
}
for (;x<width+CAM_MF_NEIGHB/2;x++) {
linesPtr[y][x]=*srcptr;
srcptr+=iROI.srcinc;
}
for (;x<width+CAM_MF_NEIGHB-1;x++) {
if (left+x-CAM_MF_NEIGHB/2<source->width) {
// Get border pixels from the source frame
linesPtr[y][x]=*srcptr;
srcptr+=iROI.srcinc;
} else {
// Out of frame : fill with border color
if (source->borderMode[CAM_SIDE_RIGHT_INDEX]==CAM_BORDER_REPLICATE) {
linesPtr[y][x]=*(srcptr-iROI.srcinc);
} else {
linesPtr[y][x]=source->borderConst[CAM_SIDE_RIGHT_INDEX];
}
}
}
srcptr=(CAM_PIXEL*)(((char*)cpsrcptr)+source->widthStep);
}
// Now process the whole image
// This is the main loop
for (y=0;y<height;y++) {
cpsrcptr=srcptr;
cpdstptr=dstptr;
// Start a new line
// Have we reached the bottom of the frame ?
if (top+y+CAM_MF_NEIGHB/2>=source->height) {
if (source->borderMode[CAM_SIDE_BOTTOM_INDEX]==CAM_BORDER_REPLICATE) {
// Go up one line (in order to stay on the last line)
cpsrcptr=(CAM_PIXEL*)(((char*)cpsrcptr)-source->widthStep);
srcptr=cpsrcptr;
for (x=0;x<CAM_MF_NEIGHB/2;x++) {
linesPtr[CAM_MF_NEIGHB-1][x]=*srcptr;
}
for (;x<width+CAM_MF_NEIGHB/2;x++) {
linesPtr[CAM_MF_NEIGHB-1][x]=*srcptr;
srcptr+=iROI.srcinc;
}
for (;x<width+CAM_MF_NEIGHB-1;x++) {
linesPtr[CAM_MF_NEIGHB-1][x]=*(srcptr-iROI.srcinc);
}
} else {
for (x=0;x<width+CAM_MF_NEIGHB-1;x++) {
linesPtr[CAM_MF_NEIGHB-1][x]=source->borderConst[CAM_SIDE_BOTTOM_INDEX];
}
}
} else {
for (x=0;x<CAM_MF_NEIGHB/2;x++) {
if (left+x-CAM_MF_NEIGHB/2<0) {
// Out of frame : fill with border color
if (source->borderMode[CAM_SIDE_LEFT_INDEX]==CAM_BORDER_REPLICATE) {
linesPtr[CAM_MF_NEIGHB-1][x]=*srcptr;
} else {
linesPtr[CAM_MF_NEIGHB-1][x]=source->borderConst[CAM_SIDE_LEFT_INDEX];
}
} else {
// Get border pixels from the source frame
linesPtr[CAM_MF_NEIGHB-1][x]=*srcptr;
srcptr+=iROI.srcinc;
}
}
// Fast transfer with memcpy
if (iROI.srcinc==1) {
memcpy(&linesPtr[CAM_MF_NEIGHB-1][x],srcptr,(source->width-left+CAM_MF_NEIGHB/2-x)<<(sizeof(CAM_PIXEL)-1));
x=source->width-left+CAM_MF_NEIGHB/2;
} else {
for (;x<source->width-left+CAM_MF_NEIGHB/2;x++) {
// Get a pixel from the source frame
linesPtr[CAM_MF_NEIGHB-1][x]=*srcptr;
srcptr+=iROI.srcinc;
}
}
for (;x<width+CAM_MF_NEIGHB-1;x++) {
// Out of frame : fill with border color
if (source->borderMode[CAM_SIDE_RIGHT_INDEX]==CAM_BORDER_REPLICATE) {
linesPtr[CAM_MF_NEIGHB-1][x]=*(srcptr+(source->width-1)*iROI.srcinc);
} else {
linesPtr[CAM_MF_NEIGHB-1][x]=source->borderConst[CAM_SIDE_RIGHT_INDEX];
}
}
}
// Process all the pixels in the line
for (x=CAM_MF_NEIGHB-1;x<width+CAM_MF_NEIGHB-1;x++) {
// Now, let's sort the pixels in the neighbourhood
first=0;
last=0;
nb=1;
i=x-CAM_MF_NEIGHB+1;
// Get the first pixel
value=linesPtr[0][i];
// And put it in the sorted list
values[0]=value;
next[0]=-1;
#define INSERT_VALUE(value) \
values[nb]=value; \
if (value>values[first]) { \
prev=first; \
j=next[first]; \
while ((j!=-1)&&(value>values[j])) { \
prev=j; \
j=next[j]; \
} \
if (j==-1) { \
next[last]=nb; \
last=nb; \
next[nb]=-1; \
} else { \
next[nb]=j; \
next[prev]=nb; \
} \
} else { \
next[nb]=first; \
first=nb; \
} \
nb++;
for (xp=1;xp<CAM_MF_NEIGHB;xp++) {
value=linesPtr[0][i+xp];
INSERT_VALUE(value);
}
for (yp=1;yp<CAM_MF_NEIGHB;yp++) {
for (xp=0;xp<CAM_MF_NEIGHB;xp++) {
value=linesPtr[yp][i+xp];
INSERT_VALUE(value);
}
}
// Retrieve the median value
j=first;
for (i=0;i<CAM_MF_NEIGHB*CAM_MF_NEIGHB/2;i++) {
j=next[j];
}
result=values[j];
// Store the result in destination image
*dstptr=result;
dstptr+=iROI.dstinc;
}
// Go to next line
srcptr=(CAM_PIXEL*)(((char*)cpsrcptr)+source->widthStep);
dstptr=(CAM_PIXEL*)(((char*)cpdstptr)+dest->widthStep);
// Reset neighborhood line pointers
tmpptr=linesPtr[0];
for (i=0;i<CAM_MF_NEIGHB-1;i++) {
linesPtr[i]=linesPtr[i+1];
}
linesPtr[CAM_MF_NEIGHB-1]=tmpptr;
}
camInternalROIPolicyExit(&iROI);
return 1;
}
int
camKeypointsRecursiveDetector(CamImage *source, CamImage *integral, CamKeypoints *points, int nb_max_keypoints, int options)
{
CamImage filter;
CamInternalROIPolicyStruct iROI;
CamROI *roi, roix;
int i, width, height;
CamKeypointShort *keypoints;
int pnb_keypoints;
int nb_keypoints = 0;
unsigned int *abs_value_lines[2];
int *value_lines[2];
unsigned char *scale_lines[2];
unsigned char *lmax_lines[2];
unsigned int *lmax[2], nblmax[2];
int widthStep;
camPatchSizeParam = 16; //32 * 2 / 3; // Equivalent optimal value wrt SURF (the descriptor can partially lie outside screen)
// Parameters checking
CAM_CHECK(camKeypointsRecursiveDetector, camInternalROIPolicy(source, NULL, &iROI, 1));
CAM_CHECK_ARGS(camKeypointsRecursiveDetector, (source->depth & CAM_DEPTH_MASK) >= 8);
CAM_CHECK_ARGS(camKeypointsRecursiveDetector, points->allocated >= 0);
CAM_CHECK_ARGS(camKeypointsRecursiveDetector, source->nChannels == 1 || ((source->nChannels == 3) && (source->dataOrder == CAM_DATA_ORDER_PLANE)));
width = iROI.srcroi.width;
height = iROI.srcroi.height;
if (!integral)
{
// Compute integral image
integral = (CamImage*)malloc(sizeof(CamImage));
integral->imageData = NULL;
roi = source->roi;
camSetMaxROI(&roix, source);
roix.coi = 1;
source->roi = &roix;
camIntegralImage(source, integral); // Computed on the whole frame, not only on ROI
integral->roi = &iROI.srcroi;
source->roi = roi;
}
widthStep = integral->widthStep / 4;
// Allocate temp memory for keypoints
keypoints = (CamKeypointShort*)malloc(CAM_MAX_KEYPOINTS * sizeof(CamKeypointShort));
points->nbPoints = 0;
// Allocate value and scale lines
for (i = 0; i < 2; i++) {
scale_lines[i] = (unsigned char*)malloc(width * sizeof(unsigned char));
value_lines[i] = (int*)malloc(width * sizeof(int));
abs_value_lines[i] = (unsigned int*)malloc(width * sizeof(unsigned int));
lmax_lines[i] = (unsigned char*)malloc(width * sizeof(unsigned char));
lmax[i] = (unsigned int*)malloc(width * sizeof(unsigned int));
nblmax[i] = 0;
}
// Go !
{
int y;
int max_scale_y;
unsigned int *abs_current_value_line, *abs_previous_value_line;
int *current_value_line, *previous_value_line;
unsigned char *current_scale_line, *previous_scale_line;
unsigned char *current_lmax_line, *previous_lmax_line;
unsigned int *current_lmax, *previous_lmax;
unsigned int current_nblmax, previous_nblmax;
current_value_line = value_lines[0];
previous_value_line = value_lines[1];
abs_current_value_line = abs_value_lines[0];
abs_previous_value_line = abs_value_lines[1];
current_scale_line = scale_lines[0];
previous_scale_line = scale_lines[1];
current_lmax_line = lmax_lines[0];
previous_lmax_line = lmax_lines[1];
current_lmax = lmax[0];
previous_lmax = lmax[1];
// Fill the previous value line with a dummy value
for (i = 0; i < width; i++) {
abs_previous_value_line[i] = 0;
previous_value_line[i] = 0;
previous_scale_line[i] = 1;
previous_lmax_line[i] = 0;
}
current_nblmax = 0;
previous_nblmax = 0;
for (y = 0; y < height; y++) {
int x, v;
// max_scale is the biggest scale we may apply. It must be more than 0, otherwise we can't do any image processing
max_scale_y = (y + iROI.srcroi.yOffset) >> 1;
// The preceding formula comes from :
// - When iROI.yOffset is 0, max_scale should be 1 when y is 2 (1 pixel margin due to integral image)
// - When iROI.yOffset is 0, max_scale should be 1 when y is 3 (1 pixel margin due to integral image)
// - When iROI.yOffset is 0, max_scale should be 2 when y is 4 (1 pixel margin due to integral image)
// Check max_scale with the bottom line
v = (source->height - 1 - (y + iROI.srcroi.yOffset)) >> 1;
// The preceding formula comes from "When iROI.yOffset is 0, max_scale should be 1 when y is equal to source->height - 3
if (v < max_scale_y) max_scale_y = v;
if (max_scale_y < 1) {
// We can't compute anything for that line
for (i = 0; i < width; i++) {
current_value_line[i] = 0;
current_scale_line[i] = 1;
}
} else {
int scale_up = 1;
int current_value = 0;
unsigned int abs_current_value = 0;
int current_scale = 1;
unsigned int *ptr = ((unsigned int*)(integral->imageData + (y + iROI.srcroi.yOffset) * integral->widthStep)) + iROI.srcroi.xOffset;
int s1, s2;
for (x = 0; x < width; x++, ptr++) {
int first_scale = current_scale;
int max_scale = max_scale_y;
// Check max_scale with the left side of the frame
v = (x + iROI.srcroi.xOffset) >> 1;
if (v < max_scale) max_scale = v;
// Check max_scale with the right side of the frame
v = (source->width -1 - (x + iROI.srcroi.xOffset)) >> 1;
if (v < max_scale) max_scale = v;
// We need a scale margin in order to compute the descriptor
max_scale -= CAM_SCALE_MARGIN;
// Choose the first scale to evaluate, depending on upper and left results
if (abs_previous_value_line[x] > abs_current_value)
first_scale = previous_scale_line[x];
if (first_scale <= max_scale) {
// Evaluate at first scale
int scale = first_scale; // alias
int yoffset = scale * widthStep;
int value;
#define CAM_RECURSIVE_PATTERN \
{ \
unsigned int *ptrAi = ptr - (yoffset + scale); \
unsigned int *ptrDi = ptr + (yoffset + scale); \
unsigned int *ptrBi = ptr - (yoffset - scale); \
unsigned int *ptrCi = ptr + (yoffset - scale); \
unsigned int valin = *ptrDi - *ptrBi - *ptrCi + *ptrAi; \
unsigned int *ptrAo = ptr - ((yoffset + scale) << 1); \
unsigned int *ptrDo = ptr + ((yoffset + scale) << 1); \
unsigned int *ptrBo = ptr - ((yoffset - scale) << 1); \
unsigned int *ptrCo = ptr + ((yoffset - scale) << 1); \
unsigned int valout = *ptrDo - *ptrBo - *ptrCo + *ptrAo; \
value = valout - (valin << 2); \
}
CAM_RECURSIVE_PATTERN;
current_scale = scale;
current_value = value;
abs_current_value = abs(value);
// Let's see if we go upscale or not...
if (scale_up && scale < max_scale) {
// OK. Let's test the upscale
scale++;
yoffset += widthStep;
CAM_RECURSIVE_PATTERN;
s1 = current_scale * current_scale; s2 = scale * scale;
if (((CAM_INT64)abs(value)) * s1 > (CAM_INT64)abs_current_value * s2) {
// Yes. That was a good hint
current_scale = scale;
current_value = value;
abs_current_value = abs(value);
// Let's test again at higher scale
if (scale < max_scale) {
scale++;
yoffset += widthStep;
CAM_RECURSIVE_PATTERN;
s1 = current_scale * current_scale; s2 = scale * scale;
if (((CAM_INT64)abs(value)) * s1 > (CAM_INT64)abs_current_value * s2) {
// Even better
current_scale = scale;
current_value = value;
abs_current_value = abs(value);
// And we stop there, satisfied...
}
}
} else {
// Oh, oh... Apparently, that was not a good hint to go upscale...
// Let's try to downscale then...
scale -= 2;
if (scale > 0 && scale <= max_scale) {
yoffset -= widthStep << 1;
CAM_RECURSIVE_PATTERN;
s1 = current_scale * current_scale; s2 = scale * scale;
if (((CAM_INT64)abs(value)) * s1 > (CAM_INT64)abs_current_value * s2) {
// Yes. It's better
scale_up = 0; // This is the hint for next time
current_scale = scale;
current_value = value;
abs_current_value = abs(value);
}
// And we stop there, satisfied...
}
}
} else {
// OK. Let's test the downscale
scale--;
if (scale > 0 && scale <= max_scale) {
yoffset -= widthStep;
CAM_RECURSIVE_PATTERN;
if (((CAM_INT64)abs(value)) * current_scale * current_scale > (CAM_INT64)abs_current_value * scale * scale) {
// Yes. That was a good hint.
current_scale = scale;
current_value = value;
abs_current_value = abs(value);
// Let's test again at lower scale
scale--;
if (scale > 0 && scale <= max_scale) {
yoffset -= widthStep;
CAM_RECURSIVE_PATTERN;
s1 = current_scale * current_scale; s2 = scale * scale;
if (((CAM_INT64)abs(value)) * s1 > (CAM_INT64)abs_current_value * s2) {
// Even better
current_scale = scale;
current_value = value;
abs_current_value = abs(value);
// And we stop there, satisfied...
}
}
} else {
// Oh, oh... Apparently, that was not a good hint to go downscale...
// Let's try to upscale then...
scale += 2;
if (scale > 0 && scale <= max_scale) {
yoffset += widthStep << 1;
CAM_RECURSIVE_PATTERN;
s1 = current_scale * current_scale; s2 = scale * scale;
if (((CAM_INT64)abs(value)) * s1 > (CAM_INT64)abs_current_value * s2) {
// Yes. It's better
scale_up = 1; // This is the hint for next time
current_scale = scale;
current_value = value;
abs_current_value = abs(value);
}
// And we stop there, satisfied...
}
}
} else {
// Oh, oh... Apparently, that was not a good hint to go downscale...
// Let's try to upscale then...
scale += 2;
if (scale > 0 && scale <= max_scale) {
yoffset += widthStep;
CAM_RECURSIVE_PATTERN;
s1 = current_scale * current_scale; s2 = scale * scale;
if (((CAM_INT64)abs(value)) * s1 > (CAM_INT64)abs_current_value * s2) {
// Yes. It's better
scale_up = 1; // This is the hint for next time
current_scale = scale;
current_value = value;
abs_current_value = abs(value);
}
// And we stop there, satisfied...
}
}
}
} else {
// Skip this pixel and go back to small scale
current_value = 0;
abs_current_value = 0;
current_scale = 1;
scale_up = 1;
}
current_value = (current_value << 4) / (current_scale * current_scale);
abs_current_value = abs(current_value);
// Now, we do have the current_value, current_scale and abs_current_value
// Let's check whether it is a local maximum or not
{
// If the current value is strictly greater than all the other values, then this is a local maximum and
// the other pixels are marked as NOT being local maxima
// If the current value is greater or equal to all neighbours, then this ia marked to be a local maximum
// but the other pixels are kept as local maxima
// If the current value is greater than any neighbour, this neighbour is marked as not being a local maximum
int local_maximum = 1;
if (x > 0) {
// Compare to the left and upper-left pixel
if (abs_current_value_line[x - 1] > abs_current_value)
local_maximum = 0;
else current_lmax_line[x - 1] = 0;
if (abs_previous_value_line[x - 1] > abs_current_value)
local_maximum = 0;
else previous_lmax_line[x - 1] = 0;
}
// Compare to pixel above
if (abs_previous_value_line[x] > abs_current_value)
local_maximum = 0;
else previous_lmax_line[x] = 0;
if (x < width - 1) {
// Compare to the upper right pixel
if (abs_previous_value_line[x + 1] > abs_current_value)
local_maximum = 0;
else previous_lmax_line[x + 1] = 0;
}
if (local_maximum && abs_current_value > 0) {
current_lmax[current_nblmax++] = x;
current_lmax_line[x] = 1;
}
}
// Record the data for next line evaluation
current_value_line[x] = current_value;
abs_current_value_line[x] = abs_current_value;
current_scale_line[x] = current_scale;
//if (y == 100) {
//printf("y=%d x=%d value=%d abs=%d scale=%d\n", y, x, current_value, abs_current_value, current_scale);
//}
}
}
// Check the local maxima on the previous line, and record the keypoints IF they are local maxima
for (i = 0; i != previous_nblmax; i++) {
if (previous_lmax_line[previous_lmax[i]]) {
int scale = previous_scale_line[previous_lmax[i]];
if (scale >= CAM_MIN_SCALE) {
// Yes, we have definitely a local maximum
// Record the keypoint
keypoints[nb_keypoints].x = previous_lmax[i];
keypoints[nb_keypoints].y = y - 1;
keypoints[nb_keypoints].scale = scale;
keypoints[nb_keypoints].value = previous_value_line[previous_lmax[i]];
nb_keypoints++;
}
}
}
// Switch previous and current lines
{
unsigned char *tmp;
tmp = current_scale_line;
current_scale_line = previous_scale_line;
previous_scale_line = tmp;
}{
unsigned int *tmp;
tmp = abs_current_value_line;
abs_current_value_line = abs_previous_value_line;
abs_previous_value_line = tmp;
}{
int *tmp;
tmp = current_value_line;
current_value_line = previous_value_line;
previous_value_line = tmp;
}{
unsigned char *tmp;
tmp = current_lmax_line;
current_lmax_line = previous_lmax_line;
previous_lmax_line = tmp;
}{
unsigned int *tmp;
tmp = current_lmax;
current_lmax = previous_lmax;
previous_lmax = tmp;
}
previous_nblmax = current_nblmax;
current_nblmax = 0;
}
}
for (i = 0; i < 2; i++) {
free(value_lines[i]);
free(abs_value_lines[i]);
free(scale_lines[i]);
free(lmax_lines[i]);
free(lmax[i]);
}
// Post-processing : remove keypoints in a given neighborhood (proportionnal to scale)
for (i = 1; i < nb_keypoints; i++) {
CamKeypointShort *keypoint = &keypoints[i];
int j = i - 1;
int neighborhood = keypoint->scale >> 2;
while (j >= 0 && keypoints[j].y >= keypoint->y - neighborhood) {
CamKeypointShort *keypoint2 = &keypoints[j];
if (keypoint2->x >= keypoint->x - neighborhood && keypoint2->x <= keypoint->x + neighborhood) {
// They are next to each other...
if (abs(keypoint2->value) > abs(keypoint->value))
keypoint->scale = 0; // Let's get rid of this one... It has a lower value
else
keypoint2->scale = 0;
}
j--;
}
}
for (i = 0; i < nb_keypoints; i++)
if (!keypoints[i].scale) keypoints[i].value = 0;
if (options & CAM_UPRIGHT) {
// Remove keypoints the descriptor of which would be outside the frame boundaries
for (i = 0; i < nb_keypoints; i++) {
if (keypoints[i].scale) {
int scale = keypoints[i].scale; // Save the scale
keypoints[i].scale = (scale << 2) + 1;
if (!camKeypointDescriptorCheckBounds(&keypoints[i], integral))
keypoints[i].value = 0;
keypoints[i].scale = scale;
}
}
}
// Sort the features according to value
qsort(keypoints, nb_keypoints, sizeof(CamKeypointShort), camSortKeypointsShort);
for (i = 0; i < nb_keypoints; i++)
if (keypoints[i].value == 0) break;
nb_keypoints = i;
if (nb_keypoints > nb_max_keypoints) nb_keypoints = nb_max_keypoints;
/* Interpolation :
* Maxima : solve([y1=a*(x1-p)^2+b,y2=a*(-p)^2+b,y3=a*(x3-p)^2+b],[a,b,p])
* Maxima yields the following formula for parabolic interpolation
2 2 2 2
x1 y3 + (x3 - x1 ) y2 - x3 y1
p = ------------------------------------
2 x1 y3 + (2 x3 - 2 x1) y2 - 2 x3 y1
*/
// Scale super-resolution
for (i = 0; i < nb_keypoints; i++) {
CamKeypointShort *keypoint = &keypoints[i];
#if 1
int x = keypoint->x, y = keypoint->y;
int v, max_scale;
max_scale = (y + iROI.srcroi.yOffset) >> 1;
v = (source->height - 1 - (y + iROI.srcroi.yOffset)) >> 1;
if (v < max_scale) max_scale = v;
v = (x + iROI.srcroi.xOffset) >> 1;
if (v < max_scale) max_scale = v;
v = (source->width -1 - (x + iROI.srcroi.xOffset)) >> 1;
if (v < max_scale) max_scale = v;
max_scale -= CAM_SCALE_MARGIN;
if (keypoint->scale < max_scale) {
unsigned int *ptr = ((unsigned int*)(integral->imageData + (y + iROI.srcroi.yOffset) * integral->widthStep)) + iROI.srcroi.xOffset + x;
int scale = keypoint->scale - 1;
int yoffset = scale * widthStep;
int value, num, den, x1, x3, y1, y2, y3;
x1 = -1;
x3 = 1;
y2 = keypoint->value;
CAM_RECURSIVE_PATTERN;
y1 = (value << 4) / (scale * scale);
if (y1 > y2) keypoint->scale <<= 2; // This is a problem : this is not a local maximum in scale...
else {
int found = 0;
scale = keypoint->scale + 1;
do {
yoffset = scale * widthStep;
CAM_RECURSIVE_PATTERN;
y3 = (value << 4) / (scale * scale);
if (y3 <= y2) {
num = (x1 * x1 * y3 + (x3 * x3 - x1 * x1) * y2 - x3 * x3 * y1);
den = x1 * y3 + (x3 - x1) * y2 - x3 * y1;
if (den == 0)
keypoint->scale <<= 2;
else {
int p = (num << 1) / den; // shift only by 1, because den is shifted by 2
keypoint->scale = p + ((scale - 1) << 2);
found = 1;
}
break;
}
y1 = y2;
y2 = y3;
scale++;
} while (scale <= max_scale);
if (!found) keypoint->scale <<= 2;
}
}
else
#endif
keypoint->scale <<= 2;
}
// Angle
if (options & CAM_UPRIGHT) {
for (i = 0; i < nb_keypoints; i++) {
keypoints[i].angle = 0;
}
} else {
camAllocateImage(&filter, CAM_ORIENTATION_STAMP_SIZE, CAM_ORIENTATION_STAMP_SIZE, CAM_DEPTH_16S);
camBuildGaussianFilter(&filter, camSigmaParam);
pnb_keypoints = nb_keypoints;
if (pnb_keypoints > nb_max_keypoints) pnb_keypoints = nb_max_keypoints;
for (i = 0; i < pnb_keypoints; i++) {
nb_keypoints += camKeypointOrientation(source, &keypoints[i], &filter, &keypoints[nb_keypoints]);
}
camDeallocateImage(&filter);
}
// Sort again the features according to value
qsort(keypoints, nb_keypoints, sizeof(CamKeypointShort), camSortKeypointsShort);
// Keypoints allocation
pnb_keypoints = nb_keypoints;
if (pnb_keypoints > nb_max_keypoints) pnb_keypoints = nb_max_keypoints;
if (points->allocated == 0) {
camAllocateKeypoints(points, pnb_keypoints);
} else if (points->allocated < pnb_keypoints) {
camFreeKeypoints(points);
camAllocateKeypoints(points, pnb_keypoints);
}
if (points->bag == NULL) {
#ifdef __SSE2__
points->bag = (CamKeypoint*)_mm_malloc(sizeof(CamKeypoint) * pnb_keypoints, 16);
#else
points->bag = (CamKeypoint*)malloc(sizeof(CamKeypoint) * pnb_keypoints);
#endif
}
for (i = 0; i < pnb_keypoints; i++) {
points->keypoint[i] = &points->bag[i];
points->keypoint[i]->x = keypoints[i].x + iROI.srcroi.xOffset;
points->keypoint[i]->y = keypoints[i].y + iROI.srcroi.yOffset;
points->keypoint[i]->scale = keypoints[i].scale;
points->keypoint[i]->value = keypoints[i].value;
points->keypoint[i]->angle = keypoints[i].angle;
}
points->nbPoints = pnb_keypoints;
free(keypoints);
if (options & CAM_UPRIGHT) {
camKeypointsDescriptor(points, integral, options);
} else {
camKeypointsDescriptor(points, source, options);
}
// Finally, set the points' set
for (i = 0; i < points->nbPoints; i++) {
points->keypoint[i]->set = points;
}
// Integral Image is now useless
camInternalROIPolicyExit(&iROI);
return 1;
}
