feature0d_to_image.h
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 
00037 #ifndef POSEDETECTION_MSGS_FEATURE0D_TO_IMAGE_H_
00038 #define POSEDETECTION_MSGS_FEATURE0D_TO_IMAGE_H_
00039 
00040 #include <ros/node_handle.h>
00041 #include <sensor_msgs/Image.h>
00042 #include <posedetection_msgs/ImageFeature0D.h>
00043 
00044 #include <opencv/cv.hpp>
00045 #include <opencv2/highgui/highgui.hpp>
00046 #include <boost/shared_ptr.hpp>
00047 
00048 #include <message_filters/subscriber.h>
00049 #include <message_filters/time_synchronizer.h>
00050 #include <message_filters/synchronizer.h>
00051 #include <message_filters/sync_policies/exact_time.h>
00052 
00053 namespace posedetection_msgs
00054 {
00055   cv::Mat draw_features(const cv::Mat src,
00056                         const std::vector<float> positions,
00057                         const std::vector<float> scales,
00058                         const std::vector<float> orientations)
00059   {
00060     cv::Mat dst;
00061     src.copyTo(dst);
00062     for(size_t i = 0; i < positions.size()/2; ++i) {
00063       float scale = i < scales.size() ? scales[i] : 10.0;
00064       cv::Point center = cv::Point(positions[2*i+0], positions[2*i+1]);
00065       cv::circle(dst, center, scale, CV_RGB(0,255,0));
00066       if( i < orientations.size() ) {
00067         // draw line indicating orientation
00068         cv::Point end_pt = cv::Point(center.x+std::cos(orientations[i])*scale,
00069             center.y+std::sin(orientations[i])*scale);
00070         cv::line(dst, center, end_pt, CV_RGB(255,0,0));
00071       }
00072     }
00073     return dst;
00074   }
00075 
00076   class Feature0DToImage
00077   {
00078   public:
00079     ros::NodeHandle _node;
00080     ros::Publisher _pub;
00081     ros::Subscriber _sub_imagefeature;
00082     typedef message_filters::sync_policies::ExactTime<
00083     sensor_msgs::Image,
00084     posedetection_msgs::Feature0D
00085     > SyncPolicy;
00086     boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > _sync;
00087     message_filters::Subscriber<sensor_msgs::Image> _sub_image;
00088     message_filters::Subscriber<posedetection_msgs::Feature0D> _sub_feature;
00089 
00090     Feature0DToImage();
00091     virtual ~Feature0DToImage();
00092     void imagefeature_cb(const posedetection_msgs::ImageFeature0DConstPtr& msg_ptr);
00093     void imagefeature_cb(const sensor_msgs::ImageConstPtr& image_msg,
00094                          const posedetection_msgs::Feature0DConstPtr& feature_msg);
00095   };
00096 }
00097 
00098 #endif


posedetection_msgs
Author(s): Rosen Diankov
autogenerated on Sat Jul 15 2017 02:43:05