bool getRectFromTopic(
recognition::RectFromTopic::Request &request,
recognition::RectFromTopic::Response &response
)
{
if (lastImage == NULL)
return false;
cv_bridge::CvImage cvImg, cvMask;
cvImg.header.stamp = ros::Time::now();
cvImg.encoding = sensor_msgs::image_encodings::TYPE_8UC3;
cvImg.image = lastImage->image(
safeRect(
lastImage->image,
request.x,
request.y,
request.width,
request.height
)
);
response.image = *cvImg.toImageMsg();
ROS_INFO("Image");
cv::Mat mask;
if (lastDepth != NULL)
{
ROS_INFO("Depth");
cv::Mat newDepth = lastDepth->image(
safeRect(
lastDepth->image,
request.x,
request.y,
request.width,
request.height
)
);
_getMaskFromDepth(newDepth, mask);
cvMask.header.stamp = ros::Time::now();
cvMask.encoding = sensor_msgs::image_encodings::TYPE_8UC1;
cvMask.image = mask;
response.mask = *cvMask.toImageMsg();
}
return true;
}
bool insertTopic(
recognition::InsertTopic::Request &request,
recognition::InsertTopic::Response &response
)
{
if (lastImage == NULL)
return false;
cv::Mat newImage = lastImage->image(
safeRect(
lastImage->image,
request.x,
request.y,
request.width,
request.height
)
);
cv::Mat mask;
if (lastDepth != NULL)
{
cv::Mat newDepth = lastDepth->image(
safeRect(
lastDepth->image,
request.x,
request.y,
request.width,
request.height
)
);
_getMaskFromDepth(newDepth, mask);
}
algorithm->insertIntoDb(newImage, request.label, mask);
response.success = true;
return true;
}
bool recognizeFromTopic(
recognition::RecognizeFromTopic::Request &request,
recognition::RecognizeFromTopic::Response &response
)
{
if (lastImage == NULL)
return false;
cv::Mat newImg = lastImage->image(
safeRect(
lastImage->image,
request.x,
request.y,
request.width,
request.height
)
);
cv::Mat mask;
if (lastDepth != NULL)
{
cv::Mat newDepth = lastDepth->image(
safeRect(
lastDepth->image,
request.x,
request.y,
request.width,
request.height
)
);
_getMaskFromDepth(newDepth, mask);
}
response.label = algorithm->recognize(newImg, mask);
return true;
}
double getSingleSurfProb( const cv::vector< cv::Point > &contour, cv_bridge::CvImagePtr cvPtr, int contourNum = 0 )
{
// Get the ROI for this contour
cv::Rect boundingRect = cv::boundingRect( contour );
cv::Mat imageROI = cvPtr->image( boundingRect );
cv::vector< cv::KeyPoint > keypoints;
surf_.detect( imageROI, keypoints );
cv::Mat descriptors;
surfDesc_.compute( imageROI, keypoints, descriptors );
std::vector< cv::DMatch > matches;
matcher_.match( descriptors, descriptors_, matches );
// only take the 25 best matches
std::nth_element( matches.begin(), matches.begin() + 24, matches.end() );
matches.erase( matches.begin() + 25 );
// get the distance of the 25 best matches
double sum = 0;
for( int i = 0; i < matches.size(); i++ )
sum += 0; // matches[i].distance;
// draw the matches
#ifdef NOIMSHOW
cv::Mat drawing = cv::Mat::zeros( cvPtr->image.size(), CV_8UC3 );
cv::drawMatches( roundelImage_, descriptors_,
ctPtr->image, descriptors,
matches,
drawing,
cv::Scalar( 255, 255, 255 ) );
char windowName[255];
sprintf( windowName, "Surf Matches %d", contourNum );
cv::namedWindow( windowName );
cv::imshow( windowName, drawing );
#endif
// the probability is how close we are to 0
return( fmax( 1.0 - sum, 0.0 ) );
}