find_keypoints.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Modified BSD License)
00003  *
00004  *  Copyright (c) 2016, PAL Robotics, S.L.
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 PAL Robotics, S.L. 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 
00037 // ROS HEADERS
00038 #include <ros/ros.h>
00039 #include <cv_bridge/cv_bridge.h>
00040 #include <sensor_msgs/image_encodings.h>
00041 #include <sensor_msgs/Image.h>
00042 #include <image_transport/image_transport.h>
00043 #include <vector>
00044 #include <opencv_tut/valueMatrix.h>
00045 
00046 // OpenCV headers
00047 #include <opencv2/core/core.hpp>
00048 #include <opencv2/features2d/features2d.hpp>
00049 #include <opencv2/nonfree/features2d.hpp>
00050 #include <opencv2/nonfree/nonfree.hpp>
00051 #include <opencv2/highgui/highgui.hpp>
00052  
00053 class FindKeypoints
00054 {
00055 public:
00056         FindKeypoints(ros::NodeHandle nh_);
00057         ~FindKeypoints();
00058 protected:
00059         void imageCB(const sensor_msgs::ImageConstPtr& msg);
00060         void matrixCB(const opencv_tut::valueMatrixConstPtr& msg);
00061         void FeaturesDetection(cv::Mat in, cv::Mat& out);
00062         void contrastChange(cv::Mat in, cv::Mat& out);
00063         void sharpenImg(cv::Mat in, cv::Mat& out);
00064         void cvWindows();
00065 
00066         image_transport::ImageTransport _imageTransport;
00067         image_transport::Subscriber image_sub;
00068         ros::Subscriber gui_sub;
00069 
00070         float alpha;
00071         int beta;
00072         int zero_zero, zero_one, zero_two, one_zero, one_one, one_two, two_zero, two_one, two_two;
00073         bool keypoints_bool, sharpen_bool, contrast_bool, original_bool, combined_bool;
00074         std::string detector_from_msg;
00075 };
00076 
00077 const std::string key_win= "Keypoints";
00078 const std::string sharp_win= "Sharpen Image";
00079 const std::string cont_win = "Contrast Manipulation";
00080 const std::string orig_win = "Original Image";
00081 const std::string combi_win= "Combined Effects";
00082 
00083 FindKeypoints::FindKeypoints(ros::NodeHandle nh_): _imageTransport(nh_)
00084 {
00085         image_sub = _imageTransport.subscribe("xtion/rgb/image_raw", 1, &FindKeypoints::imageCB, this, image_transport::TransportHints("compressed"));
00086         gui_sub = nh_.subscribe("/opencv_tut/find_keypoints_gui", 1, &FindKeypoints::matrixCB, this);
00087 
00088         alpha = 2.2;
00089         beta = 50;
00090         zero_zero = 0; zero_one = -1; zero_two = 0;
00091         one_zero = -1; one_one = 5; one_two = -1;
00092         two_zero = 0; two_one = -1; two_two = 0;
00093         keypoints_bool= true;
00094         sharpen_bool = contrast_bool = original_bool = combined_bool = false;
00095         detector_from_msg = "SURF";
00096 }
00097 
00098 FindKeypoints::~FindKeypoints()
00099 {
00100         cv::destroyAllWindows();
00101 }
00102 
00103 void FindKeypoints::matrixCB(const opencv_tut::valueMatrixConstPtr& msg)
00104 {
00105         if(msg->header.frame_id == "zero_zero")
00106                 zero_zero = msg->value;
00107         else if(msg->header.frame_id == "zero_one")
00108                 zero_one = msg->value;
00109         else if(msg->header.frame_id == "zero_two")
00110                 zero_two = msg->value;
00111         else if(msg->header.frame_id == "one_zero")
00112                 one_zero = msg->value;
00113         else if(msg->header.frame_id == "one_one")
00114                 one_one = msg->value;
00115         else if(msg->header.frame_id == "one_two")
00116                 one_two = msg->value;
00117         else if(msg->header.frame_id == "two_zero")
00118                 two_zero = msg->value;
00119         else if(msg->header.frame_id == "two_one")
00120                 two_one = msg->value;
00121         else if(msg->header.frame_id == "two_two")
00122                 two_two = msg->value;
00123         else if(msg->header.frame_id == "alpha")
00124                 alpha = msg->value;
00125         else if(msg->header.frame_id == "beta")
00126                 beta = msg->value;
00127         else if(msg->header.frame_id == "Keypoints")
00128                 keypoints_bool = msg->tick;
00129         else if(msg->header.frame_id == "Sharpen")
00130                 sharpen_bool = msg->tick;
00131         else if(msg->header.frame_id == "Contrast")
00132                 contrast_bool = msg->tick;
00133         else if(msg->header.frame_id == "Original")
00134                 original_bool = msg->tick;
00135         else if(msg->header.frame_id == "Combined")
00136                 combined_bool = msg->tick;
00137         else
00138                 detector_from_msg = msg->header.frame_id;
00139 
00140         this->cvWindows();
00141 }
00142 
00143 void FindKeypoints::cvWindows()
00144 {
00145         if(keypoints_bool == true)
00146                 cv::namedWindow(key_win, CV_WINDOW_FREERATIO);
00147         else if (keypoints_bool == false)
00148                 cv::destroyWindow(key_win);
00149 
00150         if(sharpen_bool == true)
00151                 cv::namedWindow(sharp_win, CV_WINDOW_FREERATIO);
00152         else if (sharpen_bool == false)
00153                 cv::destroyWindow(sharp_win);
00154 
00155         if(contrast_bool == true)
00156                 cv::namedWindow(cont_win, CV_WINDOW_FREERATIO);
00157         else if (contrast_bool == false)
00158                 cv::destroyWindow(cont_win);
00159 
00160         if(original_bool == true)
00161                 cv::namedWindow(orig_win, CV_WINDOW_FREERATIO);
00162         else if (original_bool == false)
00163                 cv::destroyWindow(orig_win);
00164 
00165         if(combined_bool == true)
00166                 cv::namedWindow(combi_win, CV_WINDOW_FREERATIO);
00167         else if (combined_bool == false)
00168                 cv::destroyWindow(combi_win);
00169 }
00170 
00171 void FindKeypoints::imageCB(const sensor_msgs::ImageConstPtr& msg)
00172 {
00173         cv::Mat img, keypoints_mat, sharp_mat, contrast_mat, combi_mat;
00174         cv_bridge::CvImagePtr cvPtr;
00175 
00176         try
00177         { 
00178                 cvPtr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
00179         }
00180         catch (cv_bridge::Exception& e) 
00181   {
00182                 ROS_ERROR("cv_bridge exception: %s", e.what());
00183                 return;
00184   }
00185 
00186         cvPtr->image.copyTo(img);
00187 
00188         if(original_bool == true)
00189         {
00190                 cv::imshow(orig_win, img);
00191         }
00192         if(contrast_bool == true)
00193         {
00194                 this->contrastChange(img, contrast_mat);
00195                 cv::imshow(cont_win, contrast_mat);
00196         }
00197         if(sharpen_bool == true)
00198         {
00199                 this->sharpenImg(img, sharp_mat);
00200                 cv::imshow(sharp_win, sharp_mat);
00201         }
00202         if(keypoints_bool == true)
00203         {
00204                 this->FeaturesDetection(img, keypoints_mat);
00205                 cv::imshow(key_win, keypoints_mat);
00206         }
00207         if(combined_bool == true)
00208         {
00209                 cv::Mat combi_mat_1, combi_mat_2;
00210                 this->contrastChange(img, combi_mat_1);
00211                 this->sharpenImg(combi_mat_1, combi_mat_2);
00212                 this->FeaturesDetection(combi_mat_2, combi_mat);
00213                 cv::imshow(combi_win, combi_mat);
00214         }
00215         cv::waitKey(2);
00216 }
00217 
00218 void FindKeypoints::sharpenImg(cv::Mat in, cv::Mat& out)
00219 {
00220         out = cv::Mat::zeros(in.size(), in.type());
00221         cv::Mat kern = (cv::Mat_<char>(3,3) << zero_zero, zero_one, zero_two,
00222                                                                                                                                                                         one_zero, one_one, one_two,
00223                                                                                                                                                                         zero_two, one_one, one_two);
00224         cv::filter2D(in, out, in.depth(), kern);
00225 }
00226 
00227 void FindKeypoints::contrastChange(cv::Mat in, cv::Mat& out)
00228 {
00229         out = cv::Mat::zeros(in.size(), in.type());
00230         for(int i = 0; i<in.rows; i++)
00231                 for(int j = 0; j<in.cols; j++)
00232                         for(int k = 0; k<3; ++k)
00233                                 out.at<cv::Vec3b>(i,j).val[k] = cv::saturate_cast<uchar>(alpha * (in.at<cv::Vec3b>(i,j).val[k] + beta));
00234 }
00235 
00236 void FindKeypoints::FeaturesDetection(cv::Mat in, cv::Mat& out)
00237 {       
00238         std::vector<cv::KeyPoint> keypoints;
00239         cv::initModule_nonfree();
00240         cv::Ptr<cv::FeatureDetector> detector = cv::FeatureDetector::create(detector_from_msg);
00241         detector->detect(in, keypoints);
00242         cv::drawKeypoints(in, keypoints, out, cv::Scalar::all(-1), cv::DrawMatchesFlags::DEFAULT);
00243 }
00244 
00245 int main(int argc, char** argv)
00246 {
00247   ros::init(argc, argv, "FindKeypoints");
00248 
00249   ros::NodeHandle nh;
00250 
00251   FindKeypoints fk(nh);
00252 
00253   ros::spin();
00254 }


opencv_tut
Author(s): Job van Dieten
autogenerated on Fri Aug 26 2016 13:20:16