depth_saliency_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Georgia Institute of Technology
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 Georgia Institute of Technology nor the names of
00018  *     its 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 // ROS
00035 #include <ros/ros.h>
00036 #include <sensor_msgs/Image.h>
00037 #include <math.h>
00038 
00039 #include <message_filters/subscriber.h>
00040 #include <message_filters/synchronizer.h>
00041 #include <message_filters/sync_policies/approximate_time.h>
00042 #include <cv_bridge/cv_bridge.h>
00043 #include <sensor_msgs/image_encodings.h>
00044 
00045 // OpenCV
00046 #include <opencv2/core/core.hpp>
00047 #include <opencv2/imgproc/imgproc.hpp>
00048 #include <opencv2/highgui/highgui.hpp>
00049 
00050 // Visual features
00051 #include <cpl_visual_features/saliency/center_surround.h>
00052 // #include <cpl_visual_features/sliding_window.h>
00053 
00054 
00055 // STL
00056 #include <vector>
00057 #include <string>
00058 
00059 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,sensor_msgs::Image> MySyncPolicy;
00060 
00061 class DepthSaliencyNode
00062 {
00063  public:
00064   DepthSaliencyNode(ros::NodeHandle &n) :
00065       n_(n),
00066       image_sub_(n,"/kinect_head/rgb/image_color", 1),
00067       depth_sub_(n,"/kinect_head/depth/image", 1),
00068       sync_(MySyncPolicy(1), image_sub_, depth_sub_),
00069       csm(2,3,3,4), img_count_(0)
00070   {
00071     // Combine the subscribed topics with a message filter
00072     sync_.registerCallback(&DepthSaliencyNode::callback, this);
00073     ROS_INFO_STREAM("Synced callbacks.");
00074   }
00075 
00076   void callback(const sensor_msgs::ImageConstPtr& img_msg,
00077                 const sensor_msgs::ImageConstPtr& depth_msg)
00078   {
00079     // Convert images to OpenCV format
00080     cv::Mat input_frame;
00081     cv::Mat depth_frame;
00082     cv_bridge::CvImagePtr input_cv_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::BGR8);
00083     cv_bridge::CvImagePtr depth_cv_ptr = cv_bridge::toCvCopy(depth_msg);
00084     input_frame = input_cv_ptr->image;
00085     depth_frame = depth_cv_ptr->image;
00086     ROS_INFO_STREAM("input_frame.type()" << input_frame.type());
00087     cv::imshow("input_frame", input_frame);
00088     cv::imshow("depth_frame", depth_frame);
00089     cv::cvtColor(input_frame, input_frame, CV_RGB2BGR);
00090 
00091     // Save frames to disk
00092     cv::Mat depth_to_save(input_frame.size(), CV_16UC1);
00093     std::vector<int> params;
00094     params.push_back(CV_IMWRITE_PNG_COMPRESSION);
00095     params.push_back(0);
00096     for (int r = 0; r < depth_frame.rows; ++r)
00097     {
00098       for (int c = 0; c < depth_frame.cols; ++c)
00099       {
00100         if (isnan(depth_frame.at<float>(r,c)))
00101           depth_frame.at<float>(r,c) = 0.0;
00102       }
00103     }
00104     ROS_INFO_STREAM("Removed NaNs");
00105 
00106 #ifdef SAVE_IMAGES
00107     std::stringstream intensity_name;
00108     intensity_name << "/home/thermans/Desktop/intensity" << img_count_
00109                    << ".png";
00110     std::stringstream depth_name;
00111     depth_name << "/home/thermans/Desktop/depth" << img_count_++ << ".png";
00112     depth_frame.convertTo(depth_to_save, depth_to_save.type(), 512.0);
00113     cv::imwrite(intensity_name.str(), input_frame, params);
00114     cv::imwrite(depth_name.str(), depth_to_save, params);
00115     ROS_INFO_STREAM("Saved images");
00116 #endif // SAVE_IMAGES
00117 
00118     ROS_INFO_STREAM("Scaling image");
00119     cv::Mat scaled_depth = depth_frame.clone();
00120     cv::Mat scaled_depth_frame = csm.scaleMap(depth_frame);
00121     cv::imshow("scaled_frame", scaled_depth_frame);
00122     scaled_depth_frame.convertTo(scaled_depth, CV_32FC1);
00123     ROS_INFO_STREAM("Running saliency");
00124     cv::Mat saliency_map = csm(input_frame, scaled_depth);
00125     // cv::Mat saliency_map = csm(input_frame, depth_frame);
00126     double min_val = 0;
00127     double max_val = 0;
00128     cv::minMaxLoc(saliency_map, &min_val, &max_val);
00129     ROS_INFO_STREAM("Minimum saliency unscaled: " << min_val << "\tmax: "
00130                     << max_val);
00131     cv::imshow("unsaceled saliency", saliency_map);
00132     cv::waitKey();
00133   }
00134 
00135   cv::Mat upSampleResponse(cv::Mat& m_s, int s, cv::Size size0)
00136   {
00137     // Upsample from scale s to scale 0
00138     cv::Mat m_s_prime;
00139     cv::Mat temp;
00140     m_s.copyTo(m_s_prime);
00141     m_s.copyTo(temp);
00142 
00143     // Calculate the correct sizes to up sample to
00144     std::vector<cv::Size> sizes;
00145     cv::Size current_size = size0;
00146     for (int i = 0; i < s; i++)
00147     {
00148       sizes.push_back(current_size);
00149       current_size.width /= 2;
00150       current_size.height /= 2;
00151     }
00152 
00153     for (int i = 0; i < s; i++)
00154     {
00155       cv::Size up_size;
00156       up_size = sizes.back();
00157       sizes.pop_back();
00158       cv::pyrUp(temp, m_s_prime, up_size);
00159       temp = m_s_prime;
00160     }
00161 
00162     return m_s_prime;
00163   }
00164 
00168   void spin()
00169   {
00170     ROS_INFO_STREAM("Spinning!");
00171     while(n_.ok())
00172     {
00173       ros::spinOnce();
00174     }
00175   }
00176 
00177  protected:
00178   ros::NodeHandle n_;
00179   message_filters::Subscriber<sensor_msgs::Image> image_sub_;
00180   message_filters::Subscriber<sensor_msgs::Image> depth_sub_;
00181   message_filters::Synchronizer<MySyncPolicy> sync_;
00182   cpl_visual_features::CenterSurroundMapper csm;
00183   // sensor_msgs::CvBridge bridge_;
00184   int img_count_;
00185 };
00186 
00187 int main(int argc, char ** argv)
00188 {
00189   ros::init(argc, argv, "depth_saliency_node");
00190   ros::NodeHandle n;
00191   DepthSaliencyNode saliency_node(n);
00192   saliency_node.spin();
00193 }


cpl_visual_features
Author(s): Tucker Hermans
autogenerated on Wed Nov 27 2013 11:52:35