///ボールしきい値デバッグ///
void checkBallSensor3()
{
static int ball[BALL_SENSOR_NUM] = {
0, 0, 0, 0, 0, 0, 0, 0 };
// measure the ball sensor
for (int i = 0; i < BALL_SENSOR_NUM; i++)
{
if(getBall(i) > ball[i])
{
ball[i] = getBall(i);
}
}
if (g_use_lcd == 1)
{
slcd.setCursor(0,0);
slcd.print("B");
for (int j = 0; j < BALL_SENSOR_NUM; j++)
{
if (j == 4) {
slcd.setCursor(0,1);
slcd.print("B");
}
slcd.print(ball[j], DEC);
slcd.print(" ");
}
}
}
void getBallDir()
{
int t_max = -1 , t_max2 = -1 , no = -1 , no2 = -1;
for(int i ; i < 8 ; i++)
{
if(getBall(i) - g_debugBall[i] > t_max)
{
t_max = getBall(i);
no = i;
}
else if(getBall(i) - g_debugBall[i] > t_max2)
{
t_max2 = getBall(i);
no2 = i;
}
}
int value = t_max2 / t_max * 22.5;
if(no > no2) value *= -1;
int value2 = no * 45;
g_BallDir = value + value2;
slcd.setCursor(0,1);
slcd.print("BallDir");
slcd.print(g_BallDir, DEC);
slcd.print(" ");
}
bool RelativePositionDetection::execute() {
// TODO add angle of objects to panAngle and tiltAngle
// TODO use object size in image for distance approximation
mPanAngle = DEGREE_TO_RADIAN * getBodyStatus().getPan();
mTiltAngle = DEGREE_TO_RADIAN * getBodyStatus().getTilt();
const RobotConfiguration& botConf = getRobotConfiguration();
const CameraSettings& camSet = getCameraSettings();
mRadiansPerPixelH = DEGREE_TO_RADIAN * botConf.cameraOpeningHorizontal
/ (double) (camSet.width);
mRadiansPerPixelV = DEGREE_TO_RADIAN * botConf.cameraOpeningVertical
/ (double) (camSet.height);
mPrinciplePointX = botConf.principlePointX;
mPrinciplePointY = botConf.principlePointY;
mCameraHeight = botConf.cameraHeight;
mCameraOffsetX = botConf.cameraOffsetX;
updateVector(getBall(), &getBallVector());
updateVectors( getGoalPoles().getObjects(), &getGoalPolesVectors().data);
updateVectors( getCyanRobots().getObjects(), &getCyanRobotsVectors().data);
updateVectors( getMagentaRobots().getObjects(), &getMagentaRobotsVectors().data);
updateVectors( getRobots().getObjects(), &getRobotsVectors().data);
return true;
}
int getBallDir()
{
int t_max = -1 , no = -1;
for(int i=0 ; i < 8 ; i++)
{
g_ball[i] = getBall(i);
if(g_ball[i] > t_max)
{
t_max = g_ball[i];
no = i;
}
}
if (t_max < BALL_THRESH) g_BallDir = 999; // no ball
else {
g_BallDir = 360 - 45 * no;
}
if (g_BallDir == 360) g_BallDir = 0;
return g_BallDir;
/* slcd.setCursor(0,0);
slcd.print("max:");
slcd.print(t_max, DEC);
slcd.print(" ");
slcd.print(no, DEC);
slcd.setCursor(0,1);
slcd.print("BallDir:");
slcd.print(g_BallDir, DEC);
slcd.print(" "); */
}
BallDetection::BallDetection()
: mBallCenterX( 0),
mBallCenterY( 0),
mBallRadius( 10),
mMinCloudDistance( 30),
mAdditionalScanPixelFactor( 0.6),
mAdditionalScanPixelMin( 1),
mAdditionalScanPixelCount( 20),
mRadiusCompareWeight( 1.0),
mCircleAnomalyCompareWeight( 1.0),
mFoundEdgesRatioCompareWeight( 1.0),
mCorrectEdgesRatioCompareWeight( 1.0),
mPointsAreaRatioCompareWeight( 1.0),
mMinRadius( 2),
mMaxFieldlineError( 5),
mMaxCircleAnomaly( 20.0),
mMinFoundEdgesRatio( 0.4),
mMinCorrectEdgesRatio( 0.5),
mMinPointsAreaRatio( 0.01),
mMaxAnomalyRadiusFactor( 0.2),
mDebugInfoEnabled( false),
mIsFilterFieldLine( true),
mIsFilterHorizon( true),
mMinY( 50),
mMaxU( 140),
mMinV( 140),
mIsAtBorder( false),
mVisionChangeBallDetection(30),
mDebugDrawings(0),
mColor(DebugDrawer::RED)
{
getBall().type = Object::BALL;
Config::getInstance()->registerConfigChangeHandler(this);
this->configChanged();
}
void RelativePositionDetection::showDebugInformations(const Object &object, Vector *pos, double tilt) {
double pan = pos->getAngle();
double distance = pos->getLength();
Debugger::INFO("RelativePositionDetection", "%s-Vector %.1f; %.1f : %.1f",
object.toString().c_str(), pan*RADIAN_TO_DEGREE, tilt*RADIAN_TO_DEGREE, distance);
#ifdef _DEBUG
if( mDebugDrawings >= 2) {
int x = getBall().lastImageTopLeftX;
int y = getBall().lastImageTopLeftY;
DRAW_TEXT(object.toString(), x + 2, y + 10, DebugDrawer::YELLOW,
"%.0f mm (%d px; %.2f deg)", distance, getBall().lastImageWidth, tilt*RADIAN_TO_DEGREE);
DRAW_TEXT(object.toString(), x + 2, y + 24, DebugDrawer::YELLOW,
"Pan: %.2f deg", pan*RADIAN_TO_DEGREE);
}
#endif
}
void getMaxball()
{
int t_max = -1;
g_no = 8;
for(int i = 0; i < BALL_SENSOR_NUM; i++) {
g_ball[i] =-1;
}
for(int i = 0; i < BALL_SENSOR_NUM; i++)
{
g_ball[i] = getBall(i);
if(g_ball[i] - g_debugBall[i] >= t_max)
{
t_max = g_ball[i];
g_no = i;
}
}
}
VOID Game_Paint(HWND hwnd)
{
//3.选用位图对象
SelectObject(g_bdc, g_hBackground);
//4.贴图 背景
BitBlt(g_mdc, 0, 0, WINDOW_WIDTH, WINDOW_HEIGHT, g_bdc, 0, 0, SRCCOPY);
//球
SelectObject(g_bdc, g_hBall);
Ball* ball = getBall();
if (ball != NULL)
{
ball->isLive = true;
g_angle += 0.2;
ball->angle = g_angle;
ball->x = 320 - 32;
ball->y = 240 - 32;
}
for (int i = 0; i < 100; i++)
{
if (balls[i].isLive == true)
{
balls[i].x += balls[i].v * cosf(balls[i].angle);
balls[i].y += balls[i].v * sinf(balls[i].angle);
TransparentBlt(g_mdc, balls[i].x, balls[i].y, 64, 64, g_bdc, 0, 0, 64, 64, RGB(0, 0, 0));
}
if (balls[i].x <= -32 || balls[i].x >= 640 || balls[i].y <= -32 || balls[i].y >= 480)
{
balls[i].isLive = false;
}
}
BitBlt(g_hdc, 0, 0, WINDOW_WIDTH, WINDOW_HEIGHT, g_mdc, 0, 0, SRCCOPY);
}
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;
}
bool BallDetection::execute()
{
Debugger::DEBUG("BallDetection", "BallDetection Start");
#ifdef USE_BALL_DETECTION_DOUBLE
if(getBodyStatus().getTilt() > mVisionChangeBallDetection){
Debugger::DEBUG("BallDetection", "Tilt is too high (%d), OpenCV detection is used!",
getBodyStatus().getTilt());
return false;
}
#endif
//Debugger::DEBUG("BallDetection","Using Ball Detection COLOR %d", getBodyStatus().getTilt());
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) {
for (auto cloud : mBallClouds) {
BoundingBox box = cloud.calculateBoundingBox();
Point centerPoint = cloud.getCenterPoint();
mBallCenterX = centerPoint.getX();
mBallCenterY = centerPoint.getY();
mBallRadius = (box.width + box.height) / 4; //3 // 4
//Debugger::INFO("BallDetection", "Cloud-Radius: %d", ballRadius);
//Debugger::DEBUG("BallDetection", "maxValue: %i", maxValue);
if (filterFieldLines(centerPoint)
&& filterHorizon(centerPoint)) {
// 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)) {
DEBUG_DRAWING_BALL(box, DebugDrawer::GREEN, 2);
//break;
}
ballCompareData.centerX = mBallCenterX;
ballCompareData.centerY = mBallCenterY;
ballCompareData.numCloudPixels = cloud.getSize();
DEBUG_DRAWING_CENTER(DebugDrawer::YELLOW, 6);
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;
DEBUG_DRAWING_CENTER(DebugDrawer::BLUE, 6);
}
centerPoint.updatePoint(mBallCenterX, mBallCenterY);
if (filterFieldLines(centerPoint)
&& filterHorizon(centerPoint)) {
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);
//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;
for (double radius : radiusList) {
double anomaly = (double)mBallRadius - radius;
anomaly *= anomaly;
#ifdef _DEBUG
if( mDebugInfoEnabled) {
Debugger::DEBUG("BallDetection", "%d) Anomaly centerPoint: %.2f", i, anomaly);
}
#endif
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);
#ifdef _DEBUG
if( mDebugInfoEnabled) {
Debugger::INFO("BallDetection", "CompareResult: %f", compareResult);
}
#endif
if( compareResult > bestCompareResult) {
bestCompareResult = compareResult;
bestCompareBox = box;
}
DEBUG_DRAWING_BALL(box, DebugDrawer::BROWN, 3);
} else {
DEBUG_DRAWING_BALL(box, DebugDrawer::ORANGE, 4);
}
} else {
Object obj(box.topLeft.getX(), box.topLeft.getY(), box.width, box.height);
obj.type = Object::BALL;
getRemovedBallCandidates().addObject(obj);
DEBUG_DRAWING_BALL(box, DebugDrawer::LIGHT_GRAY, 5);
}
}
if( bestCompareResult > 0.0) {
int x = bestCompareBox.topLeft.getX();
int y = bestCompareBox.topLeft.getY();
getBall().updateObject( x, y, bestCompareBox.width, bestCompareBox.height);
DEBUG_DRAWING_BALL(bestCompareBox, DebugDrawer::RED, 1);
} else {
getBall().notSeen();
}
Debugger::DEBUG("BallDetection", "BallDetection END");
return true;
}
void fight()
{
PID();
if (g_diff > 30 || g_diff < -30) turn(g_pid);
if (getBall(0) > g_debugBall[0] && getBall(0) > getBall(1) && getBall(0) >= getBall(2) && getBall(0) >= getBall(3) && getBall(0) >= getBall(4) && getBall(0) >= getBall(5) && getBall(0) >= getBall(6) && getBall(0) > getBall(7) ) moveAngle4ch(0,0,0);
else if(getBall(1) > g_debugBall[1] && getBall(1) >= getBall(2) && getBall(1) > getBall(3) && getBall(1) > getBall(4) && getBall(1) > getBall(5) && getBall(1) > getBall(6) && getBall(1) > getBall(7))
{
//if (getPing(3) < 15) moveAngle4ch(50 , 0, 0);
if (getBall(1) > 600) moveAngle4ch(50, 0, 95);
else moveAngle4ch(85 , 0, 95);
}
else if(getBall(2) > g_debugBall[2] && getBall(2) >= getBall(3) && getBall(2) > getBall(4) && getBall(2) > getBall(5) && getBall(2) > getBall(6) && getBall(2) > getBall(7))
{
if(getBall(2) > 600)moveAngle4ch(50 , 0, 95);
else moveAngle4ch(85, 0, 95);
}
else if (getBall(6) > g_debugBall[6] && getBall(6) >= getBall(5))
{
if (getBall(6) > 600)moveAngle4ch(50 , 0, 265);
else moveAngle4ch(85, 0, 265);
}
else if(getBall(7) > g_debugBall[7] && getBall(7) > getBall(1))
{
//if(getPing(1) < 15) moveAngle4ch(50 , 0, 0);
if (getBall(7) > 600) moveAngle4ch(50, 0, 265);
else moveAngle4ch(85, 0, 265);
}
else moveAngle4ch(0,0,0);
}
bool BallDetectionOpenCV::execute() {
#ifdef USE_OPENCV
//get cr channel
const cv::Mat& mat = getOpenCVImage().getChannelCR();
cv::Point minLoc;
cv::Point maxLoc;
double minVal;
double maxVal;
cv::Mat blurred;
cv::GaussianBlur(mat, blurred, cv::Size(21,21), 0, 0, cv::BORDER_DEFAULT);
cv::minMaxLoc(blurred, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat());
// Debugger::DEBUG("BallDetectionOpenCV", "maxVal: %f", maxVal);
//
// Debugger::DEBUG("BallDetectionOpenCV", "minLoc :%d | %d ", minLoc.x,
// minLoc.y);
// vector<cv::Vec3f> circles;
//
// /// Apply the Hough Transform to find the circles
// cv::HoughCircles(mat, circles, CV_HOUGH_GRADIENT, 1, mat.rows / 8, 50, 100,
// 0, 0);
//
// for (size_t i = 0; i < circles.size(); ++i) {
// Object ball;
//
// ball.lastImageX = circles[i].val[0];
// ball.lastImageY = circles[i].val[1];
// ball.lastImageWidth = circles[i].val[2];
// ball.lastImageHeigth = circles[i].val[2];
//
// Debugger::DEBUG("BallDetectionOpenCV", "Circle Found");
//
// getBalls().addObject(ball);
// }
//
//
//
// cv::Mat thresholded;
//
// cv::threshold(mat, thresholded, maxVal-50, 255, cv::THRESH_BINARY);
//
// cv::imshow("test", thresholded);
//
// vector<cv::Mat> contours;
//
// cv::findContours(thresholded, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0));
if (maxVal > 200) {
getBall().updateObject(maxLoc.x, maxLoc.y, maxLoc.x, maxLoc.y);
} else {
getBall().notSeen();
}
#endif
return true;
}