work in progress [DO NOT MERGE]

CMakeLists.txt

@@ -8,16 +8,8 @@ set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
 set(CMAKE_CXX_FLAGS "-std=c++11 -Ofast")
 
 find_package(Eigen3 REQUIRED)
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  roslib
-  pcl_ros
-  sensor_msgs
-  cv_bridge
-  image_transport
-  tf_conversions
-  nodelet
-)
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
 
 catkin_package(
   INCLUDE_DIRS include
@@ -32,7 +24,7 @@ IF(${NLOPT_FOUND})
     ${NLOPT_INCLUDE_DIRS}
   )
 
-  add_library(${PROJECT_NAME}_nodelet 
+  cs_add_library(${PROJECT_NAME}_nodelet 
   src/nodelets/image_undistort_nodelet.cpp 
   src/nodelets/stereo_info_nodelet.cpp
   src/nodelets/stereo_undistort_nodelet.cpp
@@ -41,21 +33,23 @@ IF(${NLOPT_FOUND})
   src/stereo_info.cpp
   src/stereo_undistort.cpp
   src/depth.cpp
+  src/depth_generator.cpp
   src/camera_parameters.cpp 
   src/image_undistort.cpp 
   src/undistorter.cpp
   src/point_to_bearing.cpp)
 
 ELSE()
 
-  add_library(${PROJECT_NAME}_nodelet 
+  cs_add_library(${PROJECT_NAME}_nodelet 
   src/nodelets/image_undistort_nodelet.cpp 
   src/nodelets/stereo_info_nodelet.cpp
   src/nodelets/stereo_undistort_nodelet.cpp
   src/nodelets/depth_nodelet.cpp
   src/stereo_info.cpp
   src/stereo_undistort.cpp
   src/depth.cpp
+  src/depth_generator.cpp
   src/camera_parameters.cpp 
   src/image_undistort.cpp 
   src/undistorter.cpp)
@@ -64,65 +58,45 @@ ELSE()
 ENDIF()
 
 include_directories(
-  include
   ${EIGEN3_INCLUDE_DIR}
-  ${catkin_INCLUDE_DIRS}
 )
 
 target_link_libraries(${PROJECT_NAME}_nodelet
   ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}
 )
 add_dependencies(${PROJECT_NAME}_nodelet ${catkin_EXPORTED_TARGETS})
 
-add_executable(image_undistort_node src/nodes/image_undistort_node.cpp)
+cs_add_executable(image_undistort_node src/nodes/image_undistort_node.cpp)
 target_link_libraries(image_undistort_node
   ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}
 )
 
-add_executable(stereo_info_node src/nodes/stereo_info_node.cpp)
+cs_add_executable(stereo_info_node src/nodes/stereo_info_node.cpp)
 target_link_libraries(stereo_info_node
   ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}
 )
 
-add_executable(stereo_undistort_node src/nodes/stereo_undistort_node.cpp)
+cs_add_executable(stereo_undistort_node src/nodes/stereo_undistort_node.cpp)
 target_link_libraries(stereo_undistort_node
   ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}
 )
 
-add_executable(depth_node src/nodes/depth_node.cpp)
+cs_add_executable(depth_node src/nodes/depth_node.cpp)
 target_link_libraries(depth_node
   ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}
 )
 
-add_executable(dense_stereo_node src/nodes/dense_stereo_node.cpp)
+cs_add_executable(dense_stereo_node src/nodes/dense_stereo_node.cpp)
 target_link_libraries(dense_stereo_node
   ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}
 )
 
 IF(${NLOPT_FOUND})
-  add_executable(point_to_bearing_node src/nodes/point_to_bearing_node.cpp)
+  cs_add_executable(point_to_bearing_node src/nodes/point_to_bearing_node.cpp)
   target_link_libraries(point_to_bearing_node
     ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${NLOPT_LIBRARIES}
   )
 ENDIF()
 
