image_annotator.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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   // Params
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     // ***** Resize the image based on scaling parameters in config *****
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 
00094     if (features->success)
00095     {
00096       cv::Point2i pt0(features->image_points[0].x*scaling_, 
00097             features->image_points[0].y*scaling_);
00098       cv::circle(cv_image_scaled, pt0, marker_size_*2, cv::Scalar(0,0,255), 1) ;
00099       for (unsigned int i=0; i<features->image_points.size(); i++)
00100       {
00101         cv::Point2i pt(features->image_points[i].x*scaling_, 
00102               features->image_points[i].y*scaling_);
00103         cv::circle(cv_image_scaled, pt, marker_size_, cv::Scalar(0,255,0), 1) ;
00104       }
00105     }
00106     else
00107     {
00108       for (unsigned int i=0; i<features->image_points.size(); i++)
00109       {
00110         cv::Point2i pt(features->image_points[i].x*scaling_,
00111               features->image_points[i].y*scaling_);
00112         cv::circle(cv_image_scaled, pt, marker_size_, cv::Scalar(255,0,0), 1) ;
00113       }
00114     }
00115 
00116     // Send the annotated image over ROS
00117     sensor_msgs::Image result_image = *(cv_bridge::CvImage(cv_image->header, cv_image->encoding, cv_image_scaled).toImageMsg());
00118     image_pub_.publish(result_image);
00119   } catch(cv_bridge::Exception & e) {
00120     ROS_ERROR("cv_bridge exception: %s", e.what());
00121   }
00122 }
00123 
00124 int main(int argc, char** argv)
00125 {
00126   ros::init(argc, argv, "image_annotator");
00127 
00128   ros::NodeHandle nh;
00129 
00130   ImageAnnotator annotator;
00131 
00132   message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "image", 1);
00133   message_filters::Subscriber<calibration_msgs::CalibrationPattern> features_sub(nh, "features", 1);
00134   message_filters::TimeSynchronizer<sensor_msgs::Image,
00135                                     calibration_msgs::CalibrationPattern> sync(image_sub, features_sub, 5);
00136 
00137   sync.registerCallback(boost::bind(&ImageAnnotator::processPair, &annotator, _1, _2));
00138 
00139   ros::spin();
00140   return 0;
00141 }


image_cb_detector
Author(s): Vijay Pradeep, Eitan Marder-Eppstein
autogenerated on Tue Sep 27 2016 04:06:37