Add fisheye rectification

image_proc_fisheye/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(image_proc_fisheye)
+set (CMAKE_CXX_STANDARD 11)
+
+find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
+  image_transport
+  image_geometry
+  OpenCV
+  sensor_msgs
+  std_msgs
+  roscpp
+  nodelet  
+)
+
+catkin_package(
+  CATKIN_DEPENDS image_geometry roscpp sensor_msgs
+  DEPENDS OpenCV
+  LIBRARIES ${PROJECT_NAME}
+)
+
+include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
+include_directories(include)
+
+add_library(${PROJECT_NAME}     src/nodelets/rectify_.cpp )
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+
+add_executable(image_proc_fisheye_exe src/image_proc_fisheye.cpp)
+
+target_link_libraries(image_proc_fisheye_exe
+   ${PROJECT_NAME}
+   ${catkin_LIBRARIES}
+)

image_proc_fisheye/nodelet_plugins_.xml

@@ -0,0 +1,14 @@
+<library path="libimage_proc_fisheye">
+
+
+   <class name="image_proc_fisheye/RectifyNodelet"
+	  type="image_proc_fisheye::RectifyNodelet"
+	 base_class_type="nodelet::Nodelet">
+    <description>
+      Nodelet to rectify an unrectified camera image stream using gpu
+    </description>
+  </class>
+
+
+
+</library>

image_proc_fisheye/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<package>
+  <name>image_proc_fisheye</name>
+  <version>0.0.1</version>
+  <description>Uses OpenCV gpu functions for camera calibration</description>
+
+  <maintainer email="[email protected]">Jack</maintainer>
+  <license>BSD</license>
+
+  <url type="website">https://github.com/Jack000</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>OpenCV</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>image_geometry</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>boost</build_depend>
+
+  <run_depend>cv_bridge</run_depend>
+  <run_depend>image_transport</run_depend>
+  <run_depend>OpenCV</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>image_geometry</run_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>roscpp</run_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelet_plugins_.xml" />
+  </export>
+</package>

image_proc_fisheye/readme.md

@@ -0,0 +1,35 @@
+#image_proc_fisheye
+
+A ROS nodelet for image rectification using OpenCV's gpu functions. This nodelet is useful to reduce CPU usage in rectifying large video streams.
+Needs OpenCV 3.4 with CUDA support
+
+Install:
+
+```
+cd ~/catkin_ws/src
+git clone https://github.com/DavidTorresOcana/image_pipeline
+catkin_make
+```
+
+same topics as image_proc:
+
+```
+Subscribed topics:
+  ~camera_info
+  ~image_raw
+
+Published topics:
+  ~image_rect
+```
+
+
+example launch file:
+```
+  <!-- nodelet manager from image stream -->
+  <node pkg="nodelet" type="nodelet" name="camera_nodelet_manager"  args="manager" />
+  <node pkg="nodelet" type="nodelet" name="image_proc_fisheye" args="load image_proc_fisheye/RectifyNodelet camera_nodelet_manager" output="screen">
+    <remap from="camera_info" to="/camera/color/camera_info" />
+    <remap from="image_raw" to="/camera/color/image_raw" />
+    <remap from="image_rect" to="/camera/color/image_rect" />
+  </node>
+```

image_proc_fisheye/src/image_proc_fisheye.cpp

@@ -0,0 +1,17 @@
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv){
+  ros::init(argc, argv, "image_proc_fisheye");
+  nodelet::Loader nodelet;
+  nodelet::M_string remap(ros::names::getRemappings());
+  nodelet::V_string nargv;
+  std::string nodelet_name = ros::this_node::getName();
+
+  std::string nodelet_name2 = ros::this_node::getName();
+  nodelet_name2.erase(nodelet_name2.begin());
+
+  nodelet.load(nodelet_name, nodelet_name2+"/RectifyNodelet", remap, nargv);
+  ros::spin();
+  return 0;
+}

image_proc_fisheye/src/nodelets/rectify.h

@@ -0,0 +1,29 @@
+#include <nodelet/nodelet.h>
+#include "ros/ros.h"
+#include <image_transport/image_transport.h>
+
+
+#include <opencv2/core/core.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+#include <image_geometry/pinhole_camera_model.h>
+
+namespace image_proc_fisheye
+{
+    class RectifyNodelet : public nodelet::Nodelet
+    {
+        public:
+            virtual void onInit();
+            void process_image(const sensor_msgs::ImageConstPtr& frame);
+            void camera_info(const sensor_msgs::CameraInfoConstPtr& info_msg);
+        private:
+            ros::Subscriber sub_;
+            ros::Subscriber sub_info_;
+            ros::Publisher pub_;
+            cv::Mat mapx_;
+            cv::Mat mapy_;
+            bool camera_set_;
+    };
+
+}

image_proc_fisheye/src/nodelets/rectify_.cpp

@@ -0,0 +1,46 @@
+#include "ros/ros.h"
+#include "rectify.h"
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(image_proc_fisheye::RectifyNodelet, nodelet::Nodelet)
+
+
+namespace image_proc_fisheye {
+  void RectifyNodelet::onInit(){
+    ros::NodeHandle &nh = getNodeHandle();
+    camera_set_=false;
+    sub_ = nh.subscribe("image_raw", 5, &RectifyNodelet::process_image, this);
+    sub_info_ = nh.subscribe("camera_info", 5, &RectifyNodelet::camera_info, this);
+    pub_ = nh.advertise<sensor_msgs::Image>("image_rect", 10);
+  };
+
+  void RectifyNodelet::process_image(const sensor_msgs::ImageConstPtr& frame){
+    if(!camera_set_){
+      return;
+    }
+    cv_bridge::CvImageConstPtr image = cv_bridge::toCvShare(frame);
+    cv::Mat image_gpu(image->image);
+    cv::Mat image_gpu_rect(cv::Size(image->image.rows, image->image.cols), image->image.type());
+    cv::remap(image_gpu, image_gpu_rect, mapx_, mapy_, cv::INTER_LANCZOS4, cv::BORDER_CONSTANT);
+    cv::Mat image_rect = cv::Mat(image_gpu_rect);
+
+    cv_bridge::CvImage out_msg;
+    out_msg.header   = frame->header;
+    out_msg.encoding = frame->encoding;
+    out_msg.image  = image_rect;
+    pub_.publish(out_msg.toImageMsg());
+  }
+
+  void RectifyNodelet::camera_info(const sensor_msgs::CameraInfoConstPtr& info_msg){
+    image_geometry::PinholeCameraModel camera;
+    camera.fromCameraInfo(info_msg);
+    cv::Mat m1;
+    cv::Mat m2;
+    cv::fisheye::initUndistortRectifyMap(camera.intrinsicMatrix(), camera.distortionCoeffs(), cv::Mat(), camera.intrinsicMatrix(), camera.fullResolution(), CV_32FC1, m1, m2);
+    mapx_ = cv::Mat(m1);
+    mapy_ = cv::Mat(m2);
+    sub_info_.shutdown();
+    camera_set_ = true;
+  }
+} // namespace
+