25 #include <fvmodels/shape/rcd_circle.h> 40 #define TEST_IF_IS_A_PIXEL(x) ((x)>230) 42 #define TEST_IF_IS_A_PIXEL(x) ((x)==0) 43 #endif // TBY_GRAYSCALE 45 #define TBY_SQUARED_DIST(x1,y1,x2,y2) \ 46 (((x1)-(x2))*((x1)-(x2))+((y1)-(y2))*((y1)-(y2))) 47 #define TBY_RADIUS_DIFF(x1, y1, x2, y2, r) \ 48 (((x1)-(x2))*((x1)-(x2))+((y1)-(y2))*((y1)-(y2))-(r)*(r)) 65 RcdCircleModel::RcdCircleModel(
unsigned int max_failures,
66 unsigned int min_pixels,
67 unsigned int min_interpix_dist,
68 unsigned int max_dist_p4,
69 unsigned int max_dist_a,
76 RCD_MAX_FAILURES = max_failures;
77 RCD_MIN_PIXELS = min_pixels;
78 RCD_MIN_INTERPIX_DIST = min_interpix_dist;
79 RCD_MAX_DIST_P4 = max_dist_p4;
80 RCD_MAX_DIST_A = max_dist_a;
81 RCD_HW_RATIO = hw_ratio;
82 RCD_MAX_TIME = max_time;
83 RCD_ROI_HOLLOW_RATE = hollow_rate;
88 RcdCircleModel::~RcdCircleModel(
void)
93 int RcdCircleModel::parseImage(
unsigned char* buf,
98 unsigned char *line_start = buffer;
101 vector<upoint_t> pixels,
106 struct timeval start, now;
111 const unsigned int roi_hollow_top = (int)(roi->
height * ((1.0f - RCD_ROI_HOLLOW_RATE) / 2));
112 const unsigned int roi_hollow_bottom = roi->
height - roi_hollow_top;
113 const unsigned int roi_hollow_left = (int)(roi->
width * ((1.0f - RCD_ROI_HOLLOW_RATE) / 2));
114 const unsigned int roi_hollow_right = roi->
width - roi_hollow_left;
124 unsigned int boundary_right = 0;
125 unsigned int boundary_bottom = 0;
127 gettimeofday(&start, NULL);
132 for (y = 0; y < roi_hollow_top; ++y) {
133 for (x = 0; x < roi->
width; ++x) {
134 if (TEST_IF_IS_A_PIXEL(*buffer)) {
136 pixels.push_back(pt);
137 if (x > boundary_right) boundary_right = x;
147 for (y = roi_hollow_top; y < roi_hollow_bottom; ++y) {
148 for (x = 0; x < roi_hollow_left; ++x) {
149 if (TEST_IF_IS_A_PIXEL(*buffer)) {
151 pixels.push_back(pt);
152 if (x > boundary_right) boundary_right = x;
158 buffer+=(roi_hollow_right - roi_hollow_left);
159 for (x = roi_hollow_right; x < roi->
width; ++x) {
160 if (TEST_IF_IS_A_PIXEL(*buffer)) {
162 pixels.push_back(pt);
163 if (x > boundary_right) boundary_right = x;
173 for (y = roi_hollow_bottom; y < roi->
height; ++y) {
174 for (x = 0; x < roi->
width; ++x) {
175 if (TEST_IF_IS_A_PIXEL(*buffer)) {
177 pixels.push_back(pt);
190 vector< upoint_t >::iterator pos;
192 if (pixels.size() < RCD_MIN_PIXELS) {
199 for (
int i=0; i < 4; ++i) {
200 int ri = rand() % ((int)pixels.size());
201 pos = pixels.begin() + ri;
204 remove_list.push_back(p[i]);
207 if (TBY_SQUARED_DIST(p[1].x, p[1].y, p[2].x, p[2].y) < RCD_MIN_INTERPIX_DIST ||
208 TBY_SQUARED_DIST(p[2].x, p[2].y, p[0].x, p[0].y) < RCD_MIN_INTERPIX_DIST ||
209 TBY_SQUARED_DIST(p[0].x, p[0].y, p[1].x, p[1].y) < RCD_MIN_INTERPIX_DIST ) {
215 pixels.push_back(p[0]);
216 pixels.push_back(p[1]);
217 pixels.push_back(p[2]);
218 pixels.push_back(p[3]);
222 gettimeofday(&now, NULL);
228 calcCircle(p[0], p[1], p[2], center, radius);
231 int r = (int)sqrt(TBY_SQUARED_DIST(center.
x, center.
y, pixels[3].x, pixels[3].y));
232 int dist = (int)(r - radius);
233 dist = (dist >= 0) ? dist : -dist;
234 if (radius <= 0 || (
unsigned int)dist > RCD_MAX_DIST_P4 ) {
238 pixels.push_back(p[0]);
239 pixels.push_back(p[1]);
240 pixels.push_back(p[2]);
241 pixels.push_back(p[3]);
245 gettimeofday(&now, NULL);
251 for (
unsigned int i=0; i < pixels.size(); ++i) {
252 int r = (int)sqrt(TBY_SQUARED_DIST(center.
x, center.
y, pixels[i].x, pixels[i].y));
253 int dist = (int)(r-radius);
254 dist = (dist >= 0) ? dist : -dist;
255 if ((
unsigned int)dist <= RCD_MAX_DIST_A) {
256 pos = pixels.begin() + i;
259 remove_list.push_back(pixels[i]);
267 ( boundary_right > boundary_bottom ? boundary_right : boundary_bottom ) * RCD_HW_RATIO ) {
269 if ( radius > TBY_CIRCLE_RADIUS_MIN &&
270 radius < TBY_CIRCLE_RADIUS_MAX ) {
279 m_Circles.push_back(c);
287 while(!remove_list.empty()) {
288 pixels.push_back(remove_list.back());
289 remove_list.pop_back();
291 gettimeofday(&now, NULL);
295 gettimeofday(&now, NULL);
297 diff_sec = now.tv_sec - start.tv_sec;
298 diff_usec = now.tv_usec - start.tv_usec;
301 diff_usec += 1000000;
304 f_diff_sec = diff_sec + diff_usec / 1000000.f;
306 }
while( (f < RCD_MAX_FAILURES) && (pixels.size() > RCD_MIN_PIXELS) &&
307 ( f_diff_sec < RCD_MAX_TIME) );
312 int RcdCircleModel::getShapeCount(
void)
const 314 return m_Circles.size();
317 Circle* RcdCircleModel::getShape(
int id)
const 319 if (
id < 0 || (
unsigned int)
id >= m_Circles.size())
325 return const_cast<Circle*
>(&m_Circles[id]);
329 Circle* RcdCircleModel::getMostLikelyShape(
void)
const 332 switch (m_Circles.size())
337 return const_cast<Circle*
>(&m_Circles[0]);
339 for (
unsigned int i=1; i < m_Circles.size(); ++i)
340 if (m_Circles[i].radius > m_Circles[cur].radius)
342 return const_cast<Circle*
>(&m_Circles[cur]);
346 void RcdCircleModel::calcCircle(
356 const int &x1=p1.
x, &y1=p1.
y, &x2=p2.
x, &y2=p2.
y, &x3=p3.
x, &y3=p3.
y;
358 int div = 2*((x2-x1)*(y3-y1)-(x3-x1)*(y2-y1));
366 center.
x = ((float)((x2*x2+y2*y2-x1*x1-y1*y1)*(y3-y1)
367 -(x3*x3+y3*y3-x1*x1-y1*y1)*(y2-y1))
369 center.
y = ((float)((x2-x1)*(x3*x3+y3*y3-x1*x1-y1*y1)
370 -(x3-x1)*(x2*x2+y2*y2-x1*x1-y1*y1))
374 radius = (float)sqrt(dx*dx+dy*dy);
Fawkes library namespace.
unsigned int y
y coordinate
unsigned int x
x coordinate
unsigned int width
ROI width.
unsigned char * get_roi_buffer_start(unsigned char *buffer) const
Get ROI buffer start.
Point with cartesian coordinates as unsigned integers.
void fitCircle(std::vector< fawkes::upoint_t > &points)
Fit circle.
unsigned int height
ROI height.
unsigned int line_step
line step
int count
Number of pixels.