Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <sensor_msgs/Image.h>
00003 #include <image_transport/image_transport.h>
00004 #include <cv_bridge/CvBridge.h>
00005 #include "opencv2/highgui/highgui.hpp"
00006 #include <string>
00007 #include "pcl/io/pcd_io.h"
00008 #include "pcl/point_types.h"
00009 #include "sensor_msgs/PointCloud2.h"
00010 #include "sensor_msgs/PointCloud.h"
00011 #include <cpl_superpixels/SmoothClutter.h>
00012 using cpl_superpixels::SmoothClutter;
00013
00014 struct PointXYZIL;
00015
00016 class OfflineClutterTestNode
00017 {
00018 public:
00019 OfflineClutterTestNode(ros::NodeHandle n): n_(n)
00020 {
00021 ros::NodeHandle n_private("~");
00022 std::string def_image_name = "fake.png";
00023 std::string def_mask_name = "fake_mask.png";
00024 std::string def_pcd_name = "fake.pcd";
00025 std::string def_smooth_name = "fake-smooth.png";
00026 std::string def_clutter_name = "fake-clutter.png";
00027
00028 n_private.param("input_image_name", input_image_name_, def_image_name);
00029 n_private.param("input_pcd_name", input_pcd_name_, def_pcd_name);
00030 n_private.param("input_mask_name", input_mask_name_, def_mask_name);
00031 n_private.param("smooth_output_name", smooth_output_name_, def_smooth_name);
00032 n_private.param("clutter_output_name", clutter_output_name_, def_clutter_name);
00033
00034 superpixel_client_ = n_.serviceClient<SmoothClutter>("smooth_clutter");
00035
00036 }
00037
00038 void spin()
00039 {
00040
00041 cv::Mat input_img = cv::imread(input_image_name_);
00042 cv::Mat mask_img = cv::imread(input_mask_name_, 0);
00043
00044 sensor_msgs::PointCloud2 input_cloud;
00045 pcl::PointCloud<pcl::PointXYZ> cloud;
00046
00047 if (pcl::io::loadPCDFile(input_pcd_name_, input_cloud) == -1)
00048 {
00049 ROS_ERROR_STREAM("Failed to read in pcd file: " << input_pcd_name_);
00050 return;
00051 }
00052
00053
00054 pcl::fromROSMsg(input_cloud, cloud);
00055 ROS_INFO_STREAM("Fields: " << pcl::getFieldsList(input_cloud));
00056
00057 cv::Mat label_img = mask_img;
00058
00059
00060 SmoothClutter sc;
00061 IplImage raw_ipl = input_img;
00062 IplImage label_ipl = mask_img;
00063
00064 ROS_INFO("Converting input images.");
00065 sensor_msgs::Image::ConstPtr msg_in = bridge_.cvToImgMsg(&raw_ipl);
00066 sensor_msgs::Image::ConstPtr labels_in = bridge_.cvToImgMsg(&label_ipl);
00067 sc.request.raw_img = *msg_in;
00068 sc.request.clutter_labels = *labels_in;
00069
00070 ROS_INFO("Calling smoother client.");
00071 if( !superpixel_client_.call(sc) )
00072 {
00073 ROS_ERROR_STREAM("Failure of call to superpixel smoother service!");
00074 }
00075
00076 sensor_msgs::ImageConstPtr msg_out =
00077 boost::shared_ptr<sensor_msgs::Image const>(
00078 new sensor_msgs::Image(sc.response.smooth_clutter_labels));
00079 cv::Mat smooth_labels = bridge_.imgMsgToCv(msg_out);
00080 ROS_INFO("Converted result.");
00081
00082
00083
00084 cv::imwrite(smooth_output_name_, smooth_labels);
00085 cv::imwrite(clutter_output_name_, label_img);
00086
00087
00088 ROS_INFO("Displaying results.");
00089 cv::imshow("GT Labels", mask_img);
00090 cv::imshow("Smooth Labels", smooth_labels);
00091 cv::imshow("Raw image", input_img);
00092 cv::waitKey();
00093 };
00094
00095 protected:
00096 ros::NodeHandle n_;
00097 ros::ServiceClient superpixel_client_;
00098 std::string input_pcd_name_;
00099 std::string input_image_name_;
00100 std::string input_mask_name_;
00101 std::string smooth_output_name_;
00102 std::string clutter_output_name_;
00103 sensor_msgs::CvBridge bridge_;
00104 };
00105
00106 int main(int argc, char** argv)
00107 {
00108 ros::init(argc, argv, "");
00109 ros::NodeHandle n;
00110 OfflineClutterTestNode clutter_test(n);
00111 clutter_test.spin();
00112 }