void FeatureVisualization::overlayAndPublishDisparity(const stereo_msgs::DisparityImage& disparity_img, cv::Mat imageWithFeatures)
{
//TODO:: find a better way to convert disparity img to cv.
const float d_max = disparity_img.max_disparity - disparity_img.min_disparity;
sensor_msgs::ImagePtr img_ptr(new sensor_msgs::Image(disparity_img.image));
cv::Mat disparity = convertSensorMsgToCV(img_ptr);
cv::Mat output_img;
overlayImages(imageWithFeatures, disparity, d_max, output_img);
dispaity_image_publisher_.publish(convertCVToSensorMsg(output_img));
}
void stereoImagesCallback(const sensor_msgs::ImageConstPtr & msg1,
const sensor_msgs::ImageConstPtr & msg2 )
{
cv::Mat img_matches;
cv::Mat image_left = convertSensorMsgToCV (msg1);
cv::Mat image_right = convertSensorMsgToCV (msg2);
std::vector<cv::Point2f> template_points,search_points;
matcher_.getMatches(image_left, image_right, img_matches, template_points, search_points);
publisher_.publish (convertCVToSensorMsg (img_matches));
}
void TemplateMatcher::imageCallback (const sensor_msgs::ImageConstPtr & msg)
{
cv::Mat search_image = convertSensorMsgToCV (msg);
cv::Mat img_matches;
std::vector<cv::Point2f> template_points,search_points;
matcher_.getMatches(template_image_, search_image, img_matches, template_points, search_points);
publisher_.publish (convertCVToSensorMsg (img_matches));
}
void FeatureVisualization::featuresCallback(const feature_msgs::stereo_matchesConstPtr feature_locations_msg_)
{
cv::Mat left_img, right_img, left_output_img, right_output_img;
sensor_msgs::Image* img_ptr = left_image_buffer_.retrieveImageFromCamInfo(feature_locations_msg_->l_camera_info);
if(img_ptr != NULL)
{
left_img = convertSensorMsgToCV(*img_ptr);
drawFeatures(left_img, feature_locations_msg_->lc, feature_locations_msg_->lp, left_output_img);
left_image_publisher_.publish(convertCVToSensorMsg(left_output_img));
}
stereo_msgs::DisparityImage* disp_ptr = disparity_buffer_.retrieveImageFromCamInfo(feature_locations_msg_->l_camera_info);
// calculation of disparity sometimes fails
// therefore just take the most recent one in the buffer when none is found for visualization
if(disp_ptr == NULL)
disp_ptr = disparity_buffer_.getLastImage();
if((disp_ptr != NULL) && (img_ptr != NULL))
overlayAndPublishDisparity(*disp_ptr, left_output_img);
}
void TemplateMatcher::cloudCallback (const sensor_msgs::PointCloud2Ptr& cloud_msg)
{
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr dense_cloud_color_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
// pcl::fromROSMsg(*cloud_msg,*dense_cloud_color_ptr);
// pcl::copyPointCloud(*dense_cloud_color_ptr, *current_cloud_ptr_);
// cv::Mat search_image = template_library_.restoreCVMatFromPointCloud(dense_cloud_color_ptr);
// cv::Mat img_matches;
// std::vector<cv::Point2f> template_points,search_points;
// uint max_matches = 0;
// for (uint i=0; i<template_images_.size(); i++)
// {
cv::Mat temp_img_matches;
std::vector<cv::Point2f> temp_template_points, temp_search_points;
std::vector<cv::DMatch> temp_matches;
// matcher_.getDescriptorMatches(template_images_[1], template_images_[1], template_keypoints_, template_descriptors_, temp_img_matches, temp_template_points, temp_search_points, temp_matches);
std::stringstream d_image;
d_image << "/home/karol/ros_workspace/interactive_object_recognition/template_library/data/first_home_book_template0image.jpg";
cv::Mat database_image=cv::imread( d_image.str(), 1 );
std::stringstream s_image;
s_image << "/home/karol/ros_workspace/interactive_object_recognition/template_library/training_data/first_home_book_template0image.jpg";
cv::Mat search_image=cv::imread( s_image.str(), 1 );
std::vector<cv::KeyPoint> temp_keypoints;
cv::Mat temp_descriptors;
matcher_.getFeatures(database_image, temp_keypoints, temp_descriptors);
matcher_.getDescriptorMatches(database_image, search_image, temp_keypoints, temp_descriptors, temp_img_matches, temp_template_points, temp_search_points, temp_matches);
// matcher_.getMatches(database_image, search_image, temp_img_matches, temp_template_points, temp_search_points);
// template_bin_[i].push_back(temp_template_points.size());
// if (temp_template_points.size() > max_matches)
// {
// template_image_ = template_images_[i];
// max_matches = temp_template_points.size();
// img_matches = temp_img_matches;
// template_points = temp_template_points;
// search_points = temp_search_points;
// }
// }
//recognition part
// printBins();
// std::string object ="NOT_RECOGNIZED";
// checkRecognition(object);
// std::cout<<"RECOGNIZED OBJECT ==== "<<object<<std::endl;
// std::cout<<std::endl;
// std::cout<<std::endl;
//ransacing part
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr template_color_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
// pcl::PointCloud<pcl::PointXYZ>::Ptr template_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
// pcl::PointCloud<pcl::PointXYZ>::Ptr search_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
// for(uint i=0; i < template_points.size(); i++)
// {
// template_color_cloud_ptr->points.push_back(template_library_.getTemplates()[0].cloud_ptr_->at(template_points[i].x, template_points[i].y));
// search_cloud_ptr->points.push_back(current_cloud_ptr_->at(search_points[i].x, search_points[i].y));
// }
// pcl::copyPointCloud(*template_color_cloud_ptr,*template_cloud_ptr);
// Eigen::Matrix4f transformation_result=Eigen::Matrix4f::Identity();
int inliers(0);
// if (search_cloud_ptr->points.size()>0){
// inliers = ransac_transformer_.ransacUmeyama(search_cloud_ptr,template_cloud_ptr,transformation_result);
// ROS_DEBUG_STREAM("transform: \n"<<transformation_result);
// publishTF(transformation_result,"start","estimate");
// }
ros::Duration time_since_last_callback = ros::Time::now() - publish_time_;
publish_time_ = ros::Time::now();
drawOnImage(inliers, static_cast<int>(temp_template_points.size()), (1/time_since_last_callback.toSec()), temp_img_matches);
/*
Testing for flickering matches -BEGIN
*/
// cv::Scalar color(255,0,0);
// int square_edge=32;
// if (first_one_)
// {
// upper_left_.x=template_points[0].x-4;
// upper_left_.y=template_points[0].y-4;
// bottom_right_.x=template_points[0].x;
// bottom_right_.y=template_points[0].y;
// search_upper_left_.x=search_points[0].x-4+template_image_.cols;
// search_upper_left_.y=search_points[0].y-4;
// search_bottom_right_.x=search_points[0].x+template_image_.cols;
// search_bottom_right_.y=search_points[0].y;
// first_one_=false;
// cv::Size size_four(square_edge*2,square_edge*2);
// cv::Mat image_four_temp(size_four,CV_8UC3);
// image_four_=image_four_temp;
// }
// cv::rectangle(img_matches,upper_left_,bottom_right_,color);
// cv::rectangle(img_matches,search_upper_left_,search_bottom_right_,color);
// if(std::find_if(template_points.begin(), template_points.end(),boost::bind(&cvPointEqualTo,bottom_right_,_1)) != template_points.end())
// {
// cv::Mat image=template_image_;
// cv::Mat image_search=search_image;
// cv::imwrite( "template_contains.jpg", image);
// cv::imwrite( "search_contains.jpg", image_search);
// // select a roi
// cv::Mat roi(image, cv::Rect(bottom_right_.x-square_edge/2,bottom_right_.y-square_edge/2,square_edge,square_edge));
// cv::Mat roi_search(image_search, cv::Rect(search_bottom_right_.x-template_image_.cols-square_edge/2,search_bottom_right_.y-square_edge/2,square_edge,square_edge));
// cv::Size size(roi.cols*2,roi.cols);
// cv::Mat image_both(size,CV_8UC3);
// cv::Mat roi_one (image_four_,cv::Rect(0,0,roi.cols,roi.rows));
// cv::Mat roi_two (image_four_,cv::Rect(roi.cols,0,roi_search.cols,roi_search.rows));
// roi.copyTo(roi_one);
// roi_search.copyTo(roi_two);
//// cv::imwrite( "contains.jpg", image_both);
// ROS_DEBUG("contains");
// }
// else
// {
// cv::Scalar color_not_find(0,0,255);
// cv::rectangle(img_matches,upper_left_,bottom_right_,color_not_find,-1);
// cv::rectangle(img_matches,search_upper_left_,search_bottom_right_,color_not_find,-1);
// cv::Mat image=template_image_;
// cv::Mat image_search=search_image;
// cv::imwrite( "template_doesnt_contain.jpg", image);
// cv::imwrite( "search_doesnt_contain.jpg", image_search);
// cv::Mat roi(image, cv::Rect(bottom_right_.x-square_edge/2,bottom_right_.y-square_edge/2,square_edge,square_edge));
// cv::Mat roi_search(image_search, cv::Rect(search_bottom_right_.x-template_image_.cols-square_edge/2,search_bottom_right_.y-square_edge/2,square_edge,square_edge));
// cv::Size size(roi.cols*2,roi.cols);
// cv::Mat image_both(size,CV_8UC3);
// cv::Mat roi_one (image_four_,cv::Rect(0,roi.rows,roi.cols,roi.rows));
// cv::Mat roi_two (image_four_,cv::Rect(roi.cols,roi.rows,roi_search.cols,roi_search.rows));
// roi.copyTo(roi_one);
// roi_search.copyTo(roi_two);
//// cv::imwrite( "doesnt_contain.jpg", image_four_);
// ROS_DEBUG("doesnt contain");
// }
// cv::imwrite( "image_four.jpg", image_four_);
/*
Testing for flickering matches - END
*/
publisher_.publish (convertCVToSensorMsg (temp_img_matches));
}
