00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00036
00037 #include <ros/ros.h>
00038 #include <boost/thread/mutex.hpp>
00039
00040 #include <calibration_msgs/CalibrationPattern.h>
00041 #include <sensor_msgs/Image.h>
00042 #include <cv_bridge/cv_bridge.h>
00043 #include <opencv2/imgproc/imgproc.hpp>
00044 #include <message_filters/time_synchronizer.h>
00045 #include <message_filters/subscriber.h>
00046
00047 namespace image_cb_detector
00048 {
00049
00050 class ImageAnnotator
00051 {
00052 public:
00053 ImageAnnotator();
00054
00055 void processPair(const sensor_msgs::ImageConstPtr& image, const calibration_msgs::CalibrationPatternConstPtr& features);
00056 private:
00057 ros::Publisher image_pub_;
00058 ros::NodeHandle n_;
00059
00060
00061 int marker_size_;
00062 double scaling_;
00063 };
00064
00065 }
00066
00067 using namespace image_cb_detector;
00068
00069 ImageAnnotator::ImageAnnotator()
00070 {
00071 image_pub_ = n_.advertise<sensor_msgs::Image>("annotated", 1);
00072
00073 ros::NodeHandle local_ns_("~");
00074
00075 local_ns_.param("marker_size", marker_size_, 1);
00076 local_ns_.param("scaling", scaling_, 1.0);
00077 ROS_INFO("[marker_size]: %i", marker_size_);
00078 ROS_INFO("[scaling]: %.3f", scaling_);
00079 }
00080
00081 void ImageAnnotator::processPair(const sensor_msgs::ImageConstPtr& image, const calibration_msgs::CalibrationPatternConstPtr& features)
00082 {
00083 try {
00084 cv_bridge::CvImageConstPtr cv_image = cv_bridge::toCvShare(image, "rgb8");
00085
00086
00087 const int scaled_width = (int) (.5 + cv_image->image.cols * scaling_);
00088 const int scaled_height = (int) (.5 + cv_image->image.rows * scaling_);
00089 cv::Mat cv_image_scaled;
00090 cv::resize(cv_image->image, cv_image_scaled,
00091 cv::Size(scaled_width, scaled_height), 0, 0, CV_INTER_LINEAR);
00092
00093 if (features->success)
00094 {
00095 cv::Point2i pt0(features->image_points[0].x*scaling_,
00096 features->image_points[0].y*scaling_);
00097 cv::circle(cv_image_scaled, pt0, marker_size_*2, cvScalar(0,0,255), 1) ;
00098 for (unsigned int i=0; i<features->image_points.size(); i++)
00099 {
00100 cv::Point2i pt(features->image_points[i].x*scaling_,
00101 features->image_points[i].y*scaling_);
00102 cv::circle(cv_image_scaled, pt, marker_size_, cvScalar(0,255,0), 1) ;
00103 }
00104 }
00105 else
00106 {
00107 for (unsigned int i=0; i<features->image_points.size(); i++)
00108 {
00109 cv::Point2i pt(features->image_points[i].x*scaling_,
00110 features->image_points[i].y*scaling_);
00111 cv::circle(cv_image_scaled, pt, marker_size_, cvScalar(255,0,0), 1) ;
00112 }
00113 }
00114
00115
00116 sensor_msgs::Image result_image = *(cv_bridge::CvImage(cv_image->header, cv_image->encoding, cv_image_scaled).toImageMsg());
00117 image_pub_.publish(result_image);
00118 } catch(cv_bridge::Exception & e) {
00119 ROS_ERROR("cv_bridge exception: %s", e.what());
00120 }
00121 }
00122
00123 int main(int argc, char** argv)
00124 {
00125 ros::init(argc, argv, "image_annotator");
00126
00127 ros::NodeHandle nh;
00128
00129 ImageAnnotator annotator;
00130
00131 message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "image", 1);
00132 message_filters::Subscriber<calibration_msgs::CalibrationPattern> features_sub(nh, "features", 1);
00133 message_filters::TimeSynchronizer<sensor_msgs::Image,
00134 calibration_msgs::CalibrationPattern> sync(image_sub, features_sub, 5);
00135
00136 sync.registerCallback(boost::bind(&ImageAnnotator::processPair, &annotator, _1, _2));
00137
00138 ros::spin();
00139 return 0;
00140 }