/*******************************************************************************
*
* Helper functions
*
******************************************************************************/
static void
draw_basis_circles
(struct app* app,
const float pos[3],
const float size,
const float col_x[3],
const float col_y[3],
const float col_z[3],
const uint32_t pick_id_x,
const uint32_t pick_id_y,
const uint32_t pick_id_z)
{
assert(app && pos);
#define DRAW_CIRCLE(pitch, yaw, roll, color, pick) \
APP(imdraw_ellipse \
(app, \
APP_IMDRAW_FLAG_UPPERMOST_LAYER | APP_IMDRAW_FLAG_FIXED_SCREEN_SIZE, \
pick, \
pos, \
(float[]){size, size}, \
(float[]){(pitch), (yaw), (roll)}, \
(color)))
DRAW_CIRCLE(PI * 0.5f, 0.f, 0.f, col_x, pick_id_x);
DRAW_CIRCLE(0.f, 0.f, 0.f, col_y, pick_id_y);
DRAW_CIRCLE(0.f, PI * 0.5f, 0.f, col_z, pick_id_z);
#undef DRAW_CIRCLE
}
bool BallDetection::execute()
{
mBallClouds.clear();
mBallCloudDataList.clear();
getRemovedBallCandidates().clearList();
mColor = DebugDrawer::RED;
const list<Point>* cloud = getBallPoints().getCloud();
list<Point>::const_iterator it = cloud->begin();
for( ; it != cloud->end(); ++it) {
addBallPointToNearestCloud( (*it));
}
//mergeOverlappingClouds();
BoundingBox bestCompareBox(Point(0,0),0,0);
double bestCompareResult = 0.0;
vector<PointCloud>::const_iterator pointCloudIt;
for( pointCloudIt = mBallClouds.begin(); pointCloudIt != mBallClouds.end(); ++pointCloudIt) {
BoundingBox box = pointCloudIt->calculateBoundingBox();
Point point = pointCloudIt->getCenterPoint();
mBallCenterX = point.getX();
mBallCenterY = point.getY();
mBallRadius = (box.width + box.height) / 4; //3 // 4
//Debugger::INFO("BallDetection", "Cloud-Radius: %d", ballRadius);
//Debugger::DEBUG("BallDetection", "maxValue: %i", maxValue);
if (filterFieldLines(Point(mBallCenterX, mBallCenterY))
&& filterHorizon(Point(mBallCenterX, mBallCenterY))) {
// todo: filter out all possible balls with too small pixel to size ratio
// filter out all balls with wrong expected size (tilt angle)
getBallEdges().clearCloud();
//Debugger::DEBUG("BallDetection", "-----------------------------");
//Debugger::DEBUG("BallDetection", "BallRadius 1: %d", mBallRadius);
mIsAtBorder = false;
BallCompareData ballCompareData;
if( !findAllBallEdges(ballCompareData)) {
if (mDebugDrawings >= 2) {
DRAW_CIRCLE("Ball", box.topLeft.getX(), box.topLeft.getY(), 2*mBallRadius,
DebugDrawer::GREEN);
}
//break;
}
ballCompareData.centerX = mBallCenterX;
ballCompareData.centerY = mBallCenterY;
ballCompareData.numCloudPixels = pointCloudIt->getSize();
if (mDebugDrawings >= 6) {
DRAW_POINT("Ball", mBallCenterX, mBallCenterY, DebugDrawer::YELLOW);
}
int countEdges = (int)getBallEdges().getSize();
int sumX = 0;
int sumY = 0;
list<Point>::const_iterator it2;
for (it2 = getBallEdges().getCloud()->begin();
it2 != getBallEdges().getCloud()->end(); ++it2) {
Point p = (*it2);
sumX += p.getX();
sumY += p.getY();
}
if( countEdges > 1) {
mBallCenterX = sumX / countEdges;
mBallCenterY = sumY / countEdges;
if (mDebugDrawings >= 6) {
DRAW_POINT("Ball", mBallCenterX, mBallCenterY, DebugDrawer::BLUE);
}
}
double sumDistances = 0.0;
vector<double> radiusList;
int i = 0;
list<Point>::const_iterator pointIt;
for (pointIt = getBallEdges().getCloud()->begin();
pointIt != getBallEdges().getCloud()->end(); ++pointIt) {
Point p = (*pointIt);
double dx = (double)mBallCenterX - p.getX();
double dy = (double)mBallCenterY - p.getY();
double rad = sqrt((dx * dx) + (dy * dy));
radiusList.push_back(rad);
sumDistances += rad;
/*if( mDebugInfoEnabled) {
Debugger::DEBUG("BallDetection", "%d) Radius point: %.2f (%d; %d)", i, rad, p.getX(), p.getY());
}*/
++i;
}
if( countEdges >= 4) {
mBallRadius = (int)(sumDistances / (double)countEdges);
//Debugger::INFO("Ball", "Mean-Radius: %d", ballRadius);
}
//Debugger::DEBUG("BallDetection", "BallRadius 2: %d", mBallRadius);
if (filterFieldLines(Point(mBallCenterX, mBallCenterY))
&& filterHorizon(Point(mBallCenterX, mBallCenterY))) {
//Debugger::DEBUG("BallDetection", "ballCenter: %d, %d", ballCenterX, ballCenterY);
//Debugger::DEBUG("BallDetection", "radius: %d", ballRadius);
ballCompareData.meanRadius = mBallRadius;
ballCompareData.circleAnomaly = 0.0;
int numCorrectEdges = 0;
double maxAnomaly = (mMaxAnomalyRadiusFactor * mBallRadius) * (mMaxAnomalyRadiusFactor * mBallRadius);
//double maxAnomaly = 25; // 5 pixel
//Debugger::DEBUG("BallDetection", "maxAnomaly: %.2f", maxAnomaly);
i = 0;
vector<double>::const_iterator doubleIt;
for (doubleIt = radiusList.begin(); doubleIt != radiusList.end(); ++doubleIt) {
double anomaly = (double)mBallRadius - (*doubleIt);
anomaly *= anomaly;
if( mDebugInfoEnabled) {
Debugger::DEBUG("BallDetection", "%d) Anomaly point: %.2f", i, anomaly);
}
if( anomaly > maxAnomaly) {
ballCompareData.circleAnomaly += anomaly;
//Debugger::DEBUG("BallDetection", "CircleAnomaly: %.2f",ballCompareData.circleAnomaly);
} else {
numCorrectEdges++;
}
++i;
}
if( numCorrectEdges > 0) {
//Debugger::DEBUG("BallDetection", "CircleAnomaly1: %.2f (%d)",ballCompareData.circleAnomaly, numCorrectEdges);
ballCompareData.circleAnomaly /= (double)numCorrectEdges;
}
//Debugger::DEBUG("BallDetection", "CircleAnomaly2: %.2f",ballCompareData.circleAnomaly);
if( countEdges > 0) {
ballCompareData.correctEdgesRatio = (double)numCorrectEdges / (double)countEdges;
}
box.topLeft = Point(mBallCenterX - mBallRadius, mBallCenterY - mBallRadius);
box.width = 2 * mBallRadius;
box.height = 2 * mBallRadius;
double compareResult = calculateCompareResult(ballCompareData);
if( mDebugInfoEnabled) {
Debugger::INFO("BallDetection", "CompareResult: %f", compareResult);
}
if( compareResult > bestCompareResult) {
bestCompareResult = compareResult;
bestCompareBox = box;
}
if (mDebugDrawings >= 3) {
DRAW_CIRCLE("Ball", box.topLeft.getX() +1, box.topLeft.getY() +1, box.width,
DebugDrawer::BROWN);
}
} else {
if (mDebugDrawings >= 4) {
DRAW_CIRCLE("Ball", box.topLeft.getX(), box.topLeft.getY(), box.width,
DebugDrawer::ORANGE);
}
}
} else {
Object obj(box.topLeft.getX(), box.topLeft.getY(), box.width, box.height);
obj.type = Object::BALL;
getRemovedBallCandidates().addObject(obj);
if (mDebugDrawings >= 5) {
DRAW_CIRCLE("Ball", box.topLeft.getX(), box.topLeft.getY(), box.width,
DebugDrawer::LIGHT_GRAY);
}
}
}
if( bestCompareResult > 0.0) {
int x = bestCompareBox.topLeft.getX();
int y = bestCompareBox.topLeft.getY();
getBall().updateObject( x, y, bestCompareBox.width, bestCompareBox.height);
//x = getBall().lastImageX;
//y = getBall().lastImageY;
// todo add angle from position in image
double panAngle = DEGREE_TO_RADIANS * getBodyStatus().getPan();
double tiltAngle = DEGREE_TO_RADIANS * getBodyStatus().getTilt();
double length = getRobotConfiguration().cameraHeight * tan(tiltAngle);
length += getRobotConfiguration().cameraOffsetX;
getBall().lastVector.updateVector(panAngle, length);
Debugger::INFO("BallDetection", "BallVector %.1f; %.1f : %.1f", panAngle*RADIANS_TO_DEGREE, tiltAngle*RADIANS_TO_DEGREE, length);
if( mDebugDrawings >= 1) {
x = bestCompareBox.topLeft.getX();
y = bestCompareBox.topLeft.getY();
DRAW_CIRCLE("Ball", x, y, bestCompareBox.width, DebugDrawer::RED);
if( mDebugDrawings >= 2) {
DRAW_TEXT("Ball", x + 2, y + 10, DebugDrawer::YELLOW,
"%.0f mm (%d px; %.2f deg)", length, bestCompareBox.width, tiltAngle*RADIANS_TO_DEGREE);
DRAW_TEXT("Ball", x + 2, y + 24, DebugDrawer::YELLOW,
"Pan: %.2f deg", panAngle*RADIANS_TO_DEGREE);
}
}
} else {
getBall().notSeen();
}
return true;
}
//---------------------------------------------------------
void CWKSP_Shapes_Point::Draw_Symbol(wxDC &dc, int x, int y, int size)
{
switch( m_Symbol_Type )
{
default:
case 0: // circle
DRAW_CIRCLE(1.0);
break;
case 1: // square
DRAW_SQUARE;
break;
case 2: // rhombus
DRAW_RHOMBUS;
break;
case 3: // triangle up
DRAW_TRIANGLE_UP;
break;
case 4: // triangle down
DRAW_TRIANGLE_DOWN;
break;
case 5: // square in circle
DRAW_CIRCLE(1.0);
DRAW_SQUARE;
break;
case 6: // rhombus in circle
DRAW_CIRCLE(1.1);
DRAW_RHOMBUS;
break;
case 7: // triangle up in circle
DRAW_CIRCLE(1.0);
DRAW_TRIANGLE_UP;
break;
case 8: // triangle down in circle
DRAW_CIRCLE(1.0);
DRAW_TRIANGLE_DOWN;
break;
case 9: // circle in square
DRAW_SQUARE;
DRAW_CIRCLE(0.7);
break;
case 10: // circle in rhombus
DRAW_RHOMBUS;
DRAW_CIRCLE(0.5 * sqrt(2.0));
break;
case 11: // circle in triangle up
DRAW_TRIANGLE_UP;
DRAW_CIRCLE(0.5);
break;
case 12: // circle in triangle down
DRAW_TRIANGLE_DOWN;
DRAW_CIRCLE(0.5);
break;
case 13: // image
{
double d = (double)m_Symbol.GetWidth() / (double)m_Symbol.GetHeight();
int sx, sy;
if( d > 1.0 )
{
sx = size;
sy = (int)(0.5 + size / d);
}
else
{
sx = (int)(0.5 + size * d);
sy = size;
}
if( sx > 0 && sy > 0 )
{
dc.DrawBitmap(wxBitmap(m_Symbol.Scale(2 * sx, 2 * sy)), x - sx, y - sy, true);
}
}
break;
}
}
