#include <cstdio>

#include "circle_detector.h"

#include "config.h"

using namespace std;

#define min(a,b) ((a) < (b) ? (a) : (b))

#define max(a,b) ((a) > (b) ? (a) : (b))

#define MAX_SEGMENTS 10000 // TODO: necessary?

#define CIRCULARITY_TOLERANCE 0.02

cv::CircleDetector::CircleDetector(int _width,int _height, Context* _context, float _diameter_ratio) : context(_context)

{

minSize = 10;

maxSize = 100*100; // TODO: test!

centerDistanceToleranceRatio = 0.1;

centerDistanceToleranceAbs = 5;

circularTolerance = 0.3;

ratioTolerance = 1.0;

//initialization - fixed params

width = _width;

height = _height;

len = width*height;

siz = len*3;

diameterRatio = _diameter_ratio;

float areaRatioInner_Outer = diameterRatio*diameterRatio;

outerAreaRatio = M_PI*(1.0-areaRatioInner_Outer)/4;

innerAreaRatio = M_PI/4.0;

areasRatio = (1.0-areaRatioInner_Outer)/areaRatioInner_Outer;

// << "outerRatio/innerRatio " << outerAreaRatio << " " << innerAreaRatio << " " << diameterRatio << endl;

threshold = (3 * 256) / 2;

threshold_counter = 0;

}

cv::CircleDetector::~CircleDetector()

{

}

int cv::CircleDetector::get_threshold(void) const

{

return threshold;

}

void cv::CircleDetector::change_threshold(void)

{

#if !defined(ENABLE_RANDOMIZED_THRESHOLD)

threshold_counter++;

int d = threshold_counter;

int div = 1;

while (d > 1){

d /= 2;

div *= 2;

}

int step = 256 / div;

threshold = 3 * (step * (threshold_counter - div) + step/2);

if (step <= 16) threshold_counter = 0;

#else

threshold = (rand() % 48) * 16;

#endif

cout << "attempting new threshold: " << threshold << endl;

}

bool cv::CircleDetector::examineCircle(const cv::Mat& image, cv::CircleDetector::Circle& circle, int ii, float areaRatio)

