SSegment CCircleDetect::findSegment(CRawImage* image, SSegment init)
{
SSegment result;
result.x = 0;
result.y = 0;
result.round = false;
result.valid = false;
numSegments = 0;
if (image->width != width || image->height != height){
adjustDimensions(image->width,image->height);
init.valid = false;
}
int pos = 0;
int ii = 0;
int start = 0;
bool cont = true;
int step = image->bpp;
//bufferCleanup(init);
if (init.valid && track){
ii = ((int)init.y)*image->width+init.x;
start = ii;
}
while (cont)
{
if (buffer[ii] == 0){
ptr = &image->data[ii*step];
if ((ptr[0]+ptr[1]+ptr[2]) < threshold) buffer[ii] = -2;
}
if (buffer[ii] == -2 && numSegments<MAX_SEGMENTS){
//new segment found
queueEnd = 0;
queueStart = 0;
//if the segment looks like a ring, we check its inside area
if (examineSegment(image,&segmentArray[numSegments],ii,outerAreaRatio)){
pos = segmentArray[numSegments-1].y*image->width+segmentArray[numSegments-1].x;
if (buffer[pos] == 0){
ptr = &image->data[pos*step];
buffer[pos]=(ptr[0]+ptr[1]+ptr[2] > threshold)-2;
}
if (buffer[pos] == -1 && numSegments<MAX_SEGMENTS){
if (examineSegment(image,&segmentArray[numSegments],pos,innerAreaRatio)){
//the inside area is a circle. now what is the area ratio of the black and white ? also, are the circles concentric ?
if (debug) printf("Area ratio %i/%i is %.3f %.3f %.3f\n",numSegments-2,numSegments-1,(float)segmentArray[numSegments-2].size/segmentArray[numSegments-1].size,areasRatio,segmentArray[numSegments-2].size/areasRatio/segmentArray[numSegments-1].size);
if (
((float)segmentArray[numSegments-2].size/areasRatio/(float)segmentArray[numSegments-1].size - ratioTolerance < 1.0 && (float)segmentArray[numSegments-2].size/areasRatio/(float)segmentArray[numSegments-1].size + ratioTolerance > 1.0) &&
(abs(segmentArray[numSegments-1].x-segmentArray[numSegments-2].x) <= centerDistanceToleranceAbs+centerDistanceToleranceRatio*((float)(segmentArray[numSegments-2].maxx-segmentArray[numSegments-2].minx))) &&
(abs(segmentArray[numSegments-1].y-segmentArray[numSegments-2].y) <= centerDistanceToleranceAbs+centerDistanceToleranceRatio*((float)(segmentArray[numSegments-2].maxy-segmentArray[numSegments-2].miny)))
){
float fm0,fm1,fm2,f0,f1;
long int six,siy,tx,ty,cm0,cm1,cm2;
float cm0f,cm1f,cm2f;
six=siy=cm0=cm1=cm2=0;
// segmentArray[numSegments-1].x = segmentArray[numSegments-2].x;
// segmentArray[numSegments-1].y = segmentArray[numSegments-2].y;
for (int p = queueOldStart;p<queueEnd;p++){
pos = queue[p];
tx = pos%image->width;
ty = pos/image->width;
six += tx;
siy += ty;
cm0+=tx*tx;
cm1+=tx*ty;
cm2+=ty*ty;
}
segmentArray[numSegments-2].x = (float)six/(queueEnd-queueOldStart);
segmentArray[numSegments-2].y = (float)siy/(queueEnd-queueOldStart);
/*float sxi = (float)six/(queueEnd-queueOldStart);
float syi = (float)siy/(queueEnd-queueOldStart);
cm0f = (cm0 - sxi*sxi*(queueEnd-queueOldStart));
cm1f = (cm1 - sxi*syi*(queueEnd-queueOldStart));
cm2f = (cm2 - syi*syi*(queueEnd-queueOldStart));
fm0 = cm0f/(queueEnd-queueOldStart);
fm1 = cm1f/(queueEnd-queueOldStart);
fm2 = cm2f/(queueEnd-queueOldStart);
f0 = ((fm0+fm2)+sqrt((fm0+fm2)*(fm0+fm2)-4*(fm0*fm2-fm1*fm1)))/2;
f1 = ((fm0+fm2)-sqrt((fm0+fm2)*(fm0+fm2)-4*(fm0*fm2-fm1*fm1)))/2;
float m0i = sqrt(f0);
float m1i = sqrt(f1);*/
for (int p = 0;p<queueOldStart;p++){
pos = queue[p];
tx = pos%image->width;
ty = pos/image->width;
six += tx;
siy += ty;
cm0+=tx*tx;
cm1+=tx*ty;
cm2+=ty*ty;
}
float sx = (float)six/queueEnd;
float sy = (float)siy/queueEnd;
cm0f = (cm0 - sx*sx*queueEnd);
cm1f = (cm1 - sx*sy*queueEnd);
cm2f = (cm2 - sy*sy*queueEnd);
fm0 = cm0f/queueEnd;
fm1 = cm1f/queueEnd;
fm2 = cm2f/queueEnd;
float det = (fm0+fm2)*(fm0+fm2)-4*(fm0*fm2-fm1*fm1);
if (det > 0) det = sqrt(det); else det = 0;
f0 = ((fm0+fm2)+det)/2;
f1 = ((fm0+fm2)-det)/2;
segmentArray[numSegments-1].m0 = sqrt(f0);
segmentArray[numSegments-1].m1 = sqrt(f1);
if (fm1 != 0){ //aligned ?
segmentArray[numSegments-1].v0 = -fm1/sqrt(fm1*fm1+(fm0-f0)*(fm0-f0));
segmentArray[numSegments-1].v1 = (fm0-f0)/sqrt(fm1*fm1+(fm0-f0)*(fm0-f0));
}else{
result.v0 = result.v1 = 0;
if (fm0 > fm2) result.v0 = 1.0; else result.v1 = 1.0; // aligned, hm, is is aligned with x or y ?
}
segmentArray[numSegments-1].bwRatio = (float)segmentArray[numSegments-2].size/segmentArray[numSegments-1].size;
if (track) ii = start -1;
sizer+=segmentArray[numSegments-2].size + segmentArray[numSegments-1].size; //for debugging
sizerAll+=len; //for debugging
float circularity = M_PI*4*(segmentArray[numSegments-1].m0)*(segmentArray[numSegments-1].m1)/queueEnd;
if (debug) fprintf(stdout,"Circularity: %f %i \n",circularity,queueEnd);
if (circularity-1.0 < circularityTolerance && circularity-1.0 > -circularityTolerance){
segmentArray[numSegments-2].valid = segmentArray[numSegments-1].valid = true;
threshold = (segmentArray[numSegments-2].mean+segmentArray[numSegments-1].mean)/2;
if (debug) fprintf(stdout,"Circularity: %i %03f %03f %03f \n",queueEnd,M_PI*4*(segmentArray[numSegments-1].m0)*(segmentArray[numSegments-1].m1)/queueEnd,M_PI*4*(segmentArray[numSegments-1].m0)*(segmentArray[numSegments-1].m1),segmentArray[numSegments-1].x/1000.0);
//chromatic aberation correction
float r = diameterRatio*diameterRatio;
float m0o = sqrt(f0);
float m1o = sqrt(f1);
float ratio = (float)segmentArray[numSegments-1].size/(segmentArray[numSegments-2].size + segmentArray[numSegments-1].size);
float m0i = sqrt(ratio)*m0o;
float m1i = sqrt(ratio)*m1o;
float a = (1-r);
float b = -(m0i+m1i)-(m0o+m1o)*r;
float c = (m0i*m1i)-(m0o*m1o)*r;
float t = (-b-sqrt(b*b-4*a*c))/(2*a);
//plc second version
//float t0 = (-b-sqrt(b*b-4*a*c))/(2*a);
//float t1 = (-b+sqrt(b*b-4*a*c))/(2*a);
//if (m1i - t0 > 0 && m1i - t1 >0) printf("AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA\n");
//float t0 = (m0i-diameterRatio*m0o)/(1+diameterRatio);
//float t1 = (m1i-diameterRatio*m1o)/(1+diameterRatio);
//m0i-=t;m1i-=t;m0o+=t;m1o+=t;
//fprintf(stdout,"UUU: %f R: %f %f R: %f %f\n",t,m1i/m1o*0.14,m0i/m0o*0.14,(m0o*m1o-m0i*m1i)/(m0i*m1i),(0.14*0.14-0.05*0.05)/(0.05*0.05));
segmentArray[numSegments-1].m0 = m0o;
segmentArray[numSegments-1].m1 = m1o;
segmentArray[numSegments-1].maxx = segmentArray[numSegments-2].maxx;
segmentArray[numSegments-1].maxy = segmentArray[numSegments-2].maxy;
segmentArray[numSegments-1].minx = segmentArray[numSegments-2].minx;
segmentArray[numSegments-1].miny = segmentArray[numSegments-2].miny;
segmentArray[numSegments-1].x = sx;
segmentArray[numSegments-1].y = sy;
segmentArray[numSegments-1].size = segmentArray[numSegments-2].size+segmentArray[numSegments-1].size;
segmentArray[numSegments-1].horizontal = sx-segmentArray[numSegments-2].x;
segmentArray[numSegments-1].angle = atan2(sy-segmentArray[numSegments-2].y,sx-segmentArray[numSegments-2].x);
segmentArray[numSegments-2].x = sx;
segmentArray[numSegments-2].y = sy;
}
}
}
}
}
}
ii++;
if (ii >= len) ii = 0;
cont = (ii != start);
}
for (int i = 0;i< numSegments;i++){
if (segmentArray[i].size > minSize && (segmentArray[i].valid || debug)){
if (debug)fprintf(stdout,"Segment %i Type: %i Pos: %.2f %.2f Area: %i Vx: %i Vy: %i Mean: %i Thr: %i Eigen: %03f %03f %03f Roundness: %03f\n",i,segmentArray[i].type,segmentArray[i].x,segmentArray[i].y,segmentArray[i].size,segmentArray[i].maxx-segmentArray[i].minx,segmentArray[i].maxy-segmentArray[i].miny,segmentArray[i].mean,threshold,segmentArray[i].m0,segmentArray[i].m1,M_PI*4*segmentArray[i].m0*segmentArray[i].m1,segmentArray[i].roundness);
if (segmentArray[i].valid) result = segmentArray[i];
}
}
if (numSegments == 2 && segmentArray[0].valid && segmentArray[1].valid) lastTrackOK = true; else lastTrackOK = false;
//Drawing results
if (result.valid){
lastThreshold = threshold;
drawAll = false;
numFailed = 0;
}else if (numFailed < maxFailed){
if (numFailed++%2 == 0) changeThreshold(); else threshold = lastThreshold;
if (debug) drawAll = true;
}else{
numFailed++;
if (changeThreshold()==false) numFailed = 0;
if (debug) drawAll = true;
}
int j = 0;
for (int p = queueOldStart;p<queueEnd;p++){
pos = queue[p];
image->data[step*pos] = 0;
}
for (int p = 0;p< queueOldStart;p++){
pos = queue[p];
image->data[step*pos] = 255;
}
bufferCleanup(result);
return result;
}
SSegment CCircleDetect::findSegment(CRawImage* image, SSegment init) {//printf("findingSegment\n");
SSegment result;
result.x = 0;
result.y = 0;
result.round = false;
result.valid = false;
numSegments = 0;
timer.reset();
timer.start();
tima += timer.getTime();
int pos = 0;
int ii = 0;
int start = 0;
bool cont = true;
//bufferCleanup(init);
if (init.valid && track) {
ii = ((int) init.y) * image->getwidth() + init.x;
start = ii;
}
while (cont) {
if (buffer[ii] == 0) {
ptr = &image->data[ii * 3];
//buffer[ii]=((ptr[0]+ptr[1]+ptr[2]) > threshold)-2;
if ((ptr[0] + ptr[1] + ptr[2]) < threshold)
buffer[ii] = -2;
}
if (buffer[ii] == -2 && numSegments < MAX_SEGMENTS) {
//new segment found
queueEnd = 0;
queueStart = 0;
//if the segment looks like a ring, we check its inside area
if (examineSegment(image, &segmentArray[numSegments], ii,
outerAreaRatio)) {
pos = segmentArray[numSegments - 1].y * image->getwidth()
+ segmentArray[numSegments - 1].x;
if (buffer[pos] == 0) {
ptr = &image->data[pos * 3];
buffer[pos] = ((ptr[0] + ptr[1] + ptr[2]) > threshold) - 2;
}
if (buffer[pos] == -1 && numSegments < MAX_SEGMENTS) {
if (examineSegment(image, &segmentArray[numSegments], pos,
innerAreaRatio)) {
//the inside area is a circle. now what is the area ratio of the black and white ? also, are the circles concentric ?
if (debug)
printf("Area ratio %i/%i is %.3f %.3f %.3f\n",
numSegments - 2, numSegments - 1,
(float) segmentArray[numSegments - 2].size
/ segmentArray[numSegments - 1].size,
areasRatio,
segmentArray[numSegments - 2].size
/ areasRatio
/ segmentArray[numSegments - 1].size);
if (((float) segmentArray[numSegments - 2].size
/ areasRatio
/ (float) segmentArray[numSegments - 1].size
- ratioTolerance < 1.0
&& (float) segmentArray[numSegments - 2].size
/ areasRatio
/ (float) segmentArray[numSegments - 1].size
+ ratioTolerance > 1.0)
&& (abs(
segmentArray[numSegments - 1].x
- segmentArray[numSegments - 2].x)
<= centerDistanceToleranceAbs
+ centerDistanceToleranceRatio
* ((float) (segmentArray[numSegments
- 2].maxx
- segmentArray[numSegments
- 2].minx)))
&& (abs(
segmentArray[numSegments - 1].y
- segmentArray[numSegments - 2].y)
<= centerDistanceToleranceAbs
+ centerDistanceToleranceRatio
* ((float) (segmentArray[numSegments
- 2].maxy
- segmentArray[numSegments
- 2].miny)))
) {
float tx, ty, cm0, cm1, cm2, sx, sy, fm0, fm1, fm2,
f0, f1;
sx = sy = cm0 = cm1 = cm2 = 0;
// segmentArray[numSegments-1].x = segmentArray[numSegments-2].x;
// segmentArray[numSegments-1].y = segmentArray[numSegments-2].y;
for (int p = queueOldStart; p < queueEnd; p++) {
pos = queue[p];
sx += pos % image->getwidth();
sy += pos / image->getwidth();
}
segmentArray[numSegments - 2].x = sx
/ (queueEnd - queueOldStart);
segmentArray[numSegments - 2].y = sy
/ (queueEnd - queueOldStart);
for (int p = 0; p < queueOldStart; p++) {
pos = queue[p];
sx += pos % image->getwidth();
sy += pos / image->getwidth();
}
sx = sx / queueEnd;
sy = sy / queueEnd;
for (int p = 0; p < queueEnd; p++) {
pos = queue[p];
tx = pos % image->getwidth() - sx;
ty = pos / image->getwidth() - sy;
cm0 += tx * tx;
cm1 += tx * ty;
cm2 += ty * ty;
}
fm0 = cm0 / queueEnd;
fm1 = cm1 / queueEnd;
fm2 = cm2 / queueEnd;
f0 =
((fm0 + fm2)
+ sqrt(
(fm0 + fm2) * (fm0 + fm2)
- 4
* (fm0 * fm2
- fm1
* fm1)))
/ 2;
f1 =
((fm0 + fm2)
- sqrt(
(fm0 + fm2) * (fm0 + fm2)
- 4
* (fm0 * fm2
- fm1
* fm1)))
/ 2;
segmentArray[numSegments - 1].m0 = sqrt(f0);
segmentArray[numSegments - 1].m1 = sqrt(f1);
segmentArray[numSegments - 1].v0 = -fm1
/ sqrt(fm1 * fm1 + (fm0 - f0) * (fm0 - f0));
segmentArray[numSegments - 1].v1 = (fm0 - f0)
/ sqrt(fm1 * fm1 + (fm0 - f0) * (fm0 - f0));
segmentArray[numSegments - 1].bwRatio =
(float) segmentArray[numSegments - 2].size
/ segmentArray[numSegments - 1].size;
if (track)
ii = start - 1;
sizer += segmentArray[numSegments - 2].size
+ segmentArray[numSegments - 1].size; //for debugging
sizerAll += len; //for debugging
float circularity = M_PI * 4
* (segmentArray[numSegments - 1].m0)
* (segmentArray[numSegments - 1].m1)
/ queueEnd;
if (debug)
fprintf(stdout, "Circularity: %f \n",
circularity);
if (circularity - 1.0 < circularityTolerance
&& circularity - 1.0
> -circularityTolerance) {
segmentArray[numSegments - 2].valid =
segmentArray[numSegments - 1].valid =
true;
threshold = (segmentArray[numSegments - 2].mean
+ segmentArray[numSegments - 1].mean)
/ 2;
if (debug)
fprintf(stdout,
"Circularity: %i %03f %03f %03f \n",
queueEnd,
M_PI * 4
* (segmentArray[numSegments
- 1].m0)
* (segmentArray[numSegments
- 1].m1) / queueEnd,
M_PI * 4
* (segmentArray[numSegments
- 1].m0)
* (segmentArray[numSegments
- 1].m1),
segmentArray[numSegments - 1].x
/ 1000.0);
//pixel leakage correction
float r = diameterRatio * diameterRatio;
float m0o = sqrt(f0);
float m1o = sqrt(f1);
float ratio =
(float) segmentArray[numSegments - 1].size
/ (segmentArray[numSegments - 2].size
+ segmentArray[numSegments
- 1].size);
float m0i = sqrt(ratio) * m0o;
float m1i = sqrt(ratio) * m1o;
float a = (1 - r);
float b = -(m0i + m1i) - (m0o + m1o) * r;
float c = (m0i * m1i) - (m0o * m1o) * r;
float t = (-b - sqrt(b * b - 4 * a * c))
/ (2 * a);
m0i -= t;
m1i -= t;
m0o += t;
m1o += t;
//fprintf(stdout,"UUU: %f %f %f %f %f\n",t,ratio,(m1i-t)/(m1o+t)*0.14,(m0i-t)/(m0o+t)*0.14,(m0o*m1o-m0i*m1i)/(m0i*m1i));
segmentArray[numSegments - 1].m0 = sqrt(f0) + t;
segmentArray[numSegments - 1].m1 = sqrt(f1) + t;
segmentArray[numSegments - 1].maxx =
segmentArray[numSegments - 2].maxx;
segmentArray[numSegments - 1].maxy =
segmentArray[numSegments - 2].maxy;
segmentArray[numSegments - 1].minx =
segmentArray[numSegments - 2].minx;
segmentArray[numSegments - 1].miny =
segmentArray[numSegments - 2].miny;
segmentArray[numSegments - 1].x = sx;
segmentArray[numSegments - 1].y = sy;
segmentArray[numSegments - 1].size =
segmentArray[numSegments - 2].size
+ segmentArray[numSegments - 1].size;
segmentArray[numSegments - 1].horizontal = sx
- segmentArray[numSegments - 2].x;
segmentArray[numSegments - 1].angle = atan2(
sy - segmentArray[numSegments - 2].y,
sx - segmentArray[numSegments - 2].x);
segmentArray[numSegments - 2].x = sx;
segmentArray[numSegments - 2].y = sy;
}
}
}
}
}
}
ii++;
if (ii >= len)
ii = 0;
cont = (ii != start);
}
for (int i = 0; i < numSegments; i++) {
if (segmentArray[i].size > minSize
&& (segmentArray[i].valid || debug)) {
if (debug)
fprintf(stdout,
"Segment %i Type: %i Pos: %.2f %.2f Area: %i Vx: %i Vy: %i Mean: %i Thr: %i Eigen: %03f %03f %03f Roundness: %03f\n",
i, segmentArray[i].type, segmentArray[i].x,
segmentArray[i].y, segmentArray[i].size,
segmentArray[i].maxx - segmentArray[i].minx,
segmentArray[i].maxy - segmentArray[i].miny,
segmentArray[i].mean, threshold, segmentArray[i].m0,
segmentArray[i].m1,
M_PI * 4 * segmentArray[i].m0 * segmentArray[i].m1,
segmentArray[i].roundness);
if (segmentArray[i].valid)
result = segmentArray[i];
}
}
if (numSegments == 2 && segmentArray[0].valid && segmentArray[1].valid)
lastTrackOK = true;
else
lastTrackOK = false;
//Drawing results
if (result.valid) {
lastThreshold = threshold;
drawAll = false;
numFailed = 0;
} else if (numFailed < maxFailed) {
if (numFailed++ % 2 == 0)
changeThreshold();
else
threshold = lastThreshold;
if (debug)
drawAll = true;
} else {
numFailed++;
if (changeThreshold() == false)
numFailed = 0;
if (debug)
drawAll = true;
}
int j = 0;
for (int p = queueOldStart; p < queueEnd; p++) {
pos = queue[p];
image->data[3 * pos + 0] = 0;
image->data[3 * pos + 1] = 0;
image->data[3 * pos + 2] = 0;
}
if (draw) {
for (int i = 0; i < len; i++) {
j = buffer[i];
if (j > 0) {
if (drawAll || segmentArray[j - 1].valid) {
image->data[i * 3 + j % 3] = 0;
image->data[i * 3 + (j + 1) % 3] = 255;
image->data[i * 3 + (j + 2) % 3] = 255;
}
}
}
}
bufferCleanup(result);
return result;
}