-# Install nodelet library
-install(TARGETS ${PROJECT_NAME}_nodelet
-  DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-)
-
-# Install header files
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-
-# Install launch files
-install(DIRECTORY launch/
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
-)
-
-# Install xml files
-install(FILES nodelet_plugins.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
+cs_install()
+cs_export()

include/image_undistort/depth.h

@@ -7,45 +7,18 @@
 #include <ros/callback_queue.h>
 #include <ros/ros.h>
 
-#include <pcl_conversions/pcl_conversions.h>
-#include <sensor_msgs/PointCloud2.h>
-
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.h>
 #include <message_filters/time_synchronizer.h>
 
-#include <cv_bridge/cv_bridge.h>
-#include <opencv2/core/core.hpp>
-#include <opencv2/opencv.hpp>
+#include "image_undistort/depth_generator.h"
 
 namespace image_undistort {
 
 // Default values
 
 // queue size
 constexpr int kQueueSize = 10;
-// small number used to check things are approximately equal
-constexpr double kDelta = 0.000000001;
-// stereo parameters
-constexpr int kPreFilterCap = 31;
-constexpr int kSADWindowSize = 11;
-constexpr int kMinDisparity = 0;
-constexpr int kNumDisparities = 64;
-constexpr int kUniquenessRatio = 0;
-constexpr int kSpeckleRange = 3;
-constexpr int kSpeckleWindowSize = 500;
-// bm parameters
-constexpr int kTextureThreshold = 0;
-const std::string kPreFilterType = "xsobel";
-constexpr int kPreFilterSize = 9;
-// sgbm parameters
-constexpr bool kUseSGBM = false;
-constexpr int kP1 = 120;
-constexpr int kP2 = 240;
-constexpr int kDisp12MaxDiff = -1;
-constexpr bool kUseHHMode = false;
-
-constexpr bool kDoMedianBlur = true;
 
 class Depth {
  public:
@@ -60,32 +33,6 @@ class Depth {
  private:
   int getQueueSize() const;
 
-  static bool ApproxEq(double A, double B);
-
-  bool processCameraInfo(
-      const sensor_msgs::CameraInfoConstPtr& first_camera_info,
-      const sensor_msgs::CameraInfoConstPtr& second_camera_info,
-      double* baseline, double* focal_length, bool* first_is_left, int* cx,
-      int* cy);
-
-  static void fillDisparityFromSide(const cv::Mat& input_disparity,
-                                    const cv::Mat& valid, const bool& from_left,
-                                    cv::Mat* filled_disparity);
-
-  void bulidFilledDisparityImage(const cv::Mat& input_disparity,
-                                 cv::Mat* disparity_filled,
-                                 cv::Mat* input_valid) const;
-
-  void calcDisparityImage(const sensor_msgs::ImageConstPtr& first_image_msg_in,
-                          const sensor_msgs::ImageConstPtr& second_image_msg_in,
-                          cv_bridge::CvImagePtr disparity_ptr) const;
-
-  void calcPointCloud(const cv::Mat& input_disparity, const cv::Mat& left_image,
-                      const double baseline, const double focal_length,
-                      const int cx, const int cy,
-                      pcl::PointCloud<pcl::PointXYZRGB>* pointcloud,
-                      pcl::PointCloud<pcl::PointXYZRGB>* freespace_pointcloud);
-
   // nodes
   ros::NodeHandle nh_;
   ros::NodeHandle nh_private_;
@@ -111,28 +58,7 @@ class Depth {
       CameraSyncPolicy;
   message_filters::Synchronizer<CameraSyncPolicy> camera_sync_;
 
-  // stereo parameters
-  int pre_filter_cap_;
-  int sad_window_size_;
-  int min_disparity_;
-  int num_disparities_;
-  int uniqueness_ratio_;
-  int speckle_range_;
-  int speckle_window_size_;
-
-  // bm parameters
-  int texture_threshold_;
-  int pre_filter_type_;
-  int pre_filter_size_;
-
-  // sgbm parameters
-  bool use_sgbm_;
-  int p1_;
-  int p2_;
-  int disp_12_max_diff_;
-  bool use_mode_HH_;
-
-  bool do_median_blur_;
+  std::shared_ptr<DepthGenerator> depth_gen_;
 };
 }
 

include/image_undistort/depth_generator.h

@@ -0,0 +1,80 @@
+#ifndef IMAGE_UNDISTORT_DEPTH_GENERATOR_H
+#define IMAGE_UNDISTORT_DEPTH_GENERATOR_H
+
+#include <ros/ros.h>
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/PointCloud2.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/opencv.hpp>
+
+namespace image_undistort {
+
+// small number used to check things are approximately equal
+constexpr double kDelta = 1e-8;
+
+class DepthGenerator {
+ public:
+  struct Config {
+    // stereo parameters
+    int pre_filter_cap = 31;
+    int sad_window_size = 11;
+    int min_disparity = 0;
+    int num_disparities = 64;
+    int uniqueness_ratio = 0;
+    int speckle_range = 3;
+    int speckle_window_size = 500;
+
+    // bm parameters
+    int texture_threshold = 0;
+    int pre_filter_type = cv::StereoBM::PREFILTER_XSOBEL;
+    int pre_filter_size = 9;
+
+    // sgbm parameters
+    bool use_sgbm = false;
+    int p1 = 120;
+    int p2 = 240;
+    int disp_12_max_diff = -1;
+    bool use_mode_HH = false;
+
+    bool do_median_blur = true;
+  };
+
+  DepthGenerator(const Config& config) : config_(config){}
+
+  void calcDisparityImage(const cv::Mat& first_image,
+                          const cv::Mat& second_image,
+                          cv::Mat* disparity_ptr) const;
+
+  void calcPointCloud(const cv::Mat& input_disparity, const cv::Mat& left_image,
+                      const double baseline, const double focal_length,
+                      const int cx, const int cy,
+                      pcl::PointCloud<pcl::PointXYZRGB>* pointcloud,
+                      pcl::PointCloud<pcl::PointXYZRGB>* freespace_pointcloud);
+
+  static bool processCameraInfo(
+      const sensor_msgs::CameraInfo& first_camera_info,
+      const sensor_msgs::CameraInfo& second_camera_info, double* baseline,
+      double* focal_length, bool* first_is_left, int* cx, int* cy);
+
+ private:
+  int getQueueSize() const;
+
+  static bool ApproxEq(double A, double B);
+
+  static void fillDisparityFromSide(const cv::Mat& input_disparity,
+                                    const cv::Mat& valid, const bool& from_left,
+                                    cv::Mat* filled_disparity);
+
+  void bulidFilledDisparityImage(const cv::Mat& input_disparity,
+                                 cv::Mat* disparity_filled,
+                                 cv::Mat* input_valid) const;
+
+  // holds all stereo matching parameters
+  Config config_;
+};
+}  // namespace image_undistort
+
+#endif

include/image_undistort/image_undistort.h

@@ -71,6 +71,7 @@ constexpr bool kDefaultRenameRadtanPlumbBob = false;
 
 class ImageUndistort {
  public:
+
   ImageUndistort(const ros::NodeHandle& nh_,
                  const ros::NodeHandle& nh_private_);