std::vector<CCircle> CMinSquareRecognizing::FindCircles()
{
std::vector< std::vector<cv::Point> > contours = getContours();
out.open( imageSavePath + "accuracy.csv", std::ofstream::out );
std::vector<CCircle> result;
for( size_t i = 0; i < contours.size(); ++i ) {
CCircle circle = getCircleByPoints( contours[i] );
if( checkIfCircle( circle, contours[i] ) ) {
result.push_back( circle );
}
}
out.close();
/*std::vector<cv::Point> points;
points.push_back( cv::Point( 15, 7 ) );
points.push_back( cv::Point( 10, 12 ) );
points.push_back( cv::Point( 5, 7 ) );
points.push_back( cv::Point( 10, 2 ) );*/
//points.push_back( cv::Point( 1, 3 ) );
//points.push_back( cv::Point( 2, 8 ) );
/*result.push_back( getCircleByPoints( points ) );*/
return result;
}
void logic::runLogic(){
while (true) {
//get image from stream
cv::Mat cameraFrame,bwFrame,finalFrame;
cameraFrame=v_module->getFrame();
//do a basic blur
cv::medianBlur(cameraFrame, finalFrame, 5);
//threshold region based on rule separateR1
separateRegion(finalFrame, finalFrame);
//close frame
cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(7, 7), cv::Point(3,3));
cv::morphologyEx(finalFrame, finalFrame, cv::MORPH_CLOSE, element);
//get contours
cv::cvtColor(finalFrame, finalFrame, CV_RGB2GRAY);
std::vector<std::vector<cv::Point> > contours;
getContours(finalFrame, cameraFrame, contours);
//get bounding box and COM
int com_x, com_y, bbox_y, bbox_len;
if(contours.size() > 0){
cv::Rect bBox;
cv::Moments moment;
bBox = cv::boundingRect(contours[0]);
moment = cv::moments(contours[0]);
com_x = moment.m10 / moment.m00;
com_y = moment.m01 / moment.m00;
bbox_y = bBox.y;
bbox_len = bBox.height;
cv::rectangle(cameraFrame,bBox,cv::Scalar(0, 255, 0));
cv::circle(cameraFrame, cv::Point(com_x, com_y), 30, cv::Scalar(255, 0, 0), 2);
}
cv::Size s = cameraFrame.size();
int desired_x = s.width/2;
int desired_len = 200;
int deadspot = 10;
int l_motor = 0, r_motor = 0;
if (desired_len > (bbox_len - deadspot)){
//move forward
l_motor = 30;
r_motor = 30;
}else if((desired_len < (bbox_len + deadspot))){
l_motor = -30;
r_motor = -30;
}
//show image
cv::imshow("cam", cameraFrame);
if (cv::waitKey(30) >= 0) break;
}
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
//cv_bridge::CvImagePtr cvPtr = getThreshImageColor( msg );
//cv_bridge::CvImagePtr cvPtr = getThreshImageInfrared( msg );
cv_bridge::CvImagePtr cvPtr = getImage( msg );
equalizeHistogram( cvPtr );
// static int counter = 0;
// if( counter++ % 128 == 0 )
// {
// every 128 frames do a full match
cv_bridge::CvImagePtr threshImagePtr(new cv_bridge::CvImage( cvPtr->header, cvPtr->encoding, cvPtr->image.clone() ) );
thresholdImage( threshImagePtr, /* byColor = */ false );
cv::vector< cv::vector<cv::Point> > contours;
// applyMorphology( threshImagePtr );
contours = getContours( threshImagePtr->image );
cv::vector< double > areaBasedProbs = getAreaBasedProbs( contours );
// cv::vector< double > momentMatchBasedProbs = getMomentMatchBasedProbs( contours );
// cv::vector< double > surfMatchBasedProbs = getSurfMatchBasedProbs( contours, cvPtr );
double maxProb = 0;
int chosenContour = -1;
for( int i = 0; i < contours.size(); i++ )
{
double prob = areaBasedProbs[i]; // * momentMatchBasedProbs[i] * surfMatchBasedProbs[i];
if( prob > maxProb )
{
maxProb = 0;
chosenContour = i;
}
}
// Draw centers & contours
cv::Mat drawing = cv::Mat::zeros( cvPtr->image.size(), CV_8UC3 );
for( int i = 0; i < contours.size(); i++ )
{
if( i == chosenContour )
continue;
cv::Scalar color = cv::Scalar( 50, 150, 150 );
cv::drawContours( drawing, contours, i, color, 2, 8 );
}
geometry_msgs::Point point;
if( chosenContour > -1 )
{
// Get the mass center of the chosen contour
cv::Moments chosenMoments = cv::moments( contours[chosenContour], false );
cv::Point foundCenter = cv::Point( chosenMoments.m10/chosenMoments.m00,
chosenMoments.m01/chosenMoments.m00 );
cv::drawContours( drawing, contours, chosenContour, cv::Scalar( 255, 255, 255 ),
2, 8 );
cv::circle( drawing, foundCenter, 10, cv::Scalar( 255, 255, 255 ), -1, 8, 0 );
point.x = foundCenter.x * 100.0 / cvPtr->image.size().width;
point.y = foundCenter.y * 100.0 / cvPtr->image.size().height;
// distance is the size of the contour relative to targetSize_;
double contourArea = cv::contourArea( contours[chosenContour] );
double imageArea = cvPtr->image.size().width * cvPtr->image.size().height;
// 50 means we are at the target size, lower too close, higher too far
// if contourArea/imageArea is very close to 1 something is wrong,
// regard it as a miss
if( fabs( contourArea/imageArea - 1.0 ) < .05 )
{
point.x = point.y = 0.0;
point.z = -1.0;
missesInARow_++;
}
else
{
point.z = fmax( ( targetSize_ - contourArea/imageArea * 100.0 ) / 2.0 + 50.0, 0.0 );
missesInARow_ = 0;
}
}
else
{
point.x = point.y = 0.0;
point.z = -1.0;
missesInARow_++;
}
if( missesInARow_ > 30 || pointPublished_.z == -1.0 )
{
pointPublished_ = point;
missesInARow_ = 0;
}
else if( point.z != -1.0 )
{
pointPublished_.x = ( point.x + pointPublished_.x * dampingFactor_ ) / ( 1.0 + dampingFactor_ );
pointPublished_.y = ( point.y + pointPublished_.y * dampingFactor_ ) / ( 1.0 + dampingFactor_ );
pointPublished_.z = ( point.z + pointPublished_.z * dampingFactor_ ) / ( 1.0 + dampingFactor_ );
}
// draw the pointPublished
cv::Point drawPointPublished( pointPublished_.x * cvPtr->image.size().width / 100.0,
pointPublished_.y * cvPtr->image.size().height / 100.0 );
cv::circle( drawing, drawPointPublished, 5, cv::Scalar( 255, 0, 255 ), -1, 8, 0 );
// Publish
cv_bridge::CvImage drawingImage( cvPtr->header, enc::BGR8, drawing );
centersPub_.publish( drawingImage.toImageMsg() );
pointPub_.publish( pointPublished_ );
}
cv::Mat getContoursMat(cv::Mat& im_src) {
ContourData contour_data = getContours(im_src);
return drawContours(contour_data, im_src);
}
ContourData getContourPolyMat(cv::Mat& im_src) {
ContourData contour_data = getContours(im_src);
}
