void depthCallback(const sensor_msgs::ImageConstPtr& msg)
{
if (!LineMOD_Detector::got_color)
{
return;
}
cv_bridge::CvImagePtr depth_ptr;
bool show_match_result = true;
try
{
depth_ptr = cv_bridge::toCvCopy(msg, "");
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("depth cv_bridge exception: %s", e.what());
return;
}
LineMOD_Detector::sources.push_back(LineMOD_Detector::color_img);
LineMOD_Detector::sources.push_back(depth_ptr->image);
// Perform matching
std::vector<cv::linemod::Match> matches;
std::vector<cv::String> class_ids;
std::vector<cv::Mat> quantized_images;
LineMOD_Detector::detector->match(sources, (float)LineMOD_Detector::matching_threshold, matches, class_ids, quantized_images);
LineMOD_Detector::num_classes = detector->numClasses();
int classes_visited = 0;
std::set<std::string> visited;
for (int i = 0; (i < (int)matches.size()) && (classes_visited < LineMOD_Detector::num_classes); ++i)
{
cv::linemod::Match m = matches[i];
if (visited.insert(m.class_id).second)
{
++classes_visited;
if (show_match_result)
{
printf("Similarity: %5.1f%%; x: %3d; y: %3d; class: %s; template: %3d\n",
m.similarity, m.x, m.y, m.class_id.c_str(), m.template_id);
}
// Draw matching template
const std::vector<cv::linemod::Template>& templates = LineMOD_Detector::detector->getTemplates(m.class_id, m.template_id);
drawResponse(templates, LineMOD_Detector::num_modalities, LineMOD_Detector::display, cv::Point(m.x, m.y), LineMOD_Detector::detector->getT(0));
//std::cout << "pub_threshold " << LineMOD_Detector::pub_threshold << std::endl;
if (m.similarity > LineMOD_Detector::pub_threshold)
{
LineMOD_Detector::publishPoint(templates, m, depth_ptr->image, msg->header);
}
}
}
LineMOD_Detector::sources.clear();
}
void disparityCallback(const stereo_msgs::DisparityImageConstPtr& msg)
{
ROS_DEBUG("Pub Threshold:%f ", LineMOD_Detector::pub_threshold);
if (!LineMOD_Detector::got_color)
{
return;
}
bool show_match_result = true;
cv_bridge::CvImagePtr disp_ptr;
try
{
disp_ptr = cv_bridge::toCvCopy(msg->image, "");
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("disp cv_bridge exception: %s", e.what());
return;
}
float f = msg->f;
float T = msg->T;
cv::Mat depth_img = (f*T*1000)/(disp_ptr->image).clone();
depth_img.convertTo(depth_img, CV_16U);
LineMOD_Detector::sources.push_back(LineMOD_Detector::color_img);
LineMOD_Detector::sources.push_back(depth_img);
// Perform matching
std::vector<cv::linemod::Match> matches;
std::vector<cv::String> class_ids;
std::vector<cv::Mat> quantized_images;
LineMOD_Detector::detector->match(sources, (float)LineMOD_Detector::matching_threshold, matches, class_ids, quantized_images);
LineMOD_Detector::num_classes = detector->numClasses();
ROS_DEBUG("Num Classes: %u", LineMOD_Detector::num_classes);
int classes_visited = 0;
std::set<std::string> visited;
ROS_DEBUG("Matches size: %u", (int)matches.size());
for (int i = 0; (i < (int)matches.size()) && (classes_visited < LineMOD_Detector::num_classes); ++i)
{
cv::linemod::Match m = matches[i];
ROS_DEBUG("Matching count: %u", i);
if (visited.insert(m.class_id).second)
{
++classes_visited;
if (show_match_result)
{
ROS_DEBUG("Similarity: %5.1f%%; x: %3d; y: %3d; class: %s; template: %3d\n",
m.similarity, m.x, m.y, m.class_id.c_str(), m.template_id);
printf("Similarity: %5.1f%%; x: %3d; y: %3d; class: %s; template: %3d\n",
m.similarity, m.x, m.y, m.class_id.c_str(), m.template_id);
}
// Draw matching template
const std::vector<cv::linemod::Template>& templates = LineMOD_Detector::detector->getTemplates(m.class_id, m.template_id);
drawResponse(templates, LineMOD_Detector::num_modalities, LineMOD_Detector::display, cv::Point(m.x, m.y), LineMOD_Detector::detector->getT(0));
if (m.similarity > LineMOD_Detector::pub_threshold)
{
LineMOD_Detector::publishPoint(templates, m, depth_img, msg->header);
}
}
}
LineMOD_Detector::sources.clear();
}