image_annotator.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 
37 #include <ros/ros.h>
38 #include <boost/thread/mutex.hpp>
39 
40 #include <calibration_msgs/CalibrationPattern.h>
41 #include <sensor_msgs/Image.h>
42 #include <cv_bridge/cv_bridge.h>
43 #include <opencv2/imgproc/imgproc.hpp>
46 
47 namespace image_cb_detector
48 {
49 
51 {
52 public:
54 
55  void processPair(const sensor_msgs::ImageConstPtr& image, const calibration_msgs::CalibrationPatternConstPtr& features);
56 private:
59 
60  // Params
62  double scaling_;
63 };
64 
65 }
66 
67 using namespace image_cb_detector;
68 
70 {
71  image_pub_ = n_.advertise<sensor_msgs::Image>("annotated", 1);
72 
73  ros::NodeHandle local_ns_("~");
74 
75  local_ns_.param("marker_size", marker_size_, 1);
76  local_ns_.param("scaling", scaling_, 1.0);
77  ROS_INFO("[marker_size]: %i", marker_size_);
78  ROS_INFO("[scaling]: %.3f", scaling_);
79 }
80 
81 void ImageAnnotator::processPair(const sensor_msgs::ImageConstPtr& image, const calibration_msgs::CalibrationPatternConstPtr& features)
82 {
83  try {
84  cv_bridge::CvImageConstPtr cv_image = cv_bridge::toCvShare(image, "rgb8");
85 
86  // ***** Resize the image based on scaling parameters in config *****
87  const int scaled_width = (int) (.5 + cv_image->image.cols * scaling_);
88  const int scaled_height = (int) (.5 + cv_image->image.rows * scaling_);
89  cv::Mat cv_image_scaled;
90  cv::resize(cv_image->image, cv_image_scaled,
91  cv::Size(scaled_width, scaled_height), 0, 0, cv::INTER_LINEAR);
92 
93 
94  if (features->success)
95  {
96  cv::Point2i pt0(features->image_points[0].x*scaling_,
97  features->image_points[0].y*scaling_);
98  cv::circle(cv_image_scaled, pt0, marker_size_*2, cv::Scalar(0,0,255), 1) ;
99  for (unsigned int i=0; i<features->image_points.size(); i++)
100  {
101  cv::Point2i pt(features->image_points[i].x*scaling_,
102  features->image_points[i].y*scaling_);
103  cv::circle(cv_image_scaled, pt, marker_size_, cv::Scalar(0,255,0), 1) ;
104  }
105  }
106  else
107  {
108  for (unsigned int i=0; i<features->image_points.size(); i++)
109  {
110  cv::Point2i pt(features->image_points[i].x*scaling_,
111  features->image_points[i].y*scaling_);
112  cv::circle(cv_image_scaled, pt, marker_size_, cv::Scalar(255,0,0), 1) ;
113  }
114  }
115 
116  // Send the annotated image over ROS
117  sensor_msgs::Image result_image = *(cv_bridge::CvImage(cv_image->header, cv_image->encoding, cv_image_scaled).toImageMsg());
118  image_pub_.publish(result_image);
119  } catch(cv_bridge::Exception & e) {
120  ROS_ERROR("cv_bridge exception: %s", e.what());
121  }
122 }
123 
124 int main(int argc, char** argv)
125 {
126  ros::init(argc, argv, "image_annotator");
127 
128  ros::NodeHandle nh;
129 
130  ImageAnnotator annotator;
131 
132  message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "image", 1);
134  message_filters::TimeSynchronizer<sensor_msgs::Image,
135  calibration_msgs::CalibrationPattern> sync(image_sub, features_sub, 5);
136 
137  sync.registerCallback(boost::bind(&ImageAnnotator::processPair, &annotator, _1, _2));
138 
139  ros::spin();
140  return 0;
141 }
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void processPair(const sensor_msgs::ImageConstPtr &image, const calibration_msgs::CalibrationPatternConstPtr &features)
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL void spin(Spinner &spinner)
#define ROS_INFO(...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int main(int argc, char **argv)
#define ROS_ERROR(...)
sensor_msgs::ImagePtr toImageMsg() const


image_cb_detector
Author(s): Vijay Pradeep, Eitan Marder-Eppstein
autogenerated on Thu Jun 6 2019 19:17:19