From 5c3adaf9d5a5e0591bc4683b8d0c89aadbed552e Mon Sep 17 00:00:00 2001 From: Abraham Monrroy Date: Tue, 29 Aug 2017 17:44:45 +0900 Subject: [PATCH] Feature/enet image segmenter (#784) * Initial Release of ENEt Image Segmenter * Node Publishes -segmentation results in /image_segmented -blended result with sensor /image_segmented_blended Added README * Removed unused params * Added More Instructions to README * Added extra instructions * Package renamed to image_segmenter Node renamed to image_segmenter_enet * Modified package README to reflect updated naming * Fixed dependencies * Added curly braces to one line ifs * Added Further compilation instructions * Path in readme file updated --- .../packages/image_segmenter/CMakeLists.txt | 79 ++++++++++ .../packages/image_segmenter/README.md | 33 ++++ .../include/image_segmenter_enet.hpp | 49 ++++++ .../launch/image_segmenter_enet.launch | 18 +++ .../packages/image_segmenter/package.xml | 18 +++ .../src/image_segmenter_enet.cpp | 142 +++++++++++++++++ .../src/image_segmenter_enet_node.cpp | 147 ++++++++++++++++++ 7 files changed, 486 insertions(+) create mode 100644 ros/src/computing/perception/detection/packages/image_segmenter/CMakeLists.txt create mode 100644 ros/src/computing/perception/detection/packages/image_segmenter/README.md create mode 100644 ros/src/computing/perception/detection/packages/image_segmenter/include/image_segmenter_enet.hpp create mode 100644 ros/src/computing/perception/detection/packages/image_segmenter/launch/image_segmenter_enet.launch create mode 100644 ros/src/computing/perception/detection/packages/image_segmenter/package.xml create mode 100644 ros/src/computing/perception/detection/packages/image_segmenter/src/image_segmenter_enet.cpp create mode 100644 ros/src/computing/perception/detection/packages/image_segmenter/src/image_segmenter_enet_node.cpp diff --git a/ros/src/computing/perception/detection/packages/image_segmenter/CMakeLists.txt b/ros/src/computing/perception/detection/packages/image_segmenter/CMakeLists.txt new file mode 100644 index 0000000000..c4e17d07df --- /dev/null +++ b/ros/src/computing/perception/detection/packages/image_segmenter/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 2.8.12) +project(image_segmenter) +execute_process( + COMMAND rosversion -d + OUTPUT_VARIABLE ROS_VERSION + OUTPUT_STRIP_TRAILING_WHITESPACE +) + +include(FindPkgConfig) + +if ("${ROS_VERSION}" MATCHES "(indigo|jade)") +FIND_PACKAGE(catkin REQUIRED COMPONENTS + cv_bridge + image_transport + roscpp + sensor_msgs +) +elseif ("${ROS_VERSION}" MATCHES "(kinetic)") +FIND_PACKAGE(catkin REQUIRED COMPONENTS + cv_bridge + image_transport + roscpp + sensor_msgs +) +endif() + +FIND_PACKAGE(CUDA) +FIND_PACKAGE(OpenCV REQUIRED) + +catkin_package(CATKIN_DEPENDS + cv_bridge + image_transport + roscpp + sensor_msgs +) +########### +## Build ## +########### + +SET(CMAKE_CXX_FLAGS "-std=c++11 -O2 -g -Wall ${CMAKE_CXX_FLAGS}") + +INCLUDE_DIRECTORIES( + ${catkin_INCLUDE_DIRS} +) + + +#####ENET######## +##############################ENet's CAFFE FORK NEEDS TO BE PREVIOUSLY COMPILED#################### +set(ENET_CAFFE_PATH "$ENV{HOME}/ENet/caffe-enet/distribute") +#################################################################################################### +if(EXISTS "${ENET_CAFFE_PATH}") + + ADD_EXECUTABLE(image_segmenter_enet + src/image_segmenter_enet_node.cpp + src/image_segmenter_enet.cpp + ) + + TARGET_LINK_LIBRARIES(image_segmenter_enet + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ${CUDA_LIBRARIES} + ${CUDA_CUBLAS_LIBRARIES} + ${CUDA_curand_LIBRARY} + ${ENET_CAFFE_PATH}/lib/libcaffe.so + glog + ) + + TARGET_INCLUDE_DIRECTORIES(image_segmenter_enet PRIVATE + ${CUDA_INCLUDE_DIRS} + ${ENET_CAFFE_PATH}/include + include + ) + + ADD_DEPENDENCIES(image_segmenter_enet + ${catkin_EXPORTED_TARGETS} + ) +else() + message("'ENet/Caffe' is not installed. 'image_segmenter_enet' will not be built.") +endif() \ No newline at end of file diff --git a/ros/src/computing/perception/detection/packages/image_segmenter/README.md b/ros/src/computing/perception/detection/packages/image_segmenter/README.md new file mode 100644 index 0000000000..3c28103d9d --- /dev/null +++ b/ros/src/computing/perception/detection/packages/image_segmenter/README.md @@ -0,0 +1,33 @@ +# How to install Caffe for ENet + +1. Complete the [pre-requisites](http://caffe.berkeleyvision.org/install_apt.html) + +2. Clone ENet + +``` +cd ~ +git clone --recursive https://github.com/TimoSaemann/ENet.git +cd ENet/caffe-enet +``` + +3. Compile ENet fork of Caffe using **Make** *(Don't use CMake to compile Caffe)* +Create Makefile.config from Makefile.config.example and setup your paths +as indicated in http://caffe.berkeleyvision.org/installation.html#compilation +``` +make && make distribute +``` + +4. Download pretrained models as pointed in +https://github.com/TimoSaemann/ENet/tree/master/Tutorial#kick-start or use your own. + +If you didn't install ENet Caffe in `ENet` inside your home for some reason, +modify the Autoware ENet's node CMakeFile and point the paths to match your system. + +Once compiled, run from the terminal + +``` +% roslaunch image_segmenter image_segmenter_enet.launch +``` +Remember to modify the launch file located inside +`computing/perception/detection/packages/image_segmenter/launch/image_segmenter_enet.launch` +and configure the network configuration file, the pretrained models and the LUT file. diff --git a/ros/src/computing/perception/detection/packages/image_segmenter/include/image_segmenter_enet.hpp b/ros/src/computing/perception/detection/packages/image_segmenter/include/image_segmenter_enet.hpp new file mode 100644 index 0000000000..7143e203de --- /dev/null +++ b/ros/src/computing/perception/detection/packages/image_segmenter/include/image_segmenter_enet.hpp @@ -0,0 +1,49 @@ +/* + * image_segmenter_enet.hpp + * + * Created on: Aug 23, 2017 + * Author: ne0 + */ + +#ifndef ENET_IMAGE_SEGMENTER_HPP_ +#define ENET_IMAGE_SEGMENTER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +class ENetSegmenter +{ +public: + ENetSegmenter(const std::string& in_model_file, const std::string& in_trained_file, const std::string& in_lookuptable_file); + + void Predict(const cv::Mat& in_img, cv::Mat& out_segmented); + +private: + void SetMean(const std::string& in_mean_file); + + void WrapInputLayer(std::vector* in_input_channels); + + void Preprocess(const cv::Mat& img, std::vector* in_input_channels); + + cv::Mat Visualization(cv::Mat in_prediction_map, std::string in_lookuptable_file); + +private: + std::shared_ptr > net_; + cv::Size input_geometry_; + int num_channels_; + std::string lookuptable_file_; + cv::Scalar pixel_mean_; + +}; + +#endif /* ENET_IMAGE_SEGMENTER_HPP_ */ diff --git a/ros/src/computing/perception/detection/packages/image_segmenter/launch/image_segmenter_enet.launch b/ros/src/computing/perception/detection/packages/image_segmenter/launch/image_segmenter_enet.launch new file mode 100644 index 0000000000..c9a661cd04 --- /dev/null +++ b/ros/src/computing/perception/detection/packages/image_segmenter/launch/image_segmenter_enet.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/ros/src/computing/perception/detection/packages/image_segmenter/package.xml b/ros/src/computing/perception/detection/packages/image_segmenter/package.xml new file mode 100644 index 0000000000..30338f1e2b --- /dev/null +++ b/ros/src/computing/perception/detection/packages/image_segmenter/package.xml @@ -0,0 +1,18 @@ + + + image_segmenter + 1.0.0 + Image Segmentation + Abraham Monrroy + BSD + catkin + cv_bridge + image_transport + roscpp + sensor_msgs + cv_bridge + image_transport + roscpp + cv_bridge + sensor_msgs + diff --git a/ros/src/computing/perception/detection/packages/image_segmenter/src/image_segmenter_enet.cpp b/ros/src/computing/perception/detection/packages/image_segmenter/src/image_segmenter_enet.cpp new file mode 100644 index 0000000000..571275e27a --- /dev/null +++ b/ros/src/computing/perception/detection/packages/image_segmenter/src/image_segmenter_enet.cpp @@ -0,0 +1,142 @@ +/* + * image_segmenter_enet.cpp + * + * Created on: Aug 23, 2017 + * Author: amc + */ + +#include "image_segmenter_enet.hpp" + +ENetSegmenter::ENetSegmenter(const std::string& in_model_file, + const std::string& in_trained_file, + const std::string& in_lookuptable_file) +{ + + caffe::Caffe::set_mode(caffe::Caffe::GPU); + + /* Load the network. */ + net_.reset(new caffe::Net(in_model_file, caffe::TEST)); + net_->CopyTrainedLayersFrom(in_trained_file); + + CHECK_EQ(net_->num_inputs(), 1)<< "Network should have exactly one input."; + CHECK_EQ(net_->num_outputs(), 1)<< "Network should have exactly one output."; + + caffe::Blob* input_layer = net_->input_blobs()[0]; + num_channels_ = input_layer->channels(); + CHECK(num_channels_ == 3 || num_channels_ == 1) << "Input layer should have 1 or 3 channels."; + input_geometry_ = cv::Size(input_layer->width(), input_layer->height()); + + lookuptable_file_ = in_lookuptable_file; + + pixel_mean_ = cv::Scalar(102.9801, 115.9465, 122.7717); +} + +void ENetSegmenter::Predict(const cv::Mat& in_image_mat, cv::Mat& out_segmented) +{ + caffe::Blob* input_layer = net_->input_blobs()[0]; + input_layer->Reshape(1, num_channels_, input_geometry_.height, + input_geometry_.width); + /* Forward dimension change to all layers. */ + net_->Reshape(); + + std::vector input_channels; + WrapInputLayer(&input_channels); + + Preprocess(in_image_mat, &input_channels); + + net_->Forward(); + + /* Copy the output layer to a std::vector */ + caffe::Blob* output_layer = net_->output_blobs()[0]; + + int width = output_layer->width(); + int height = output_layer->height(); + int channels = output_layer->channels(); + + // compute argmax + cv::Mat class_each_row(channels, width * height, CV_32FC1, + const_cast(output_layer->cpu_data())); + class_each_row = class_each_row.t(); // transpose to make each row with all probabilities + cv::Point maxId; // point [x,y] values for index of max + double maxValue; // the holy max value itself + cv::Mat prediction_map(height, width, CV_8UC1); + for (int i = 0; i < class_each_row.rows; i++) + { + minMaxLoc(class_each_row.row(i), 0, &maxValue, 0, &maxId); + prediction_map.at(i) = maxId.x; + } + + out_segmented = Visualization(prediction_map, lookuptable_file_); + + cv::resize(out_segmented, out_segmented, cv::Size(in_image_mat.cols, in_image_mat.rows)); + + +} + +cv::Mat ENetSegmenter::Visualization(cv::Mat in_prediction_map, std::string in_lookuptable_file) +{ + + cv::cvtColor(in_prediction_map.clone(), in_prediction_map, CV_GRAY2BGR); + cv::Mat label_colours = cv::imread(in_lookuptable_file, 1); + cv::cvtColor(label_colours, label_colours, CV_RGB2BGR); + cv::Mat output_image; + LUT(in_prediction_map, label_colours, output_image); + + return output_image; +} + +void ENetSegmenter::WrapInputLayer(std::vector* in_input_channels) +{ + caffe::Blob* input_layer = net_->input_blobs()[0]; + + int width = input_layer->width(); + int height = input_layer->height(); + float* input_data = input_layer->mutable_cpu_data(); + for (int i = 0; i < input_layer->channels(); ++i) + { + cv::Mat channel(height, width, CV_32FC1, input_data); + in_input_channels->push_back(channel); + input_data += width * height; + } +} + +void ENetSegmenter::Preprocess(const cv::Mat& in_image_mat, + std::vector* in_input_channels) +{ + /* Convert the input image to the input image format of the network. */ + cv::Mat sample; + if (in_image_mat.channels() == 3 && num_channels_ == 1){ + cv::cvtColor(in_image_mat, sample, cv::COLOR_BGR2GRAY);} + else if (in_image_mat.channels() == 4 && num_channels_ == 1){ + cv::cvtColor(in_image_mat, sample, cv::COLOR_BGRA2GRAY);} + else if (in_image_mat.channels() == 4 && num_channels_ == 3){ + cv::cvtColor(in_image_mat, sample, cv::COLOR_BGRA2BGR);} + else if (in_image_mat.channels() == 1 && num_channels_ == 3){ + cv::cvtColor(in_image_mat, sample, cv::COLOR_GRAY2BGR);} + else{ + sample = in_image_mat; + } + + cv::Mat sample_resized; + if (sample.size() != input_geometry_){ + cv::resize(sample, sample_resized, input_geometry_);} + else{ + sample_resized = sample; + } + + cv::Mat sample_float; + if (num_channels_ == 3) + { + sample_resized.convertTo(sample_float, CV_32FC3); + } + else + { + sample_resized.convertTo(sample_float, CV_32FC1); + } + + cv::split(sample_float, *in_input_channels); + + CHECK(reinterpret_cast(in_input_channels->at(0).data) + == net_->input_blobs()[0]->cpu_data()) + << "Input channels are not wrapping the input layer of the network."; +} diff --git a/ros/src/computing/perception/detection/packages/image_segmenter/src/image_segmenter_enet_node.cpp b/ros/src/computing/perception/detection/packages/image_segmenter/src/image_segmenter_enet_node.cpp new file mode 100644 index 0000000000..f234c2bbec --- /dev/null +++ b/ros/src/computing/perception/detection/packages/image_segmenter/src/image_segmenter_enet_node.cpp @@ -0,0 +1,147 @@ +/* + * image_segmenter_enet_node.cpp + * + * Created on: Aug 23, 2017 + * Author: ne0 + */ + +#include +#include +#include +#include +#include + +#include "image_segmenter_enet.hpp" + +class RosENetSegmenterApp +{ + ros::Subscriber subscriber_image_raw_; + ros::NodeHandle node_handle_; + + ros::Publisher publisher_image_segmented_; + ros::Publisher publisher_image_segmented_blended_; + + ENetSegmenter* enet_segmenter_; + + void image_callback(const sensor_msgs::Image& in_image_sensor) + { + //Receive Image, convert it to OpenCV Mat + cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(in_image_sensor, "bgr8"); + cv::Mat image = cv_image->image; + + cv::Mat segmented_mat; + cv::Mat blended_mat; + + enet_segmenter_->Predict(image, segmented_mat); + + if (!segmented_mat.empty()) + { + addWeighted( image, 0.4, segmented_mat, 0.6, 0.0, blended_mat); + + cv_bridge::CvImage segmented_rosimage; + segmented_rosimage.header = in_image_sensor.header; segmented_rosimage.encoding = "bgr8"; + segmented_rosimage.image = segmented_mat; + + cv_bridge::CvImage blended_rosimage; + blended_rosimage.header = in_image_sensor.header; blended_rosimage.encoding = "bgr8"; + blended_rosimage.image = blended_mat; + + publisher_image_segmented_.publish(segmented_rosimage.toImageMsg()); + publisher_image_segmented_blended_.publish(blended_rosimage.toImageMsg()); + } + } + +public: + void Run() + { + ros::NodeHandle private_node_handle("~");//to receive args + + std::string image_raw_topic_str; + if (private_node_handle.getParam("image_src", image_raw_topic_str)) + { + ROS_INFO("Setting image node to %s", image_raw_topic_str.c_str()); + } + else + { + ROS_INFO("No image node received, defaulting to /image_raw, you can use _image_src:=YOUR_TOPIC"); + image_raw_topic_str = "/image_raw"; + } + + std::string network_definition_file; + std::string pretrained_model_file; + std::string lookuptable_file; + if (private_node_handle.getParam("network_definition_file", network_definition_file)) + { + ROS_INFO("Network Definition File: %s", network_definition_file.c_str()); + } + else + { + ROS_INFO("No Network Definition File was received. Finishing execution."); + return; + } + if (private_node_handle.getParam("pretrained_model_file", pretrained_model_file)) + { + ROS_INFO("Pretrained Model File: %s", pretrained_model_file.c_str()); + } + else + { + ROS_INFO("No Pretrained Model File was received. Finishing execution."); + return; + } + if (private_node_handle.getParam("lookuptable_file", lookuptable_file)) + { + ROS_INFO("lookuptable File: %s", lookuptable_file.c_str()); + } + else + { + ROS_INFO("No lookuptable File was received. Finishing execution."); + return; + } + + enet_segmenter_ = new ENetSegmenter(network_definition_file, + pretrained_model_file, + lookuptable_file); + + if (NULL == enet_segmenter_) + { + ROS_INFO("Error while creating ENetSegmenter Object"); + return; + } + ROS_INFO("ENetSegmenter initialized."); + + ROS_INFO("Subscribing to... %s", image_raw_topic_str.c_str()); + subscriber_image_raw_ = node_handle_.subscribe(image_raw_topic_str, 1, &RosENetSegmenterApp::image_callback, this); + + publisher_image_segmented_ = node_handle_.advertise("/image_segmented", 1); + ROS_INFO("Publishing /image_segmented"); + publisher_image_segmented_blended_ = node_handle_.advertise("/image_segmented_blended", 1); + ROS_INFO("Publishing /image_segmented_blended"); + + ROS_INFO("Waiting for data..."); + ros::spin(); + ROS_INFO("END ENetSegmenter"); + } + + ~RosENetSegmenterApp() + { + if (NULL != enet_segmenter_) + delete enet_segmenter_; + } + + RosENetSegmenterApp() + { + enet_segmenter_ = NULL; + } +}; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "image_segmenter_enet"); + + RosENetSegmenterApp app; + + app.Run(); + + return 0; +} +