From 8b33bb7a0599938735e9922ab53043b3c585af84 Mon Sep 17 00:00:00 2001 From: Zachary Taylor Date: Fri, 13 Jul 2018 15:36:30 +0200 Subject: [PATCH 1/2] split out depth processing --- CMakeLists.txt | 54 +-- include/image_undistort/depth.h | 78 +--- include/image_undistort/image_undistort.h | 1 + package.xml | 1 + src/depth.cpp | 410 +++------------------- 5 files changed, 66 insertions(+), 478 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 68b23ef..3a024b2 100644 --- a/CMakeLists.txt +++ b/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,6 +33,7 @@ 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 @@ -48,7 +41,7 @@ IF(${NLOPT_FOUND}) 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 @@ -56,6 +49,7 @@ ELSE() 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,9 +58,7 @@ ELSE() ENDIF() include_directories( - include ${EIGEN3_INCLUDE_DIR} - ${catkin_INCLUDE_DIRS} ) target_link_libraries(${PROJECT_NAME}_nodelet @@ -74,55 +66,37 @@ target_link_libraries(${PROJECT_NAME}_nodelet ) 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() diff --git a/include/image_undistort/depth.h b/include/image_undistort/depth.h index f6a10cc..566221c 100644 --- a/include/image_undistort/depth.h +++ b/include/image_undistort/depth.h @@ -7,16 +7,11 @@ #include #include -#include -#include - #include #include #include -#include -#include -#include +#include "image_undistort/depth_generator.h" namespace image_undistort { @@ -24,28 +19,6 @@ namespace image_undistort { // 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* pointcloud, - pcl::PointCloud* freespace_pointcloud); - // nodes ros::NodeHandle nh_; ros::NodeHandle nh_private_; @@ -111,28 +58,7 @@ class Depth { CameraSyncPolicy; message_filters::Synchronizer 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 depth_gen_; }; } diff --git a/include/image_undistort/image_undistort.h b/include/image_undistort/image_undistort.h index 0ee9cac..969cf4c 100644 --- a/include/image_undistort/image_undistort.h +++ b/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_); diff --git a/package.xml b/package.xml index 6ce1ecc..57a84fa 100644 --- a/package.xml +++ b/package.xml @@ -10,6 +10,7 @@ BSD catkin + catkin_simple roscpp roslib diff --git a/src/depth.cpp b/src/depth.cpp index 77f26f1..d571508 100644 --- a/src/depth.cpp +++ b/src/depth.cpp @@ -25,13 +25,16 @@ Depth::Depth(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private) camera_sync_(CameraSyncPolicy(queue_size_), first_image_sub_, second_image_sub_, first_camera_info_sub_, second_camera_info_sub_) { + DepthGenerator::Config config; + std::string pre_filter_type_string; - nh_private_.param("pre_filter_type", pre_filter_type_string, kPreFilterType); + nh_private_.param("pre_filter_type", pre_filter_type_string, + std::string("xsobel")); if (pre_filter_type_string == std::string("xsobel")) { - pre_filter_type_ = cv::StereoBM::PREFILTER_XSOBEL; + config.pre_filter_type = cv::StereoBM::PREFILTER_XSOBEL; } else if (pre_filter_type_string == std::string("normalized_response")) { - pre_filter_type_ = cv::StereoBM::PREFILTER_NORMALIZED_RESPONSE; + config.pre_filter_type = cv::StereoBM::PREFILTER_NORMALIZED_RESPONSE; } else { throw std::runtime_error( "Unrecognized prefilter type, choices are 'xsobel' or " @@ -39,27 +42,37 @@ Depth::Depth(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private) } // general stereo parameters - nh_private_.param("min_disparity", min_disparity_, kMinDisparity); - nh_private_.param("num_disparities", num_disparities_, kNumDisparities); - nh_private_.param("pre_filter_cap", pre_filter_cap_, kPreFilterCap); - nh_private_.param("uniqueness_ratio", uniqueness_ratio_, kUniquenessRatio); - nh_private_.param("speckle_range", speckle_range_, kSpeckleRange); - nh_private_.param("speckle_window_size", speckle_window_size_, - kSpeckleWindowSize); - nh_private_.param("sad_window_size", sad_window_size_, kSADWindowSize); + nh_private_.param("min_disparity", config.min_disparity, + config.min_disparity); + nh_private_.param("num_disparities", config.num_disparities, + config.num_disparities); + nh_private_.param("pre_filter_cap", config.pre_filter_cap, + config.pre_filter_cap); + nh_private_.param("uniqueness_ratio", config.uniqueness_ratio, + config.uniqueness_ratio); + nh_private_.param("speckle_range", config.speckle_range, + config.speckle_range); + nh_private_.param("speckle_window_size", config.speckle_window_size, + config.speckle_window_size); + nh_private_.param("sad_window_size", config.sad_window_size, + config.sad_window_size); // bm parameters - nh_private_.param("texture_threshold", texture_threshold_, kTextureThreshold); - nh_private_.param("pre_filter_size", pre_filter_size_, kPreFilterSize); + nh_private_.param("texture_threshold", config.texture_threshold, + config.texture_threshold); + nh_private_.param("pre_filter_size", config.pre_filter_size, + config.pre_filter_size); // sgbm parameters - nh_private_.param("use_sgbm", use_sgbm_, kUseSGBM); - nh_private_.param("p1", p1_, kP1); - nh_private_.param("p2", p2_, kP2); - nh_private_.param("disp_12_max_diff", disp_12_max_diff_, kDisp12MaxDiff); - nh_private_.param("use_mode_HH", use_mode_HH_, kUseHHMode); + nh_private_.param("use_sgbm", config.use_sgbm, config.use_sgbm); + nh_private_.param("p1", config.p1, config.p1); + nh_private_.param("p2", config.p2, config.p2); + nh_private_.param("disp_12_max_diff", config.disp_12_max_diff, + config.disp_12_max_diff); + nh_private_.param("use_mode_HH", config.use_mode_HH, config.use_mode_HH); - nh_private_.param("do_median_blur", do_median_blur_, kDoMedianBlur); + nh_private_.param("do_median_blur", config.do_median_blur, + config.do_median_blur); camera_sync_.registerCallback( boost::bind(&Depth::camerasCallback, this, _1, _2, _3, _4)); @@ -71,342 +84,6 @@ Depth::Depth(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private) "freespace_pointcloud", queue_size_); } -// simply replaces invalid disparity values with a valid value found by scanning -// horizontally (note: if disparity values are already valid or if no valid -// value can be found int_max is inserted) -void Depth::fillDisparityFromSide(const cv::Mat& input_disparity, - const cv::Mat& valid, const bool& from_left, - cv::Mat* filled_disparity) { - *filled_disparity = - cv::Mat(input_disparity.rows, input_disparity.cols, CV_16S); - - for (size_t y_pixels = 0; y_pixels < input_disparity.rows; ++y_pixels) { - bool prev_valid = false; - int16_t prev_value; - - for (size_t x_pixels = 0; x_pixels < input_disparity.cols; ++x_pixels) { - size_t x_scan; - if (from_left) { - x_scan = x_pixels; - } else { - x_scan = (input_disparity.cols - x_pixels - 1); - } - - if (valid.at(y_pixels, x_scan)) { - prev_valid = true; - prev_value = input_disparity.at(y_pixels, x_scan); - filled_disparity->at(y_pixels, x_scan) = - std::numeric_limits::max(); - } else if (prev_valid) { - filled_disparity->at(y_pixels, x_scan) = prev_value; - } else { - filled_disparity->at(y_pixels, x_scan) = - std::numeric_limits::max(); - } - } - } -} - -void Depth::bulidFilledDisparityImage(const cv::Mat& input_disparity, - cv::Mat* disparity_filled, - cv::Mat* input_valid) const { - // mark valid pixels - *input_valid = cv::Mat(input_disparity.rows, input_disparity.cols, CV_8U); - - int side_bound = sad_window_size_ / 2; - - for (size_t y_pixels = 0; y_pixels < input_disparity.rows; ++y_pixels) { - for (size_t x_pixels = 0; x_pixels < input_disparity.cols; ++x_pixels) { - // the last check is because the sky has a bad habit of having a disparity - // at just less than the max disparity - if ((x_pixels < side_bound + min_disparity_ + num_disparities_) || - (y_pixels < side_bound) || - (x_pixels > (input_disparity.cols - side_bound)) || - (y_pixels > (input_disparity.rows - side_bound)) || - (input_disparity.at(y_pixels, x_pixels) < 0) || - (input_disparity.at(y_pixels, x_pixels) >= - (min_disparity_ + num_disparities_ - 1) * 16)) { - input_valid->at(y_pixels, x_pixels) = 0; - } else { - input_valid->at(y_pixels, x_pixels) = 1; - } - } - } - - // erode by size of SAD window, this prevents issues with background pixels - // being given the same depth as neighboring objects in the foreground. - cv::Mat kernel = cv::getStructuringElement( - cv::MORPH_RECT, cv::Size(sad_window_size_, sad_window_size_)); - cv::erode(*input_valid, *input_valid, kernel); - - // take a guess for the depth of the invalid pixels by scanning along the row - // and giving them the same value as the closest horizontal point. - cv::Mat disparity_filled_left, disparity_filled_right; - fillDisparityFromSide(input_disparity, *input_valid, true, - &disparity_filled_left); - fillDisparityFromSide(input_disparity, *input_valid, false, - &disparity_filled_right); - - // take the most conservative disparity of the two - *disparity_filled = cv::max(disparity_filled_left, disparity_filled_right); - - // 0 disparity is valid but cannot have a depth associated with it, because of - // this we take these points and replace them with a disparity of 1. - for (size_t y_pixels = 0; y_pixels < input_disparity.rows; ++y_pixels) { - for (size_t x_pixels = 0; x_pixels < input_disparity.cols; ++x_pixels) { - if (input_disparity.at(y_pixels, x_pixels) == 0) { - disparity_filled->at(y_pixels, x_pixels) = 1; - } - } - } -} - -void Depth::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* pointcloud, - pcl::PointCloud* freespace_pointcloud) { - pointcloud->clear(); - freespace_pointcloud->clear(); - - if (left_image.depth() != CV_8U) { - ROS_ERROR( - "Pointcloud generation is currently only supported on 8 bit images"); - return; - } - - cv::Mat disparity_filled, input_valid; - bulidFilledDisparityImage(input_disparity, &disparity_filled, &input_valid); - - int side_bound = sad_window_size_ / 2; - - // build pointcloud - for (int y_pixels = side_bound; y_pixels < input_disparity.rows - side_bound; - ++y_pixels) { - for (int x_pixels = side_bound + min_disparity_ + num_disparities_; - x_pixels < input_disparity.cols - side_bound; ++x_pixels) { - const uint8_t& is_valid = input_valid.at(y_pixels, x_pixels); - const int16_t& input_value = - input_disparity.at(y_pixels, x_pixels); - const int16_t& filled_value = - disparity_filled.at(y_pixels, x_pixels); - - bool freespace; - double disparity_value; - - // if the filled disparity is valid it must be a freespace ray - if (filled_value < std::numeric_limits::max()) { - disparity_value = static_cast(filled_value); - freespace = true; - } - // else it is a normal ray - else if (is_valid) { - disparity_value = static_cast(input_value); - freespace = false; - } else { - continue; - } - - pcl::PointXYZRGB point; - - // the 16* is needed as opencv stores disparity maps as 16 * the true - // values - point.z = (16 * focal_length * baseline) / disparity_value; - point.x = point.z * (x_pixels - cx) / focal_length; - point.y = point.z * (y_pixels - cy) / focal_length; - - if (left_image.channels() == 3) { - const cv::Vec3b& color = left_image.at(y_pixels, x_pixels); - point.b = color[0]; - point.g = color[1]; - point.r = color[2]; - } else if (left_image.channels() == 4) { - const cv::Vec4b& color = left_image.at(y_pixels, x_pixels); - point.b = color[0]; - point.g = color[1]; - point.r = color[2]; - } else { - point.b = left_image.at(y_pixels, x_pixels); - point.g = point.b; - point.r = point.b; - } - - if (freespace) { - freespace_pointcloud->push_back(point); - } else { - pointcloud->push_back(point); - } - } - } -} - -void Depth::calcDisparityImage( - const sensor_msgs::ImageConstPtr& left_image_msg, - const sensor_msgs::ImageConstPtr& right_image_msg, - cv_bridge::CvImagePtr disparity_ptr) const { - cv_bridge::CvImageConstPtr left_ptr = - cv_bridge::toCvShare(left_image_msg, "mono8"); - - cv_bridge::CvImageConstPtr right_ptr = - cv_bridge::toCvShare(right_image_msg, "mono8"); - -#if (defined(CV_VERSION_EPOCH) && CV_VERSION_EPOCH == 2) - - if (use_sgbm_) { - std::shared_ptr left_matcher = - std::make_shared(); - - left_matcher->numberOfDisparities = num_disparities_; - left_matcher->SADWindowSize = sad_window_size_; - left_matcher->preFilterCap = pre_filter_cap_; - left_matcher->minDisparity = min_disparity_; - left_matcher->uniquenessRatio = uniqueness_ratio_; - left_matcher->speckleRange = speckle_range_; - left_matcher->speckleWindowSize = speckle_window_size_; - left_matcher->P1 = p1_; - left_matcher->P2 = p2_; - left_matcher->disp12MaxDiff = disp_12_max_diff_; - left_matcher->fullDP = use_mode_HH_; - left_matcher->operator()(left_ptr->image, right_ptr->image, - disparity_ptr->image); - } else { - std::shared_ptr left_matcher = - std::make_shared(); - - left_matcher->state->numberOfDisparities = num_disparities_; - left_matcher->state->SADWindowSize = sad_window_size_; - left_matcher->state->preFilterCap = pre_filter_cap_; - left_matcher->state->preFilterSize = pre_filter_size_; - left_matcher->state->minDisparity = min_disparity_; - left_matcher->state->textureThreshold = texture_threshold_; - left_matcher->state->uniquenessRatio = uniqueness_ratio_; - left_matcher->state->speckleRange = speckle_range_; - left_matcher->state->speckleWindowSize = speckle_window_size_; - left_matcher->operator()(left_ptr->image, right_ptr->image, - disparity_ptr->image); - } -#else - if (use_sgbm_) { - int mode; - if (use_mode_HH_) { - mode = cv::StereoSGBM::MODE_HH; - } else { - mode = cv::StereoSGBM::MODE_SGBM; - } - - cv::Ptr left_matcher = cv::StereoSGBM::create( - min_disparity_, num_disparities_, sad_window_size_, p1_, p2_, - disp_12_max_diff_, pre_filter_cap_, uniqueness_ratio_, - speckle_window_size_, speckle_range_, mode); - - left_matcher->compute(left_ptr->image, right_ptr->image, - disparity_ptr->image); - - } else { - cv::Ptr left_matcher = - cv::StereoBM::create(num_disparities_, sad_window_size_); - - left_matcher->setPreFilterType(pre_filter_type_); - left_matcher->setPreFilterSize(pre_filter_size_); - left_matcher->setPreFilterCap(pre_filter_cap_); - left_matcher->setMinDisparity(min_disparity_); - left_matcher->setTextureThreshold(texture_threshold_); - left_matcher->setUniquenessRatio(uniqueness_ratio_); - left_matcher->setSpeckleRange(speckle_range_); - left_matcher->setSpeckleWindowSize(speckle_window_size_); - left_matcher->compute(left_ptr->image, right_ptr->image, - disparity_ptr->image); - } -#endif - - if(do_median_blur_){ - cv::medianBlur(disparity_ptr->image, disparity_ptr->image, 5); - } -} - -bool Depth::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) { - if (first_camera_info->height != second_camera_info->height) { - ROS_ERROR("Image heights do not match"); - return false; - } - if (first_camera_info->width != second_camera_info->width) { - ROS_ERROR("Image widths do not match"); - return false; - } - - for (double d : first_camera_info->D) { - if (!ApproxEq(d, 0)) { - ROS_ERROR("First image has non-zero distortion"); - return false; - } - } - for (double d : second_camera_info->D) { - if (!ApproxEq(d, 0)) { - ROS_ERROR("Second image has non-zero distortion"); - return false; - } - } - - for (size_t i = 0; i < 12; ++i) { - if ((i != 3) && - !ApproxEq(first_camera_info->P[i], second_camera_info->P[i])) { - ROS_ERROR("Image P matrices must match (excluding x offset)"); - return false; - } - } - - if (!ApproxEq(first_camera_info->P[1], 0) || - !ApproxEq(first_camera_info->P[4], 0)) { - ROS_ERROR("Image P matrix contains skew"); - return false; - } - - if (!ApproxEq(first_camera_info->P[0], first_camera_info->P[5])) { - ROS_ERROR("Image P matrix has different values for Fx and Fy"); - return false; - } - - if (first_camera_info->P[0] <= 0) { - ROS_ERROR("Focal length must be greater than 0"); - return false; - } - - // downgraded to warning so that the color images of the KITTI dataset can be - // processed - if (!ApproxEq(first_camera_info->P[8], 0) || - !ApproxEq(first_camera_info->P[9], 0) || - !ApproxEq(first_camera_info->P[10], 1) || - !ApproxEq(first_camera_info->P[11], 0)) { - ROS_WARN_ONCE( - "Image P matrix does not end in [0,0,1,0], these values will be " - "ignored"); - } - - // again downgraded to warning because KITTI has ugly matrices - if (!ApproxEq(first_camera_info->P[7], 0)) { - ROS_WARN_ONCE("P contains Y offset, this value will be ignored"); - } - - *focal_length = first_camera_info->P[0]; - *baseline = (second_camera_info->P[3] - first_camera_info->P[3]) / - first_camera_info->P[0]; - if (*baseline > 0) { - *first_is_left = false; - } else { - *first_is_left = true; - *baseline *= -1; - } - *cx = first_camera_info->P[2]; - *cy = first_camera_info->P[6]; - - return true; -} - -bool Depth::ApproxEq(double A, double B) { return (std::abs(A - B) <= kDelta); } - void Depth::camerasCallback( const sensor_msgs::ImageConstPtr& first_image_msg, const sensor_msgs::ImageConstPtr& second_image_msg, @@ -416,8 +93,9 @@ void Depth::camerasCallback( bool first_is_left; int cx, cy; - if (!processCameraInfo(first_camera_info, second_camera_info, &baseline, - &focal_length, &first_is_left, &cx, &cy)) { + if (!depth_gen_->processCameraInfo(*first_camera_info, *second_camera_info, + &baseline, &focal_length, &first_is_left, + &cx, &cy)) { ROS_ERROR("Camera info processing failed, skipping disparity generation"); return; } @@ -435,17 +113,25 @@ void Depth::camerasCallback( cv::Mat disparity_image = cv::Mat(left_image_msg->height, left_image_msg->width, CV_16S); + cv_bridge::CvImagePtr disparity_ptr(new cv_bridge::CvImage( left_image_msg->header, "mono16", disparity_image)); + cv_bridge::CvImageConstPtr left_ptr = + cv_bridge::toCvShare(left_image_msg, "mono8"); + cv_bridge::CvImageConstPtr right_ptr = + cv_bridge::toCvShare(right_image_msg, "mono8"); - calcDisparityImage(left_image_msg, right_image_msg, disparity_ptr); + depth_gen_->calcDisparityImage(left_ptr->image, right_ptr->image, + &(disparity_ptr->image)); disparity_pub_.publish(*(disparity_ptr->toImageMsg())); pcl::PointCloud pointcloud; pcl::PointCloud freespace_pointcloud; - cv_bridge::CvImageConstPtr left_ptr = cv_bridge::toCvShare(left_image_msg); - calcPointCloud(disparity_ptr->image, left_ptr->image, baseline, focal_length, - cx, cy, &pointcloud, &freespace_pointcloud); + + left_ptr = cv_bridge::toCvShare(left_image_msg); + depth_gen_->calcPointCloud(disparity_ptr->image, left_ptr->image, baseline, + focal_length, cx, cy, &pointcloud, + &freespace_pointcloud); sensor_msgs::PointCloud2 pointcloud_msg; pcl::toROSMsg(pointcloud, pointcloud_msg); @@ -457,4 +143,4 @@ void Depth::camerasCallback( freespace_pointcloud_msg.header = left_image_msg->header; freespace_pointcloud_pub_.publish(freespace_pointcloud_msg); } -} +} // namespace image_undistort From 8529869b2d697af5b04faf9a6b04a5a09d380de6 Mon Sep 17 00:00:00 2001 From: Zachary Taylor Date: Fri, 13 Jul 2018 15:50:50 +0200 Subject: [PATCH 2/2] added generator files --- include/image_undistort/depth_generator.h | 80 +++++ include/image_undistort/stereo_undistort.h | 71 ++--- src/depth_generator.cpp | 335 +++++++++++++++++++++ src/stereo_undistort.cpp | 4 +- 4 files changed, 439 insertions(+), 51 deletions(-) create mode 100644 include/image_undistort/depth_generator.h create mode 100644 src/depth_generator.cpp diff --git a/include/image_undistort/depth_generator.h b/include/image_undistort/depth_generator.h new file mode 100644 index 0000000..ce4b8a4 --- /dev/null +++ b/include/image_undistort/depth_generator.h @@ -0,0 +1,80 @@ +#ifndef IMAGE_UNDISTORT_DEPTH_GENERATOR_H +#define IMAGE_UNDISTORT_DEPTH_GENERATOR_H + +#include + +#include +#include + +#include +#include +#include + +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* pointcloud, + pcl::PointCloud* 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 diff --git a/include/image_undistort/stereo_undistort.h b/include/image_undistort/stereo_undistort.h index bfa2e54..c8656b4 100644 --- a/include/image_undistort/stereo_undistort.h +++ b/include/image_undistort/stereo_undistort.h @@ -23,43 +23,27 @@ namespace image_undistort { // Default values -// queue size -constexpr int kQueueSize = 10; -// true to load input cam_info from ros parameters, false to get it from a -// cam_info topic -constexpr bool kDefaultInputCameraInfoFromROSParams = true; -// namespace to use when loading first camera parameters from ros params -const std::string kDefaultFirstCameraNamespace = "first_camera"; -// namespace to use when loading second camera parameters from ros params -const std::string kDefaultSecondCameraNamespace = "second_camera"; -// downsamples output rate if <= 1, every frame is processed. -constexpr int kDefaultProcessEveryNthFrame = 1; -// converts the output image to the given format, set to the empty string "" to -// copy input type. Consult cv_bridge documentation for valid strings -const std::string kDefaultOutputImageType = ""; -// if the output camera source is set to "auto_generated", the output focal -// length will be multiplied by this value. This has the effect of -// resizing the image by this scale factor. -constexpr double kDefaultScale = 1.0; -// if a tf between the input and output frame should be created -constexpr bool kDefaultPublishTF = true; -// name of first output image frame -const std::string kDefaultFirstOutputFrame = "first_camera_rect"; -// name of second output image frame -const std::string kDefaultSecondOutputFrame = "second_camera_rect"; -// rename input frames -constexpr bool kDefaultRenameInputFrame = false; -// new name of first input frame -const std::string kDefaultFirstInputFrame = "first_camera"; -// new name of second input frame -const std::string kDefaultSecondInputFrame = "second_camera"; -// if radtan distortion should be called radtan (ASL standard) or plumb_bob (ros -// standard) -constexpr bool kDefaultRenameRadtanPlumbBob = false; - class StereoUndistort { - public: - StereoUndistort(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); + struct Config { + // true to load input cam_info from ros parameters, false to get it from a + // cam_info topic + bool input_camera_info_from_ros_params_ = true; + int queue_size_ = 10; + int process_every_nth_frame_ = 1; + // converts the output image to the given format, set to the empty string "" + // to copy input type. Consult cv_bridge documentation for valid strings + std::string output_image_type_ = ""; + bool publish_tf_ = true; + std::string first_output_frame_ = "first_camera_rect"; + std::string second_output_frame_ = "second_camera_rect"; + bool rename_input_frame_ = false; + std::string first_input_frame_ = "first_camera"; + std::string second_input_frame_ = "second_camera"; + // if radtan distortion should be called radtan (ASL standard) or plumb_bob + // (ros standard) + bool rename_radtan_plumb_bob_ = false; + } public : StereoUndistort(const ros::NodeHandle& nh, + const ros::NodeHandle& nh_private); void imagesCallback(const sensor_msgs::ImageConstPtr& first_image_msg_in, const sensor_msgs::ImageConstPtr& second_image_msg_in); @@ -126,20 +110,9 @@ class StereoUndistort { std::shared_ptr first_undistorter_ptr_; std::shared_ptr second_undistorter_ptr_; - // other variables - bool input_camera_info_from_ros_params_; - int queue_size_; - int process_every_nth_frame_; - std::string output_image_type_; - bool publish_tf_; - std::string first_output_frame_; - std::string second_output_frame_; - bool rename_input_frame_; - std::string first_input_frame_; - std::string second_input_frame_; - bool rename_radtan_plumb_bob_; int frame_counter_; + Config config_; }; -} +} // namespace image_undistort #endif diff --git a/src/depth_generator.cpp b/src/depth_generator.cpp new file mode 100644 index 0000000..20dea0a --- /dev/null +++ b/src/depth_generator.cpp @@ -0,0 +1,335 @@ +#include "image_undistort/depth.h" + +namespace image_undistort { + +// simply replaces invalid disparity values with a valid value found by scanning +// horizontally (note: if disparity values are already valid or if no valid +// value can be found int_max is inserted) +void DepthGenerator::fillDisparityFromSide(const cv::Mat& input_disparity, + const cv::Mat& valid, + const bool& from_left, + cv::Mat* filled_disparity) { + *filled_disparity = + cv::Mat(input_disparity.rows, input_disparity.cols, CV_16S); + + for (size_t y_pixels = 0; y_pixels < input_disparity.rows; ++y_pixels) { + bool prev_valid = false; + int16_t prev_value; + + for (size_t x_pixels = 0; x_pixels < input_disparity.cols; ++x_pixels) { + size_t x_scan; + if (from_left) { + x_scan = x_pixels; + } else { + x_scan = (input_disparity.cols - x_pixels - 1); + } + + if (valid.at(y_pixels, x_scan)) { + prev_valid = true; + prev_value = input_disparity.at(y_pixels, x_scan); + filled_disparity->at(y_pixels, x_scan) = + std::numeric_limits::max(); + } else if (prev_valid) { + filled_disparity->at(y_pixels, x_scan) = prev_value; + } else { + filled_disparity->at(y_pixels, x_scan) = + std::numeric_limits::max(); + } + } + } +} + +void DepthGenerator::bulidFilledDisparityImage(const cv::Mat& input_disparity, + cv::Mat* disparity_filled, + cv::Mat* input_valid) const { + // mark valid pixels + *input_valid = cv::Mat(input_disparity.rows, input_disparity.cols, CV_8U); + + int side_bound = config_.sad_window_size / 2; + + for (size_t y_pixels = 0; y_pixels < input_disparity.rows; ++y_pixels) { + for (size_t x_pixels = 0; x_pixels < input_disparity.cols; ++x_pixels) { + // the last check is because the sky has a bad habit of having a disparity + // at just less than the max disparity + if ((x_pixels < + side_bound + config_.min_disparity + config_.num_disparities) || + (y_pixels < side_bound) || + (x_pixels > (input_disparity.cols - side_bound)) || + (y_pixels > (input_disparity.rows - side_bound)) || + (input_disparity.at(y_pixels, x_pixels) < 0) || + (input_disparity.at(y_pixels, x_pixels) >= + (config_.min_disparity + config_.num_disparities - 1) * 16)) { + input_valid->at(y_pixels, x_pixels) = 0; + } else { + input_valid->at(y_pixels, x_pixels) = 1; + } + } + } + + // erode by size of SAD window, this prevents issues with background pixels + // being given the same depth as neighboring objects in the foreground. + cv::Mat kernel = cv::getStructuringElement( + cv::MORPH_RECT, + cv::Size(config_.sad_window_size, config_.sad_window_size)); + cv::erode(*input_valid, *input_valid, kernel); + + // take a guess for the depth of the invalid pixels by scanning along the row + // and giving them the same value as the closest horizontal point. + cv::Mat disparity_filled_left, disparity_filled_right; + fillDisparityFromSide(input_disparity, *input_valid, true, + &disparity_filled_left); + fillDisparityFromSide(input_disparity, *input_valid, false, + &disparity_filled_right); + + // take the most conservative disparity of the two + *disparity_filled = cv::max(disparity_filled_left, disparity_filled_right); + + // 0 disparity is valid but cannot have a depth associated with it, because of + // this we take these points and replace them with a disparity of 1. + for (size_t y_pixels = 0; y_pixels < input_disparity.rows; ++y_pixels) { + for (size_t x_pixels = 0; x_pixels < input_disparity.cols; ++x_pixels) { + if (input_disparity.at(y_pixels, x_pixels) == 0) { + disparity_filled->at(y_pixels, x_pixels) = 1; + } + } + } +} + +void DepthGenerator::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* pointcloud, + pcl::PointCloud* freespace_pointcloud) { + pointcloud->clear(); + freespace_pointcloud->clear(); + + if (left_image.depth() != CV_8U) { + ROS_ERROR( + "Pointcloud generation is currently only supported on 8 bit images"); + return; + } + + cv::Mat disparity_filled, input_valid; + bulidFilledDisparityImage(input_disparity, &disparity_filled, &input_valid); + + int side_bound = config_.sad_window_size / 2; + + // build pointcloud + for (int y_pixels = side_bound; y_pixels < input_disparity.rows - side_bound; + ++y_pixels) { + for (int x_pixels = side_bound + config_.min_disparity + config_.num_disparities; + x_pixels < input_disparity.cols - side_bound; ++x_pixels) { + const uint8_t& is_valid = input_valid.at(y_pixels, x_pixels); + const int16_t& input_value = + input_disparity.at(y_pixels, x_pixels); + const int16_t& filled_value = + disparity_filled.at(y_pixels, x_pixels); + + bool freespace; + double disparity_value; + + // if the filled disparity is valid it must be a freespace ray + if (filled_value < std::numeric_limits::max()) { + disparity_value = static_cast(filled_value); + freespace = true; + } + // else it is a normal ray + else if (is_valid) { + disparity_value = static_cast(input_value); + freespace = false; + } else { + continue; + } + + pcl::PointXYZRGB point; + + // the 16* is needed as opencv stores disparity maps as 16 * the true + // values + point.z = (16 * focal_length * baseline) / disparity_value; + point.x = point.z * (x_pixels - cx) / focal_length; + point.y = point.z * (y_pixels - cy) / focal_length; + + if (left_image.channels() == 3) { + const cv::Vec3b& color = left_image.at(y_pixels, x_pixels); + point.b = color[0]; + point.g = color[1]; + point.r = color[2]; + } else if (left_image.channels() == 4) { + const cv::Vec4b& color = left_image.at(y_pixels, x_pixels); + point.b = color[0]; + point.g = color[1]; + point.r = color[2]; + } else { + point.b = left_image.at(y_pixels, x_pixels); + point.g = point.b; + point.r = point.b; + } + + if (freespace) { + freespace_pointcloud->push_back(point); + } else { + pointcloud->push_back(point); + } + } + } +} + +void DepthGenerator::calcDisparityImage(const cv::Mat& left_image, + const cv::Mat& right_image, + cv::Mat* disparity_ptr) const { +#if (defined(CV_VERSION_EPOCH) && CV_VERSION_EPOCH == 2) + + if (use_sgbm_) { + std::shared_ptr left_matcher = + std::make_shared(); + + left_matcher->numberOfDisparities = config_.num_disparities; + left_matcher->SADWindowSize = config_.sad_window_size; + left_matcher->preFilterCap = config_.pre_filter_cap; + left_matcher->minDisparity = config_.min_disparity; + left_matcher->uniquenessRatio = config_.uniqueness_ratio; + left_matcher->speckleRange = config_.speckle_range; + left_matcher->speckleWindowSize = config_.speckle_window_size; + left_matcher->P1 = config_.p1; + left_matcher->P2 = config_.p2; + left_matcher->disp12MaxDiff = config_.disp_12_max_diff; + left_matcher->fullDP = config_.use_mode_HH; + left_matcher->operator()(left_image, right_image, *disparity_ptr); + } else { + std::shared_ptr left_matcher = + std::make_shared(); + + left_matcher->state->numberOfDisparities = config_.num_disparities; + left_matcher->state->SADWindowSize = config_.sad_window_size; + left_matcher->state->preFilterCap = config_.pre_filter_cap; + left_matcher->state->preFilterSize = config_.pre_filter_size; + left_matcher->state->minDisparity = config_.min_disparity; + left_matcher->state->textureThreshold = config_.texture_threshold; + left_matcher->state->uniquenessRatio = config_.uniqueness_ratio; + left_matcher->state->speckleRange = config_.speckle_range; + left_matcher->state->speckleWindowSize = config_.speckle_window_size; + left_matcher->operator()(left_image, right_image, *disparity_ptr); + } +#else + if (config_.use_sgbm) { + int mode; + if (config_.use_mode_HH) { + mode = cv::StereoSGBM::MODE_HH; + } else { + mode = cv::StereoSGBM::MODE_SGBM; + } + + cv::Ptr left_matcher = cv::StereoSGBM::create( + config_.min_disparity, config_.num_disparities, config_.sad_window_size, config_.p1, config_.p2, + config_.disp_12_max_diff, config_.pre_filter_cap, config_.uniqueness_ratio, + config_.speckle_window_size, config_.speckle_range, mode); + + left_matcher->compute(left_image, right_image, *disparity_ptr); + + } else { + cv::Ptr left_matcher = + cv::StereoBM::create(config_.num_disparities, config_.sad_window_size); + + left_matcher->setPreFilterType(config_.pre_filter_type); + left_matcher->setPreFilterSize(config_.pre_filter_size); + left_matcher->setPreFilterCap(config_.pre_filter_cap); + left_matcher->setMinDisparity(config_.min_disparity); + left_matcher->setTextureThreshold(config_.texture_threshold); + left_matcher->setUniquenessRatio(config_.uniqueness_ratio); + left_matcher->setSpeckleRange(config_.speckle_range); + left_matcher->setSpeckleWindowSize(config_.speckle_window_size); + left_matcher->compute(left_image, right_image, *disparity_ptr); + } +#endif + + if (config_.do_median_blur) { + cv::medianBlur(*disparity_ptr, *disparity_ptr, 5); + } +} + +bool DepthGenerator::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) { + if (first_camera_info.height != second_camera_info.height) { + ROS_ERROR("Image heights do not match"); + return false; + } + if (first_camera_info.width != second_camera_info.width) { + ROS_ERROR("Image widths do not match"); + return false; + } + + for (double d : first_camera_info.D) { + if (!ApproxEq(d, 0)) { + ROS_ERROR("First image has non-zero distortion"); + return false; + } + } + for (double d : second_camera_info.D) { + if (!ApproxEq(d, 0)) { + ROS_ERROR("Second image has non-zero distortion"); + return false; + } + } + + for (size_t i = 0; i < 12; ++i) { + if ((i != 3) && + !ApproxEq(first_camera_info.P[i], second_camera_info.P[i])) { + ROS_ERROR("Image P matrices must match (excluding x offset)"); + return false; + } + } + + if (!ApproxEq(first_camera_info.P[1], 0) || + !ApproxEq(first_camera_info.P[4], 0)) { + ROS_ERROR("Image P matrix contains skew"); + return false; + } + + if (!ApproxEq(first_camera_info.P[0], first_camera_info.P[5])) { + ROS_ERROR("Image P matrix has different values for Fx and Fy"); + return false; + } + + if (first_camera_info.P[0] <= 0) { + ROS_ERROR("Focal length must be greater than 0"); + return false; + } + + // downgraded to warning so that the color images of the KITTI dataset can be + // processed + if (!ApproxEq(first_camera_info.P[8], 0) || + !ApproxEq(first_camera_info.P[9], 0) || + !ApproxEq(first_camera_info.P[10], 1) || + !ApproxEq(first_camera_info.P[11], 0)) { + ROS_WARN_ONCE( + "Image P matrix does not end in [0,0,1,0], these values will be " + "ignored"); + } + + // again downgraded to warning because KITTI has ugly matrices + if (!ApproxEq(first_camera_info.P[7], 0)) { + ROS_WARN_ONCE("P contains Y offset, this value will be ignored"); + } + + *focal_length = first_camera_info.P[0]; + *baseline = (second_camera_info.P[3] - first_camera_info.P[3]) / + first_camera_info.P[0]; + if (*baseline > 0) { + *first_is_left = false; + } else { + *first_is_left = true; + *baseline *= -1; + } + *cx = first_camera_info.P[2]; + *cy = first_camera_info.P[6]; + + return true; +} + +bool DepthGenerator::ApproxEq(double A, double B) { + return (std::abs(A - B) <= kDelta); +} + +} // namespace image_undistort diff --git a/src/stereo_undistort.cpp b/src/stereo_undistort.cpp index 6fb5405..38965e9 100644 --- a/src/stereo_undistort.cpp +++ b/src/stereo_undistort.cpp @@ -27,8 +27,8 @@ StereoUndistort::StereoUndistort(const ros::NodeHandle& nh, frame_counter_(0) { // set parameters from ros nh_private_.param("input_camera_info_from_ros_params", - input_camera_info_from_ros_params_, - kDefaultInputCameraInfoFromROSParams); + config_.input_camera_info_from_ros_params, + config_.input_camera_info_from_ros_params); nh_private_.param("rename_radtan_plumb_bob", rename_radtan_plumb_bob_, kDefaultRenameRadtanPlumbBob);