|
| 1 | +// -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*- |
| 2 | +/********************************************************************* |
| 3 | +* Software License Agreement (BSD License) |
| 4 | +* |
| 5 | +* Copyright (c) 2016, JSK Lab. |
| 6 | +* All rights reserved. |
| 7 | +* |
| 8 | +* Redistribution and use in source and binary forms, with or without |
| 9 | +* modification, are permitted provided that the following conditions |
| 10 | +* are met: |
| 11 | +* |
| 12 | +* * Redistributions of source code must retain the above copyright |
| 13 | +* notice, this list of conditions and the following disclaimer. |
| 14 | +* * Redistributions in binary form must reproduce the above |
| 15 | +* copyright notice, this list of conditions and the following |
| 16 | +* disclaimer in the documentation and/or other materials provided |
| 17 | +* with the distribution. |
| 18 | +* * Neither the name of the Kei Okada nor the names of its |
| 19 | +* contributors may be used to endorse or promote products derived |
| 20 | +* from this software without specific prior written permission. |
| 21 | +* |
| 22 | +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 23 | +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| 24 | +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
| 25 | +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
| 26 | +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
| 27 | +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
| 28 | +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
| 29 | +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| 30 | +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| 31 | +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
| 32 | +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 33 | +* POSSIBILITY OF SUCH DAMAGE. |
| 34 | +*********************************************************************/ |
| 35 | +#include <ros/ros.h> |
| 36 | +#include "opencv_apps/nodelet.h" |
| 37 | +#include <image_transport/image_transport.h> |
| 38 | +#include <sensor_msgs/image_encodings.h> |
| 39 | +#include <cv_bridge/cv_bridge.h> |
| 40 | + |
| 41 | +#include <opencv2/highgui/highgui.hpp> |
| 42 | +#include <opencv2/imgproc/imgproc.hpp> |
| 43 | + |
| 44 | +#include <dynamic_reconfigure/server.h> |
| 45 | +#include "opencv_apps/CornerHarrisConfig.h" |
| 46 | + |
| 47 | +// https://github.com/opencv/opencv/blob/master/samples/cpp/tutorial_code/TrackingMotion/cornerHarris_Demo.cpp |
| 48 | +/** |
| 49 | + * @function cornerHarris_Demo.cpp |
| 50 | + * @brief Demo code for detecting corners using Harris-Stephens method |
| 51 | + * @author OpenCV team |
| 52 | + */ |
| 53 | + |
| 54 | +namespace corner_harris { |
| 55 | +class CornerHarrisNodelet : public opencv_apps::Nodelet |
| 56 | +{ |
| 57 | + image_transport::Publisher img_pub_; |
| 58 | + image_transport::Subscriber img_sub_; |
| 59 | + image_transport::CameraSubscriber cam_sub_; |
| 60 | + ros::Publisher msg_pub_; |
| 61 | + |
| 62 | + boost::shared_ptr<image_transport::ImageTransport> it_; |
| 63 | + |
| 64 | + typedef corner_harris::CornerHarrisConfig Config; |
| 65 | + typedef dynamic_reconfigure::Server<Config> ReconfigureServer; |
| 66 | + Config config_; |
| 67 | + boost::shared_ptr<ReconfigureServer> reconfigure_server_; |
| 68 | + |
| 69 | + bool debug_view_; |
| 70 | + |
| 71 | + std::string window_name_; |
| 72 | + static bool need_config_update_; |
| 73 | + |
| 74 | + int threshold_; |
| 75 | + |
| 76 | + void reconfigureCallback(Config &new_config, uint32_t level) |
| 77 | + { |
| 78 | + config_ = new_config; |
| 79 | + threshold_ = config_.threshold; |
| 80 | + } |
| 81 | + |
| 82 | + const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame) |
| 83 | + { |
| 84 | + if (frame.empty()) |
| 85 | + return image_frame; |
| 86 | + return frame; |
| 87 | + } |
| 88 | + |
| 89 | + void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) |
| 90 | + { |
| 91 | + do_work(msg, cam_info->header.frame_id); |
| 92 | + } |
| 93 | + |
| 94 | + void imageCallback(const sensor_msgs::ImageConstPtr& msg) |
| 95 | + { |
| 96 | + do_work(msg, msg->header.frame_id); |
| 97 | + } |
| 98 | + |
| 99 | + static void trackbarCallback( int, void* ) |
| 100 | + { |
| 101 | + need_config_update_ = true; |
| 102 | + } |
| 103 | + |
| 104 | + void do_work(const sensor_msgs::ImageConstPtr& image_msg, const std::string input_frame_from_msg) |
| 105 | + { |
| 106 | + // Work on the image. |
| 107 | + try |
| 108 | + { |
| 109 | + // Convert the image into something opencv can handle. |
| 110 | + cv::Mat frame = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8)->image; |
| 111 | + |
| 112 | + // Do the work |
| 113 | + cv::Mat dst, dst_norm, dst_norm_scaled; |
| 114 | + dst = cv::Mat::zeros( frame.size(), CV_32FC1 ); |
| 115 | + |
| 116 | + cv::Mat src_gray; |
| 117 | + |
| 118 | + if ( frame.channels() > 1 ) { |
| 119 | + cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY ); |
| 120 | + } else { |
| 121 | + src_gray = frame; |
| 122 | + cv::cvtColor( src_gray, frame, cv::COLOR_GRAY2BGR ); |
| 123 | + } |
| 124 | + |
| 125 | + /// Detector parameters |
| 126 | + int blockSize = 2; |
| 127 | + int apertureSize = 3; |
| 128 | + double k = 0.04; |
| 129 | + |
| 130 | + /// Detecting corners |
| 131 | + cv::cornerHarris( src_gray, dst, blockSize, apertureSize, k, cv::BORDER_DEFAULT ); |
| 132 | + |
| 133 | + /// Normalizing |
| 134 | + cv::normalize( dst, dst_norm, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat() ); |
| 135 | + cv::convertScaleAbs( dst_norm, dst_norm_scaled ); |
| 136 | + |
| 137 | + /// Drawing a circle around corners |
| 138 | + for( int j = 0; j < dst_norm.rows ; j++ ) { |
| 139 | + for( int i = 0; i < dst_norm.cols; i++ ) { |
| 140 | + if( (int) dst_norm.at<float>(j,i) > threshold_ ) { |
| 141 | + cv::circle( frame, cv::Point(i, j), 5, cv::Scalar(0), 2, 8, 0 ); |
| 142 | + } |
| 143 | + } |
| 144 | + } |
| 145 | + |
| 146 | + /// Create window |
| 147 | + if( debug_view_) { |
| 148 | + cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE ); |
| 149 | + const int max_threshold = 255; |
| 150 | + if (need_config_update_) { |
| 151 | + config_.threshold = threshold_; |
| 152 | + reconfigure_server_->updateConfig(config_); |
| 153 | + need_config_update_ = false; |
| 154 | + } |
| 155 | + cv::createTrackbar( "Threshold:", window_name_, &threshold_, max_threshold, trackbarCallback); |
| 156 | + } |
| 157 | + |
| 158 | + if( debug_view_) { |
| 159 | + cv::imshow( window_name_, frame ); |
| 160 | + int c = cv::waitKey(1); |
| 161 | + } |
| 162 | + |
| 163 | + // Publish the image. |
| 164 | + sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(image_msg->header, sensor_msgs::image_encodings::BGR8, frame).toImageMsg(); |
| 165 | + img_pub_.publish(out_img); |
| 166 | + } |
| 167 | + catch (cv::Exception &e) |
| 168 | + { |
| 169 | + NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); |
| 170 | + } |
| 171 | + } |
| 172 | + |
| 173 | + void subscribe() |
| 174 | + { |
| 175 | + NODELET_DEBUG("Subscribing to image topic."); |
| 176 | + if (config_.use_camera_info) |
| 177 | + cam_sub_ = it_->subscribeCamera("image", 3, &CornerHarrisNodelet::imageCallbackWithInfo, this); |
| 178 | + else |
| 179 | + img_sub_ = it_->subscribe("image", 3, &CornerHarrisNodelet::imageCallback, this); |
| 180 | + } |
| 181 | + |
| 182 | + void unsubscribe() |
| 183 | + { |
| 184 | + NODELET_DEBUG("Unsubscribing from image topic."); |
| 185 | + img_sub_.shutdown(); |
| 186 | + cam_sub_.shutdown(); |
| 187 | + } |
| 188 | + |
| 189 | +public: |
| 190 | + virtual void onInit() |
| 191 | + { |
| 192 | + Nodelet::onInit(); |
| 193 | + it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_)); |
| 194 | + |
| 195 | + pnh_->param("debug_view", debug_view_, false); |
| 196 | + |
| 197 | + if (debug_view_) { |
| 198 | + always_subscribe_ = true; |
| 199 | + } |
| 200 | + |
| 201 | + window_name_ = "CornerHarris Demo"; |
| 202 | + |
| 203 | + reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_); |
| 204 | + dynamic_reconfigure::Server<Config>::CallbackType f = |
| 205 | + boost::bind(&CornerHarrisNodelet::reconfigureCallback, this, _1, _2); |
| 206 | + reconfigure_server_->setCallback(f); |
| 207 | + |
| 208 | + img_pub_ = advertiseImage(*pnh_, "image", 1); |
| 209 | + |
| 210 | + onInitPostProcess(); |
| 211 | + } |
| 212 | +}; |
| 213 | +bool CornerHarrisNodelet::need_config_update_ = false; |
| 214 | +} |
| 215 | + |
| 216 | +#include <pluginlib/class_list_macros.h> |
| 217 | +PLUGINLIB_EXPORT_CLASS(corner_harris::CornerHarrisNodelet, nodelet::Nodelet); |
0 commit comments