{

//int64_t ticks = cv::getTickCount();

// get shorter names for elements in Context

vector<int>& buffer = context->buffer;

vector<int>& queue = context->queue;

int vx,vy;

queueOldStart = queueStart;

int position = 0;

int pos;

bool result = false;

int type = buffer[ii];

int maxx,maxy,minx,miny;

int pixel_class;

//cout << "examine (type " << type << ") at " << ii / width << "," << ii % width << " (numseg " << numSegments << ")" << endl;

buffer[ii] = ++numSegments;

circle.x = ii%width;

circle.y = ii/width;

minx = maxx = circle.x;

miny = maxy = circle.y;

circle.valid = false;

circle.round = false;

//push segment coords to the queue

queue[queueEnd++] = ii;

//and until queue is empty

while (queueEnd > queueStart){

//pull the coord from the queue

position = queue[queueStart++];

//search neighbours

pos = position+1;

pixel_class = buffer[pos];

if (pixel_class == 0) {

uchar* ptr = &image.data[pos*3];

pixel_class = ((ptr[0]+ptr[1]+ptr[2]) > threshold)-2;

if (pixel_class != type) buffer[pos] = pixel_class;

}

if (pixel_class == type) {

queue[queueEnd++] = pos;

maxx = max(maxx,pos%width);

buffer[pos] = numSegments;

}

pos = position-1;

pixel_class = buffer[pos];

if (pixel_class == 0) {

uchar* ptr = &image.data[pos*3];

pixel_class = ((ptr[0]+ptr[1]+ptr[2]) > threshold)-2;

if (pixel_class != type) buffer[pos] = pixel_class;

}

if (pixel_class == type) {

queue[queueEnd++] = pos;

minx = min(minx,pos%width);

buffer[pos] = numSegments;

}

pos = position-width;

pixel_class = buffer[pos];

if (pixel_class == 0) {

uchar* ptr = &image.data[pos*3];

pixel_class = ((ptr[0]+ptr[1]+ptr[2]) > threshold)-2;

if (pixel_class != type) buffer[pos] = pixel_class;

}

if (pixel_class == type) {

queue[queueEnd++] = pos;

miny = min(miny,pos/width);

buffer[pos] = numSegments;

}

pos = position+width;

pixel_class = buffer[pos];

if (pixel_class == 0) {

uchar* ptr = &image.data[pos*3];

pixel_class = ((ptr[0]+ptr[1]+ptr[2]) > threshold)-2;

if (pixel_class != type) buffer[pos] = pixel_class;

}

if (pixel_class == type) {

queue[queueEnd++] = pos;

maxy = max(maxy,pos/width);

buffer[pos] = numSegments;

}

//if (queueEnd-queueOldStart > maxSize) return false;

}

//once the queue is empty, i.e. segment is complete, we compute its size

circle.size = queueEnd-queueOldStart;

if (circle.size > minSize){

//and if its large enough, we compute its other properties

circle.maxx = maxx;

circle.maxy = maxy;

circle.minx = minx;

circle.miny = miny;

circle.type = -type;

vx = (circle.maxx-circle.minx+1);

vy = (circle.maxy-circle.miny+1);

circle.x = (circle.maxx+circle.minx)/2;

circle.y = (circle.maxy+circle.miny)/2;

circle.roundness = vx*vy*areaRatio/circle.size;

//we check if the segment is likely to be a ring

if (circle.roundness - circularTolerance < 1.0 && circle.roundness + circularTolerance > 1.0)

{

//if its round, we compute yet another properties

circle.round = true;

// TODO: mean computation could be delayed until the inner ring also satisfies above condition, right?

circle.mean = 0;

for (int p = queueOldStart;p<queueEnd;p++){

pos = queue[p];

circle.mean += image.data[pos*3]+image.data[pos*3+1]+image.data[pos*3+2];

}

circle.mean = circle.mean/circle.size;

result = true;

// << "segment size " << circle.size << " " << vx << " " << vy << endl;

}// else cout << "not round enough (" << circle.roundness << ") vx/vy " << vx << " " << vy << " ctr " << circle.x << " " << circle.y << " " << circle.size << " " << areaRatio << endl;

}

//double delta = (double)(cv::getTickCount() - ticks) / cv::getTickFrequency();

//cout << "examineCircle: " << delta << " " << " fps: " << 1/delta << " pix: " << circle.size << " " << threshold << endl;

return result;

}

cv::CircleDetector::Circle cv::CircleDetector::detect(const cv::Mat& image, const cv::CircleDetector::Circle& previous_circle)

