void HandTranslating::getFeature7To10(int ¤tFeatures) {
int halfWidth = imageWidth / 2;
int halfHeight = imageHeight / 2;
Mat subImage;
Rect rect1(0, 0, halfWidth, halfHeight);
subImage = binaryMat(rect1);
double feature = (double) (subImage.total()-countNonZero(subImage))
/ (double) subImage.total();
currentFeatures++;
nodeTest[currentFeatures-1].index = currentFeatures;
nodeTest[currentFeatures-1].value = feature;
Rect rect2(halfWidth, 0, halfWidth, halfHeight);
subImage = binaryMat(rect2);
feature = (double) (subImage.total()-countNonZero(subImage)) / (double) subImage.total();
currentFeatures++;
nodeTest[currentFeatures-1].index = currentFeatures;
nodeTest[currentFeatures-1].value = feature;
Rect rect3(0, halfHeight, halfWidth, halfHeight);
subImage = binaryMat(rect3);
feature = (double) (subImage.total()-countNonZero(subImage)) / (double) subImage.total();
currentFeatures++;
nodeTest[currentFeatures-1].index = currentFeatures;
nodeTest[currentFeatures-1].value = feature;
Rect rect4(halfWidth, halfHeight, halfWidth, halfHeight);
subImage = binaryMat(rect4);
feature = (double) (subImage.total()-countNonZero(subImage)) / (double) subImage.total();
currentFeatures++;
nodeTest[currentFeatures-1].index = currentFeatures;
nodeTest[currentFeatures-1].value = feature;
}
void HandTranslating::getFeature1To3(int ¤tFeatures) {
int step = 3;
int range = imageHeight / step;
int position = 0;
for (position = 0; position < imageHeight; position += range) {
Rect rect(0, position, imageWidth, range);
Mat subMat = binaryMat(rect);
double feature = (double) (subMat.total()-countNonZero(subMat))
/ (double) subMat.total();
currentFeatures++;
nodeTest[currentFeatures-1].index = currentFeatures;
nodeTest[currentFeatures-1].value = feature;
}
}
void HandTranslating::getFeature15To30(int ¤tFeatures) {
int halfWidth = imageWidth / 4;
int halfHeight = imageHeight / 4;
Mat subImage;
for (int row = 0; row < imageHeight; row += halfHeight) {
for (int col = 0; col < imageWidth; col += halfWidth) {
Rect rect(row, col, halfWidth, halfHeight);
subImage = binaryMat(rect);
double feature = (double) (subImage.total() - countNonZero(subImage))
/ (double) subImage.total();
currentFeatures++;
nodeTest[currentFeatures-1].index = currentFeatures;
nodeTest[currentFeatures-1].value = feature;
}
}
}
void HoleDetector::getImageCallback(const sensor_msgs::ImageConstPtr& original_image)
{
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
}
catch (cv_bridge::Exception& e){
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::imshow("original", cv_ptr->image);
cv::Mat grayMat;
// Convert it to gray
cv::cvtColor( cv_ptr->image, grayMat, CV_BGR2GRAY );
cv::imshow("grayscale", grayMat);
// Convert to binary
cv::Mat binaryMat(grayMat.size(), grayMat.type());
cv::Mat binaryMatTest(grayMat.size(), grayMat.type());
//Apply thresholding
// cv::threshold(grayMat, binaryMat, 100, 255, CV_THRESH_BINARY | CV_THRESH_OTSU);
cv::threshold(grayMat, binaryMat, 100, 255, CV_THRESH_BINARY);
cv::threshold(grayMat, binaryMatTest, 200, 255, CV_THRESH_BINARY);
cv::imshow("binary", binaryMat);
// cvMoveWindow(WINDOW, 50, 50);
// Reduce the noise so we avoid false circle detection
int erosion_type = cv::MORPH_ELLIPSE;
int ksize1 =2;
int ksize2 = 4;
cv::Mat kernel1 = cv::getStructuringElement(erosion_type,cv::Size(ksize1,ksize1));
cv::Mat kernel2 = cv::getStructuringElement(erosion_type,cv::Size(ksize2,ksize2));
cv::dilate(binaryMat, binaryMat, kernel1);
cv::erode(binaryMat, binaryMat, kernel2);
// cv::Canny(src_gray, src_gray, 5, 70, 3);
// GaussianBlur(grayMat, grayMat, cv::Size(7, 7), 2, 2 );
// GaussianBlur(src_gray, src_gray, cv::Size(9, 9), 2, 2 );
cv::imshow("binary_filtered", binaryMat);
// Apply the Hough Transform to find the circles
// cv::HoughCircles(src_gray, mCircles, CV_HOUGH_GRADIENT, 2, 32.0, 50, 140, 0, 0 );
// Draw the circles detected
// for(size_t i = 0; i < mCircles.size(); i++ )
// {
// cv::Point center(cvRound(mCircles[i][0]), cvRound(mCircles[i][1]));
// int radius = cvRound(mCircles[i][2]);
// // circle center
// circle(cv_ptr->image, center, 3, cv::Scalar(0,255,0), -1, 8, 0 );
// circleare an external function as friend of a class, thus allowing this function to have
// circle(cv_ptr->image, center, radius, cv::Scalar(0,0,255), 3, 8, 0 );
// }
//Display the Data in the OpenCV HighGUI Window.
// cv::imshow(WINDOW, binaryMat);
cv::SimpleBlobDetector::Params params;
params.filterByColor = true;
params.blobColor = 0;
params.minDistBetweenBlobs = 5.0f;
params.filterByInertia = true;
params.minInertiaRatio = 0.2;
params.maxInertiaRatio = 1.0;
params.filterByConvexity = false;
params.minConvexity = 1.0;
params.maxConvexity = 10.0;
params.filterByCircularity = false;
params.filterByArea = true;
params.minArea = 3.0f;
params.maxArea = 400.0f;
cv::Ptr<cv::FeatureDetector> blob_detector = new cv::SimpleBlobDetector(params);
blob_detector->create("SimpleBlob");
std::vector<cv::KeyPoint> keypoints;
// blob_detector->detect(cv_ptr->image, keypoints);
blob_detector->detect(binaryMat, keypoints);
for (int i=0; i<keypoints.size(); i++){
float X=keypoints[i].pt.x;
float Y=keypoints[i].pt.y;
cv::circle(cv_ptr->image,cv::Point(X,Y),5,cv::Scalar(0,255,0),-1);
}
cv::imshow("simple_blob", cv_ptr->image);
cv::waitKey(3);
/* Set tolerance as fraction of smallest image dimension*/
double coeff = 0.5;
int width = cv_ptr->image.cols;
int height = cv_ptr->image.rows;
const double tol = coeff * std::min(width,height);
float center[] = { width*0.5, height*0.5};
float toolXY[2];
float targetXY[] = {0.0, 0.0};
/* Set tool tip to center for now */
toolXY[0] = center[0];
toolXY[1] = center[1];
if( getTargetPosition (keypoints, toolXY, targetXY, tol) ) {
hole_detection::PositionXY target_msg;
target_msg.X = targetXY[0] - toolXY[0];
target_msg.Y = targetXY[1] - toolXY[1];
target_msg.header.stamp = ros::Time::now();
TargetPosition.publish(target_msg);
cv::circle(cv_ptr->image,cv::Point(toolXY[0],toolXY[1]),5,cv::Scalar(255,0,0),-1);
cv::circle(cv_ptr->image,cv::Point(targetXY[0],targetXY[1]),5,cv::Scalar(0,0,255),-1);
}
cv::imshow("targeted", cv_ptr->image);
mImagePub.publish(cv_ptr->toImageMsg());
}