{

vector<int>& buffer = context->buffer;

vector<int>& queue = context->queue;

int pos = (height-1)*width;

int ii = 0;

int start = 0;

Circle inner, outer;

//int outer_id;

if (previous_circle.valid){

ii = ((int)previous_circle.y)*width+(int)previous_circle.x;

start = ii;

}

//cout << "detecting (thres " << threshold << ") at " << ii << endl;

numSegments = 0;

do

{

if (numSegments > MAX_SEGMENTS) break;

//if (start != 0) cout << "it " << ii << endl;

// if current position needs to be thresholded

int pixel_class = buffer[ii];

if (pixel_class == 0){

uchar* ptr = &image.data[ii*3];

//cout << "value: " << (ptr[0]+ptr[1]+ptr[2]) << endl;

pixel_class = ((ptr[0]+ptr[1]+ptr[2]) > threshold)-2;

if (pixel_class == -2) buffer[ii] = pixel_class; // only tag black pixels, to avoid dirtying the buffer outside the ellipse

// NOTE: the inner white area will not initially be tagged, but once the inner circle is processed, it will

}

//cout << pixel_class << endl;

// if the current pixel is detected as "black"

if (pixel_class == -2){

queueEnd = 0;

queueStart = 0;

// << "black pixel " << ii << endl;

// check if looks like the outer portion of the ring

if (examineCircle(image, outer, ii, outerAreaRatio)){

pos = outer.y * width + outer.x; // jump to the middle of the ring

// treshold the middle of the ring and check if it is detected as "white"

pixel_class = buffer[pos];

if (pixel_class == 0){

uchar* ptr = &image.data[pos*3];

pixel_class = ((ptr[0]+ptr[1]+ptr[2]) > threshold)-2;

buffer[pos] = pixel_class;

}

if (pixel_class == -1){

// check if it looks like the inner portion

if (examineCircle(image, inner, pos, innerAreaRatio)){

// it does, now actually check specific properties to see if it is a valid target

if (

((float)outer.size/areasRatio/(float)inner.size - ratioTolerance < 1.0 &&

(float)outer.size/areasRatio/(float)inner.size + ratioTolerance > 1.0) &&

(fabsf(inner.x - outer.x) <= centerDistanceToleranceAbs + centerDistanceToleranceRatio * ((float)(outer.maxx - outer.minx))) &&

(fabsf(inner.y - outer.y) <= centerDistanceToleranceAbs + centerDistanceToleranceRatio * ((float)(outer.maxy - outer.miny)))

)

{

float cm0,cm1,cm2;

cm0 = cm1 = cm2 = 0;

inner.x = outer.x;

inner.y = outer.y;

// computer centroid

float sx = 0;

float sy = 0;

queueOldStart = 0;

for (int p = 0;p<queueEnd;p++){

pos = queue[p];

sx += pos % width;

sy += pos / width;

}

// update pixel-based position oreviously computed

inner.x = sx / queueEnd;

inner.y = sy / queueEnd;

outer.x = sx / queueEnd;

outer.y = sy / queueEnd;

// compute covariance

for (int p = 0;p<queueEnd;p++){

pos = queue[p];

float tx = pos % width - outer.x;

float ty = pos / width - outer.y;

cm0 += tx * tx;

cm2 += ty * ty;

cm1 += tx * ty;

}

float fm0,fm1,fm2;

fm0 = ((float)cm0)/queueEnd; // cov(x,x)

fm1 = ((float)cm1)/queueEnd; // cov(x,y)

fm2 = ((float)cm2)/queueEnd; // cov(y,y)

float trace = fm0 + fm2; // sum of elements in diag.

float det = trace * trace - 4*(fm0 * fm2 - fm1 * fm1);

if (det > 0) det = sqrt(det); else det = 0; //yes, that can happen as well:(

float f0 = (trace + det)/2;

float f1 = (trace - det)/2;

inner.m0 = sqrt(f0);

inner.m1 = sqrt(f1);

if (fm1 != 0) { //aligned ?

inner.v0 = -fm1/sqrt(fm1*fm1+(fm0-f0)*(fm0-f0)); // no-> standard calculatiion

inner.v1 = (fm0-f0)/sqrt(fm1*fm1+(fm0-f0)*(fm0-f0));

}

else{

inner.v0 = inner.v1 = 0;

if (fm0 > fm2) inner.v0 = 1.0; else inner.v1 = 1.0; // aligned, hm, is is aligned with x or y ?

}

inner.bwRatio = (float)outer.size/inner.size;

// TODO: purpose? should be removed? if next if fails, it will go over and over to the same place until number of segments

// reaches max, right?

ii = start - 1; // track position

float circularity = M_PI*4*(inner.m0)*(inner.m1)/queueEnd;

if (fabsf(circularity - 1) < CIRCULARITY_TOLERANCE){

outer.valid = inner.valid = true; // at this point, the target is considered valid

/*inner_id = numSegments; outer_id = numSegments - 1;*/

threshold = (outer.mean + inner.mean) / 2; // use a new threshold estimate based on current detection

cout << "threshold set to: " << threshold << endl;

//pixel leakage correction

float r = diameterRatio*diameterRatio;

float m0o = sqrt(f0);

float m1o = sqrt(f1);

float ratio = (float)inner.size/(outer.size + inner.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;

inner.m0 = sqrt(f0)+t;

inner.m1 = sqrt(f1)+t;

inner.minx = outer.minx;

inner.maxx = outer.maxx;

inner.maxy = outer.maxy;

inner.miny = outer.miny;

break;

}

}

}

}

}

}

ii++;

if (ii >= len) ii = 0;

} while (ii != start);

// draw

if (inner.valid)

threshold_counter = 0;

else

change_threshold(); // update threshold for next run. inner is what user receives

/*cv::namedWindow("buffer", CV_WINDOW_NORMAL);

cv::Mat buffer_img;

context->debug_buffer(image, buffer_img);

cv::imshow("buffer", buffer_img);*/

//cv::waitKey();

// if this is not the first call (there was a previous valid circle where search started),

// the current call found a valid match, and only two segments were found during the search (inner/outer)

// then, only the bounding box area of the outer ellipse needs to be cleaned in 'buffer'

bool fast_cleanup = (previous_circle.valid && numSegments == 2 && inner.valid);

context->cleanup(outer, fast_cleanup);

if (!inner.valid) cout << "detection failed" << endl;

else cout << "detected at " << inner.x << " " << inner.y << endl;

return inner;

}

void cv::CircleDetector::cover_last_detected(cv::Mat& image)

{

const vector<int>& queue = context->queue;

for (int i = queueOldStart; i < queueEnd; i++) {

int pos = queue[i];

uchar* ptr = image.data + 3*pos;

*ptr = 255; ptr++;

*ptr = 255; ptr++;

*ptr = 255;

}

}

void cv::CircleDetector::improveEllipse(const cv::Mat& image, Circle& c)

{

cv::Mat subimg;

int delta = 10;

cout << image.rows << " x " << image.cols << endl;

cv::Range row_range(max(0, c.miny - delta), min(c.maxy + delta, image.rows));

cv::Range col_range(max(0, c.minx - delta), min(c.maxx + delta, image.cols));

cout << row_range.start << " " << row_range.end << " " << col_range.start << " " << col_range.end << endl;

image(row_range, col_range).copyTo(subimg);

cv::Mat cannified;

cv::Canny(subimg, cannified, 4000, 8000, 5, true);

/*cv::namedWindow("bleh");

cv::imshow("bleh", subimg);

cv::waitKey();*/

std::vector< std::vector<cv::Point> > contours;

cv::findContours(cannified, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);

if (contours.empty() || contours[0].size() < 5) return;

cv::Mat contour_img;

subimg.copyTo(contour_img);

cv::drawContours(contour_img, contours, 0, cv::Scalar(255,0,255), 1);

/*cv::namedWindow("bleh2");

cv::imshow("bleh2", contour_img);

cv::waitKey();*/

cv::RotatedRect rect = cv::fitEllipse(contours[0]);

cout << "old: " << c.x << " " << c.y << " " << c.m0 << " " << c.m1 << " " << c.v0 << " " << c.v1 << endl;

c.x = rect.center.x + col_range.start;

c.y = rect.center.y + row_range.start;

/*float max_size = max(rect.size.width, rect.size.height);

float min_size = min(rect.size.width, rect.size.height);*/

c.m0 = rect.size.width * 0.25;

c.m1 = rect.size.height * 0.25;

c.v0 = cos(rect.angle / 180.0 * M_PI);

c.v1 = sin(rect.angle / 180.0 * M_PI);

cout << "new: " << c.x << " " << c.y << " " << c.m0 << " " << c.m1 << " " << c.v0 << " " << c.v1 << endl;

/*cv::Mat ellipse_img;

image(row_range, col_range).copyTo(subimg);

subimg.copyTo(ellipse_img);

cv::ellipse(ellipse_img, rect, cv::Scalar(255,0,255));

cv::namedWindow("bleh3");

cv::imshow("bleh3", ellipse_img);

cv::waitKey();*/

}

cv::CircleDetector::Circle::Circle(void)

{

x = y = 0;

round = valid = false;

}

void cv::CircleDetector::Circle::draw(cv::Mat& image, const std::string& text, cv::Vec3b color, float thickness) const

{

//cv::circle(image, cv::Point(x, y), 1, color, 1, 4);

//cv::ellipse(image, cv::Point(x, y), cv::Size2f(m0 * 2, m1 * 2), atan2(v1, v0) * 180.0 / M_PI, 0, 360, color, thickness, 8);

for (float e = 0; e < 2 * M_PI; e += 0.05) {

float fx = x + cos(e) * v0 * m0 * 2 + v1 * m1 * 2 * sin(e);

float fy = y + cos(e) * v1 * m0 * 2 - v0 * m1 * 2 * sin(e);

int fxi = (int)(fx + 0.5);

int fyi = (int)(fy + 0.5);

if (fxi >= 0 && fxi < image.cols && fyi >= 0 && fyi < image.rows)

image.at<cv::Vec3b>(fyi, fxi) = color;

}

float scale = image.size().width / 1800.0f;

//float thickness = scale * 3.0;

//if (thickness < 1) thickness = 1;

cv::putText(image, text.c_str(), cv::Point(x + 2 * m0, y + 2 * m1), CV_FONT_HERSHEY_SIMPLEX, scale, cv::Scalar(color), thickness, CV_AA);

cv::line(image, cv::Point(x + v0 * m0 * 2, y + v1 * m0 * 2), cv::Point(x - v0 * m0 * 2, y - v1 * m0 * 2), cv::Scalar(color), 1, 8);

cv::line(image, cv::Point(x + v1 * m1 * 2, y - v0 * m1 * 2), cv::Point(x - v1 * m1 * 2, y + v0 * m1 * 2), cv::Scalar(color), 1, 8);

}

void cv::CircleDetector::Circle::write(cv::FileStorage& fs) const {

fs << "{" << "x" << x << "y" << y << "size" << size <<

"maxy" << maxy << "maxx" << maxx << "miny" << miny << "minx" << minx <<

"mean" << mean << "type" << type << "roundness" << roundness << "bwRatio" << bwRatio <<

"round" << round << "valid" << valid << "m0" << m0 << "m1" << m1 << "v0" << v0 << "v1" << v1 << "}";

}

void cv::CircleDetector::Circle::read(const cv::FileNode& node)

{

x = (float)node["x"];

y = (float)node["y"];

size = (int)node["size"];

maxy = (int)node["maxy"];

maxx = (int)node["maxx"];

miny = (int)node["miny"];

minx = (int)node["minx"];

mean = (int)node["mean"];

type = (int)node["type"];

roundness = (float)node["roundness"];

bwRatio = (float)node["bwRatio"];

round = (int)node["round"];

valid = (int)node["valid"];

m0 = (float)node["m0"];

m1 = (float)node["m1"];

v0 = (float)node["v0"];

v1 = (float)node["v1"];

}

cv::CircleDetector::Context::Context(int _width, int _height)

{

width = _width;

height = _height;

int len = width * height;

buffer.resize(len);

queue.resize(len);

cleanup(Circle(), false);

}

void cv::CircleDetector::Context::cleanup(const Circle& c, bool fast_cleanup) {

if (c.valid && fast_cleanup)

{

// zero only parts modified when detecting 'c'

int ix = max(c.minx - 2,1);

int ax = min(c.maxx + 2, width - 2);

int iy = max(c.miny - 2,1);

int ay = min(c.maxy + 2, height - 2);

for (int y = iy; y < ay; y++){

int pos = y * width;

for (int x = ix; x < ax; x++) buffer[pos + x] = 0; // TODO: user ptr and/or memset

}

}

else {

cout << "clean whole buffer" << endl;

memset(&buffer[0], 0, sizeof(int)*buffer.size());

//image delimitation

for (int i = 0;i<width;i++){

buffer[i] = -1000;

buffer[(height - 1) * width + i] = -1000;

}

for (int i = 0;i<height;i++){

buffer[width * i] = -1000;

buffer[width * i + width - 1] = -1000;

}

}

}

void cv::CircleDetector::Context::debug_buffer(const cv::Mat& image, cv::Mat& out)

{

out.create(height, width, CV_8UC3);

cv::Vec3b* out_ptr = out.ptr<cv::Vec3b>(0);

const cv::Vec3b* im_ptr = image.ptr<cv::Vec3b>(0);

out = cv::Scalar(128,128,128);

for (uint i = 0; i < out.total(); i++, ++out_ptr, ++im_ptr) {

/*if (buffer[i] == -1) *ptr = cv::Vec3b(0,0,0);

else if (buffer[i] == -2) *ptr = cv::Vec3b(255,255,255);*/

//else if (buffer[i] < 0) *ptr = cv::Vec3b(0, 255, 0);

if (buffer[i] > 0) *out_ptr = cv::Vec3b(255, 0, 255);

else *out_ptr = *im_ptr;

}

}