diff --git a/CMakeLists.txt b/CMakeLists.txt index edaedb1..3c46c38 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,20 +1,17 @@ -option(BUILD_JSK_PKGS "Enable building of required components of jsk_recognition_msgs and jsk_rviz_plugins" ON) - -if(BUILD_JSK_PKGS) - add_subdirectory(include/jsk_recognition_msgs) -# add_subdirectory(include/jsk_rviz_plugins) #TODO: allow building of rviz plugins as well -endif() - cmake_minimum_required(VERSION 3.0.2) project(patchworkpp) -add_compile_options(-std=c++17) set(CMAKE_BUILD_TYPE "Release") - -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) +option(BUILD_JSK_PKGS "Enable building of required components of jsk_recognition_msgs and jsk_rviz_plugins" ON) +if(BUILD_JSK_PKGS) + add_subdirectory(include/jsk_recognition_msgs) +# add_subdirectory(include/jsk_rviz_plugins) #TODO: allow building of rviz plugins as well +endif() + find_package(catkin REQUIRED COMPONENTS roscpp rospy @@ -27,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS laser_geometry sensor_msgs message_generation + dynamic_reconfigure jsk_recognition_msgs ) @@ -48,6 +46,10 @@ if (OPENMP_FOUND) set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") endif() +generate_dynamic_reconfigure_options( + cfg/patchwork.cfg +) + catkin_package( INCLUDE_DIRS LIBRARIES @@ -69,6 +71,10 @@ add_executable(demo src/demo.cpp) target_link_libraries(demo ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) add_dependencies(demo patchworkpp_generate_messages_cpp) +add_executable(demo_reconf src/demo_reconf.cpp) +target_link_libraries(demo_reconf ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) +add_dependencies(demo_reconf patchworkpp_generate_messages_cpp) + add_executable(video src/video.cpp) target_link_libraries(video ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) add_dependencies(video patchworkpp_generate_messages_cpp) diff --git a/README.md b/README.md index 9d34987..3bc66cf 100644 --- a/README.md +++ b/README.md @@ -57,6 +57,15 @@ $ roslaunch patchworkpp demo.launch $ rosbag play kitti_00_sample.bag ``` +or run the demo that is dynamically reconfigurable with `rqt_reconfigure` + +```bash +# Start reconfigurable Patchwork++ +$ roslaunch patchworkpp demo_reconf.launch +# Start the bag file +$ rosbag play kitti_00_sample.bag +``` + ## :pushpin: TODO List - [ ] Update additional demo codes processing data with .bin file format - [ ] Generalize point type in the source code diff --git a/cfg/patchwork.cfg b/cfg/patchwork.cfg new file mode 100644 index 0000000..d661024 --- /dev/null +++ b/cfg/patchwork.cfg @@ -0,0 +1,44 @@ +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +# save_flag +gen.add("save_flag", bool_t, 0, "Flag to save the configuration file", True) + +# patchworkpp +gen.add("sensor_height", double_t, 0, "Height of the sensor from the ground", 1.723, 0.0, 100.0) +gen.add("mode", str_t, 0, "Mode of operation", "czm") +gen.add("cloud_topic", str_t, 0, "PointCloud Topic", "/kitti/velo/pointcloud") +gen.add("verbose", bool_t, 0, "Verbose mode for debugging", False) +gen.add("visualize", bool_t, 0, "Flag to visualize the ground likelihood estimation", True) + +# Ground Plane Fitting parameters +gen.add("num_iter", int_t, 0, "Number of iterations for ground plane estimation using PCA", 3, 1, 10) +gen.add("num_lpr", int_t, 0, "Maximum number of points to be selected as lowest points representative", 20, 0, 100) +gen.add("num_min_pts", int_t, 0, "Minimum number of points to be estimated as ground plane in each patch", 10, 0, 100) +gen.add("th_seeds", double_t, 0, "Threshold for lowest point representatives using in initial seeds selection of ground points", 0.3, 0.0, 1.0) +gen.add("th_dist", double_t, 0, "Threshold for thickness of ground", 0.125, 0.0, 1.0) +gen.add("th_seeds_v", double_t, 0, "Threshold for lowest point representatives using in initial seeds selection of vertical structural points", 0.25, 0.0, 1.0) +gen.add("th_dist_v", double_t, 0, "Threshold for thickness of vertical structure", 0.1, 0.0, 1.0) +gen.add("max_r", double_t, 0, "Maximum range of ground estimation area", 80.0, 0.0, 1000.0) +gen.add("min_r", double_t, 0, "Minimum range of ground estimation area", 2.7, 0.0, 100.0) +gen.add("uprightness_thr", double_t, 0, "Threshold of uprightness using in Ground Likelihood Estimation(GLE)", 0.707, 0.0, 1.0) +gen.add("adaptive_seed_selection_margin", double_t, 0, "The points below the adaptive_seed_selection_margin * sensor_height are filtered", -1.2, -10.0, 10.0) + +# czm +gen.add("czm_num_sectors_each_zone", str_t, 0, "Number of sectors in each zone", "16, 32, 54, 32") +gen.add("czm_num_rings_each_zone", str_t, 0, "Number of rings in each zone", "2, 4, 4, 4") +gen.add("czm_elevation_thresholds", str_t, 0, "Threshold of elevation for each ring using in GLE. Those values are updated adaptively.", "0.0, 0.0, 0.0, 0.0") +gen.add("czm_flatness_thresholds", str_t, 0, "Threshold of flatness for each ring using in GLE. Those values are updated adaptively.", "0.0, 0.0, 0.0, 0.0") + +# enable/disable options +gen.add("enable_RNR", bool_t, 0, "Flag to enable/disable RNR", True) +gen.add("enable_RVPF", bool_t, 0, "Flag to enable/disable RVPF", True) +gen.add("enable_TGR", bool_t, 0, "Flag to enable/disable TGR", True) + +# RNR parameters +gen.add("RNR_ver_angle_thr", double_t, 0, "Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points.", -15.0, -90.0, 90.0) +gen.add("RNR_intensity_thr", double_t, 0, "Noise points intensity threshold. The reflected points have relatively small intensity than others.", 0.2, 0.0, 1.0) + +# Generate the .cfg file +exit(gen.generate("patchworkpp", "demo", "Patchworkpp")) \ No newline at end of file diff --git a/include/patchworkpp/params.hpp b/include/patchworkpp/params.hpp new file mode 100644 index 0000000..4598fa3 --- /dev/null +++ b/include/patchworkpp/params.hpp @@ -0,0 +1,276 @@ +#ifndef PATCHWORKPP_PARAMS +#define PATCHWORKPP_PARAMS + +#include +#include +#include +#include +#include +#include +#include + +class ParamsHandler { +public: + ParamsHandler(std::recursive_mutex& mutex) : mutex_(mutex), czm_changed_(false), topic_changed_(false), czm() { + czm.num_zones_ = 4; + auto f = boost::bind(&ParamsHandler::reconfigure_callback, this, _1, _2); + server_.setCallback(f); + } + + ParamsHandler(const ros::NodeHandle& nh, std::recursive_mutex& mutex) : mutex_(mutex), czm() { + nh.param("verbose", verbose_, false); + nh.param("visualize", visualize_, true); + nh.param("sensor_height", sensor_height_, 1.723); + nh.param("num_iter", num_iter_, 3); + nh.param("num_lpr", num_lpr_, 20); + int tmp = 0; + nh.param("num_min_pts", tmp, 10); + num_min_pts_ = tmp; + nh.param("th_seeds", th_seeds_, 0.4); + nh.param("th_dist", th_dist_, 0.3); + nh.param("th_seeds_v", th_seeds_v_, 0.4); + nh.param("th_dist_v", th_dist_v_, 0.3); + nh.param("max_r", max_range_, 80.0); + nh.param("min_r", min_range_, 2.7); + nh.param("uprightness_thr", uprightness_thr_, 0.5); + nh.param("adaptive_seed_selection_margin", adaptive_seed_selection_margin_, -1.1); + nh.param("RNR_ver_angle_thr", RNR_ver_angle_thr_, -15.0); + nh.param("RNR_intensity_thr", RNR_intensity_thr_, 0.2); + nh.param("max_flatness_storage", max_flatness_storage_, 1000); + nh.param("max_elevation_storage", max_elevation_storage_, 1000); + nh.param("enable_RNR", enable_RNR_, true); + nh.param("enable_RVPF", enable_RVPF_, true); + nh.param("enable_TGR", enable_TGR_, true); + + // CZM denotes 'Concentric Zone Model'. Please refer to our paper + std::vector temp; + nh.getParam("czm/num_sectors_each_zone", temp); + std::transform(temp.begin(), temp.end(), std::back_inserter(czm.num_sectors_each_zone_), [](int value) { return static_cast(value); }); + temp.clear(); + nh.getParam("czm/num_rings_each_zone", temp); + std::transform(temp.begin(), temp.end(), std::back_inserter(czm.num_rings_each_zone_), [](int value) { return static_cast(value); }); + nh.getParam("czm/elevation_thresholds", czm.elevation_thr_); + nh.getParam("czm/flatness_thresholds", czm.flatness_thr_); + + nh.param("cloud_topic", cloud_topic_, "/pointcloud"); + + czm.num_zones_ = 4; + params_valid_ = validate(); + if (params_valid_) { + set_ranges_rings_sectors(); + } + } + + void print_params() const { + std::unique_lock lock(mutex_); + if (not params_valid_) + { + ROS_WARN_STREAM("Can't print parameters. Not initialized"); + return; + } + + ROS_INFO("Sensor Height: %f", sensor_height_); + ROS_INFO("Cloud topic: %s", cloud_topic_.c_str()); + ROS_INFO("Num of Iteration: %d", num_iter_); + ROS_INFO("Num of LPR: %d", num_lpr_); + ROS_INFO("Num of min. points: %ld", num_min_pts_); + ROS_INFO("Seeds Threshold: %f", th_seeds_); + ROS_INFO("Distance Threshold: %f", th_dist_); + ROS_INFO("Max. range: %f", max_range_); + ROS_INFO("Min. range: %f", min_range_); + ROS_INFO("Normal vector threshold: %f", uprightness_thr_); + ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); + ROS_INFO("Num. zones: %ld", czm.num_zones_); + ROS_INFO_STREAM((boost::format("Num. sectors: %ld, %ld, %ld, %ld") % czm.num_sectors_each_zone_[0] % czm.num_sectors_each_zone_[1] % + czm.num_sectors_each_zone_[2] % + czm.num_sectors_each_zone_[3]).str()); + ROS_INFO_STREAM((boost::format("Num. rings: %01ld, %01ld, %01ld, %01ld") % czm.num_rings_each_zone_[0] % + czm.num_rings_each_zone_[1] % + czm.num_rings_each_zone_[2] % + czm.num_rings_each_zone_[3]).str()); + ROS_INFO_STREAM((boost::format("elevation_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % czm.elevation_thr_[0] % czm.elevation_thr_[1] % + czm.elevation_thr_[2] % + czm.elevation_thr_[3]).str()); + ROS_INFO_STREAM((boost::format("flatness_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % czm.flatness_thr_[0] % czm.flatness_thr_[1] % + czm.flatness_thr_[2] % + czm.flatness_thr_[3]).str()); + } + + bool validate() const { + return check(min_range_ > 0, "min range must be larger than 0") &&\ + check(min_range_ < max_range_, "min range must be smaller than max range") &&\ + check(max_range_ > 0, "max range must be larger than 0") &&\ + check(czm.num_zones_ == 4, (boost::format("Number of zones must be four! Got %d") % czm.num_zones_).str()) &&\ + check(czm.num_zones_ == czm.num_sectors_each_zone_.size(), "num_zones (4) and length of num_sectors_each_zone must match") &&\ + check(czm.num_zones_ == czm.num_rings_each_zone_.size(), "num_zones and length of num_rings_each_zone must match") &&\ + check(czm.num_zones_ == czm.elevation_thr_.size(), "num_zones and length of elevation_thresholds must match") &&\ + check(czm.num_zones_ == czm.flatness_thr_.size(), "num_zones and length of flatness_thresholds must match"); + } + + bool czm_changed() { + std::unique_lock lock(mutex_); + auto result = czm_changed_; + czm_changed_ = false; + return result; + } + + std::pair topic_changed() { + std::unique_lock lock(mutex_); + auto result = topic_changed_; + topic_changed_ = false; + return { result, cloud_topic_ }; + } + + std::string topic() { + std::unique_lock lock(mutex_); + return cloud_topic_; + } + + std::string mode_; + std::string cloud_topic_; + bool params_valid_; + bool verbose_; + bool visualize_; + double sensor_height_; + int num_iter_; + int num_lpr_; + size_t num_min_pts_; + double th_seeds_; + double th_dist_; + double th_seeds_v_; + double th_dist_v_; + double max_range_; + double min_range_; + double uprightness_thr_; + double adaptive_seed_selection_margin_; + double RNR_ver_angle_thr_; + double RNR_intensity_thr_; + int max_flatness_storage_; + int max_elevation_storage_; + bool enable_RNR_; + bool enable_RVPF_; + bool enable_TGR_; + size_t num_rings_of_interest_; + std::vector min_ranges_; + std::vector ring_sizes_; + std::vector sector_sizes_; + + struct CZM + { + size_t num_zones_; + std::vector num_sectors_each_zone_; + std::vector num_rings_each_zone_; + std::vector elevation_thr_; + std::vector flatness_thr_; + } czm; + +private: + void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level) { + std::unique_lock lock(mutex_); + verbose_ = config.verbose; + visualize_ = config.visualize; + sensor_height_ = config.sensor_height; + num_iter_ = config.num_iter; + num_lpr_ = config.num_lpr; + num_min_pts_ = config.num_min_pts; + th_seeds_ = config.th_seeds; + th_dist_ = config.th_dist; + th_seeds_v_ = config.th_seeds_v; + th_dist_v_ = config.th_dist_v; + uprightness_thr_ = config.uprightness_thr; + adaptive_seed_selection_margin_ = config.adaptive_seed_selection_margin; + RNR_ver_angle_thr_ = config.RNR_ver_angle_thr; + RNR_intensity_thr_ = config.RNR_intensity_thr; + enable_RNR_ = config.enable_RNR; + enable_RVPF_ = config.enable_RVPF; + enable_TGR_ = config.enable_TGR; + min_range_ = config.min_r; + max_range_ = config.max_r; + + if (cloud_topic_ != config.cloud_topic) { + topic_changed_ = true; + } + cloud_topic_ = config.cloud_topic; + + auto num_sectors_each_zone = convert_string_to_vector(config.czm_num_sectors_each_zone); + auto num_rings_each_zone = convert_string_to_vector(config.czm_num_rings_each_zone); + auto elevation_thr = convert_string_to_vector(config.czm_elevation_thresholds); + auto flatness_thr = convert_string_to_vector(config.czm_flatness_thresholds);; + if (czm.num_sectors_each_zone_ != num_sectors_each_zone || \ + czm.num_rings_each_zone_ != num_rings_each_zone || \ + czm.elevation_thr_ != elevation_thr || \ + czm.flatness_thr_ != flatness_thr) { + czm_changed_ = true; + } + + czm.num_sectors_each_zone_ = num_sectors_each_zone; + czm.num_rings_each_zone_ = num_rings_each_zone; + czm.elevation_thr_ = elevation_thr; + czm.flatness_thr_ = flatness_thr; + + params_valid_ = validate(); + if (params_valid_) + { + set_ranges_rings_sectors(); + ROS_INFO("Updated params"); + } else { + ROS_WARN("Parameter update failed"); + } + } + + + template + std::vector convert_string_to_vector(const std::string& str, char separator = ',') { + std::vector result; + std::istringstream iss(str); + std::string token; + + while (std::getline(iss, token, separator)) { + token = std::regex_replace(token, std::regex("\\s+"), ""); + T num; + std::stringstream stream(token); + stream >> num; + if (stream.fail()) { + ROS_WARN_STREAM("Can't convert " << token << " to number"); + return result; + } + result.push_back(num); + } + + return result; + } + + void set_ranges_rings_sectors() { + num_rings_of_interest_ = czm.elevation_thr_.size(); + + auto min_range_z2 = (7 * min_range_ + max_range_) / 8.0; + auto min_range_z3 = (3 * min_range_ + max_range_) / 4.0; + auto min_range_z4 = (min_range_ + max_range_) / 2.0; + + min_ranges_ = {min_range_, min_range_z2, min_range_z3, min_range_z4}; + ring_sizes_ = {(min_range_z2 - min_range_) / czm.num_rings_each_zone_.at(0), + (min_range_z3 - min_range_z2) / czm.num_rings_each_zone_.at(1), + (min_range_z4 - min_range_z3) / czm.num_rings_each_zone_.at(2), + (max_range_ - min_range_z4) / czm.num_rings_each_zone_.at(3)}; + sector_sizes_ = {2 * M_PI / czm.num_sectors_each_zone_.at(0), 2 * M_PI / czm.num_sectors_each_zone_.at(1), + 2 * M_PI / czm.num_sectors_each_zone_.at(2), + 2 * M_PI / czm.num_sectors_each_zone_.at(3)}; + } + + + bool check(bool assertion, std::string description) const { + if (not assertion) { + ROS_WARN_STREAM(description); + return false; + } + + return true; + } + + bool czm_changed_; + bool topic_changed_; + std::recursive_mutex& mutex_; + dynamic_reconfigure::Server server_; +}; + +#endif \ No newline at end of file diff --git a/include/patchworkpp/patchworkpp.hpp b/include/patchworkpp/patchworkpp.hpp index f45e249..cbf3a50 100755 --- a/include/patchworkpp/patchworkpp.hpp +++ b/include/patchworkpp/patchworkpp.hpp @@ -21,6 +21,7 @@ #include #include +#include #define MARKER_Z_VALUE -2.2 #define UPRIGHT_ENOUGH 0.55 @@ -45,7 +46,7 @@ bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; } template struct RevertCandidate { - int concentric_idx; + size_t concentric_idx; int sector_idx; double ground_flatness; double line_variable; @@ -63,119 +64,54 @@ class PatchWorkpp { typedef std::vector> Ring; typedef std::vector Zone; - PatchWorkpp() {}; - - PatchWorkpp(ros::NodeHandle *nh) : node_handle_(*nh) { - // Init ROS related - ROS_INFO("Inititalizing PatchWork++..."); - - node_handle_.param("verbose", verbose_, false); - - node_handle_.param("sensor_height", sensor_height_, 1.723); - node_handle_.param("num_iter", num_iter_, 3); - node_handle_.param("num_lpr", num_lpr_, 20); - node_handle_.param("num_min_pts", num_min_pts_, 10); - node_handle_.param("th_seeds", th_seeds_, 0.4); - node_handle_.param("th_dist", th_dist_, 0.3); - node_handle_.param("th_seeds_v", th_seeds_v_, 0.4); - node_handle_.param("th_dist_v", th_dist_v_, 0.3); - node_handle_.param("max_r", max_range_, 80.0); - node_handle_.param("min_r", min_range_, 2.7); - node_handle_.param("uprightness_thr", uprightness_thr_, 0.5); - node_handle_.param("adaptive_seed_selection_margin", adaptive_seed_selection_margin_, -1.1); - node_handle_.param("RNR_ver_angle_thr", RNR_ver_angle_thr_, -15.0); - node_handle_.param("RNR_intensity_thr", RNR_intensity_thr_, 0.2); - node_handle_.param("max_flatness_storage", max_flatness_storage_, 1000); - node_handle_.param("max_elevation_storage", max_elevation_storage_, 1000); - node_handle_.param("enable_RNR", enable_RNR_, true); - node_handle_.param("enable_RVPF", enable_RVPF_, true); - node_handle_.param("enable_TGR", enable_TGR_, true); - - ROS_INFO("Sensor Height: %f", sensor_height_); - ROS_INFO("Num of Iteration: %d", num_iter_); - ROS_INFO("Num of LPR: %d", num_lpr_); - ROS_INFO("Num of min. points: %d", num_min_pts_); - ROS_INFO("Seeds Threshold: %f", th_seeds_); - ROS_INFO("Distance Threshold: %f", th_dist_); - ROS_INFO("Max. range:: %f", max_range_); - ROS_INFO("Min. range:: %f", min_range_); - ROS_INFO("Normal vector threshold: %f", uprightness_thr_); - ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); - - // CZM denotes 'Concentric Zone Model'. Please refer to our paper - node_handle_.getParam("czm/num_zones", num_zones_); - node_handle_.getParam("czm/num_sectors_each_zone", num_sectors_each_zone_); - node_handle_.getParam("czm/mum_rings_each_zone", num_rings_each_zone_); - node_handle_.getParam("czm/elevation_thresholds", elevation_thr_); - node_handle_.getParam("czm/flatness_thresholds", flatness_thr_); - - ROS_INFO("Num. zones: %d", num_zones_); - - if (num_zones_ != 4 || num_sectors_each_zone_.size() != num_rings_each_zone_.size()) { - throw invalid_argument("Some parameters are wrong! Check the num_zones and num_rings/sectors_each_zone"); - } - if (elevation_thr_.size() != flatness_thr_.size()) { - throw invalid_argument("Some parameters are wrong! Check the elevation/flatness_thresholds"); + PatchWorkpp() : mutex_(), params_(mutex_), using_reconf_(true) { + plane_viz_ = node_handle_.advertise("plane", 100, true); + pub_revert_pc_ = node_handle_.advertise("revert_pc", 100, true); + pub_reject_pc_ = node_handle_.advertise("reject_pc", 100, true); + pub_normal_ = node_handle_.advertise("normals", 100, true); + pub_noise_ = node_handle_.advertise("noise", 100, true); + pub_vertical_ = node_handle_.advertise("vertical", 100, true); + + revert_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); + + if (params_.params_valid_) + { + params_.print_params(); + ROS_INFO("INITIALIZATION COMPLETE"); + reset_poly_list(); + reset_concentric_zone_model(); } + } + + PatchWorkpp(const ros::NodeHandle &nh) : node_handle_(nh), mutex_(), params_(nh, mutex_), using_reconf_(false) { + params_.print_params(); - cout << (boost::format("Num. sectors: %d, %d, %d, %d") % num_sectors_each_zone_[0] % num_sectors_each_zone_[1] % - num_sectors_each_zone_[2] % - num_sectors_each_zone_[3]).str() << endl; - cout << (boost::format("Num. rings: %01d, %01d, %01d, %01d") % num_rings_each_zone_[0] % - num_rings_each_zone_[1] % - num_rings_each_zone_[2] % - num_rings_each_zone_[3]).str() << endl; - cout << (boost::format("elevation_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % elevation_thr_[0] % elevation_thr_[1] % - elevation_thr_[2] % - elevation_thr_[3]).str() << endl; - cout << (boost::format("flatness_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % flatness_thr_[0] % flatness_thr_[1] % - flatness_thr_[2] % - flatness_thr_[3]).str() << endl; - num_rings_of_interest_ = elevation_thr_.size(); - - node_handle_.param("visualize", visualize_, true); - - int num_polygons = std::inner_product(num_rings_each_zone_.begin(), num_rings_each_zone_.end(), num_sectors_each_zone_.begin(), 0); - poly_list_.header.frame_id = "map"; - poly_list_.polygons.reserve(num_polygons); + plane_viz_ = node_handle_.advertise("plane", 100, true); + pub_revert_pc_ = node_handle_.advertise("revert_pc", 100, true); + pub_reject_pc_ = node_handle_.advertise("reject_pc", 100, true); + pub_normal_ = node_handle_.advertise("normals", 100, true); + pub_noise_ = node_handle_.advertise("noise", 100, true); + pub_vertical_ = node_handle_.advertise("vertical", 100, true); revert_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - PlaneViz = node_handle_.advertise("plane", 100, true); - pub_revert_pc = node_handle_.advertise("revert_pc", 100, true); - pub_reject_pc = node_handle_.advertise("reject_pc", 100, true); - pub_normal = node_handle_.advertise("normals", 100, true); - pub_noise = node_handle_.advertise("noise", 100, true); - pub_vertical = node_handle_.advertise("vertical", 100, true); - - min_range_z2_ = (7 * min_range_ + max_range_) / 8.0; - min_range_z3_ = (3 * min_range_ + max_range_) / 4.0; - min_range_z4_ = (min_range_ + max_range_) / 2.0; - - min_ranges_ = {min_range_, min_range_z2_, min_range_z3_, min_range_z4_}; - ring_sizes_ = {(min_range_z2_ - min_range_) / num_rings_each_zone_.at(0), - (min_range_z3_ - min_range_z2_) / num_rings_each_zone_.at(1), - (min_range_z4_ - min_range_z3_) / num_rings_each_zone_.at(2), - (max_range_ - min_range_z4_) / num_rings_each_zone_.at(3)}; - sector_sizes_ = {2 * M_PI / num_sectors_each_zone_.at(0), 2 * M_PI / num_sectors_each_zone_.at(1), - 2 * M_PI / num_sectors_each_zone_.at(2), - 2 * M_PI / num_sectors_each_zone_.at(3)}; - - cout << "INITIALIZATION COMPLETE" << endl; - - for (int i = 0; i < num_zones_; i++) { - Zone z; - initialize_zone(z, num_sectors_each_zone_[i], num_rings_each_zone_[i]); - ConcentricZoneModel_.push_back(z); - } + reset_poly_list(); + reset_concentric_zone_model(); + + ROS_INFO("INITIALIZATION COMPLETE"); } - void estimate_ground(pcl::PointCloud cloud_in, + bool estimate_ground(pcl::PointCloud cloud_in, pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, double &time_taken); + std::pair topic_changed(); + std::string topic(); private: // Every private member variable is written with the undescore("_") in its end. @@ -184,33 +120,10 @@ class PatchWorkpp { std::recursive_mutex mutex_; - int num_iter_; - int num_lpr_; - int num_min_pts_; - int num_zones_; - int num_rings_of_interest_; - - double sensor_height_; - double th_seeds_; - double th_dist_; - double th_seeds_v_; - double th_dist_v_; - double max_range_; - double min_range_; - double uprightness_thr_; - double adaptive_seed_selection_margin_; - double min_range_z2_; // 12.3625 - double min_range_z3_; // 22.025 - double min_range_z4_; // 41.35 - double RNR_ver_angle_thr_; - double RNR_intensity_thr_; - - bool verbose_; - bool enable_RNR_; - bool enable_RVPF_; - bool enable_TGR_; - - int max_flatness_storage_, max_elevation_storage_; + ParamsHandler params_; + + bool using_reconf_; + std::vector update_flatness_[4]; std::vector update_elevation_[4]; @@ -222,25 +135,13 @@ class PatchWorkpp { Eigen::Matrix3f cov_; Eigen::Vector4f pc_mean_; - // For visualization - bool visualize_; - - vector num_sectors_each_zone_; - vector num_rings_each_zone_; - - vector sector_sizes_; - vector ring_sizes_; - vector min_ranges_; - vector elevation_thr_; - vector flatness_thr_; - - queue noise_idxs_; + queue noise_idxs_; vector ConcentricZoneModel_; jsk_recognition_msgs::PolygonArray poly_list_; - ros::Publisher PlaneViz, pub_revert_pc, pub_reject_pc, pub_normal, pub_noise, pub_vertical; + ros::Publisher plane_viz_, pub_revert_pc_, pub_reject_pc_, pub_normal_, pub_noise_, pub_vertical_; pcl::PointCloud revert_pc_, reject_pc_, noise_pc_, vertical_pc_; pcl::PointCloud ground_pc_; @@ -248,7 +149,11 @@ class PatchWorkpp { pcl::PointCloud regionwise_ground_, regionwise_nonground_; + void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level); + void initialize_zone(Zone &z, int num_sectors, int num_rings); + void reset_poly_list(); + void reset_concentric_zone_model(); void flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings); void flush_patches(std::vector &czm); @@ -259,7 +164,7 @@ class PatchWorkpp { void temporal_ground_revert(pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, std::vector ring_flatness, std::vector> candidates, - int concentric_idx); + size_t concentric_idx); void calc_mean_stdev(std::vector vec, double &mean, double &stdev); @@ -292,7 +197,7 @@ class PatchWorkpp { void set_ground_likelihood_estimation_status( const int zone_idx, const int ring_idx, - const int concentric_idx, + const size_t concentric_idx, const double z_vec, const double z_elevation, const double ground_flatness); @@ -324,15 +229,33 @@ void PatchWorkpp::flush_patches_in_zone(Zone &patches, int num_sectors, template inline void PatchWorkpp::flush_patches(vector &czm) { - for (int k = 0; k < num_zones_; k++) { - for (int i = 0; i < num_rings_each_zone_[k]; i++) { - for (int j = 0; j < num_sectors_each_zone_[k]; j++) { + for (size_t k = 0; k < params_.czm.num_zones_; k++) { + for (size_t i = 0; i < params_.czm.num_rings_each_zone_[k]; i++) { + for (size_t j = 0; j < params_.czm.num_sectors_each_zone_[k]; j++) { if (!czm[k][i][j].points.empty()) czm[k][i][j].points.clear(); } } } - if( verbose_ ) cout << "Flushed patches" << endl; + if( params_ .verbose_ ) cout << "Flushed patches" << endl; +} + +template inline +void PatchWorkpp::reset_poly_list() { + int num_polygons = std::inner_product(params_.czm.num_rings_each_zone_.begin(), params_.czm.num_rings_each_zone_.end(), params_.czm.num_sectors_each_zone_.begin(), 0); + poly_list_.header.frame_id = "map"; + poly_list_.polygons.clear(); + poly_list_.polygons.reserve(num_polygons); +} + +template inline +void PatchWorkpp::reset_concentric_zone_model() { + ConcentricZoneModel_.clear(); + for (size_t i = 0; i < params_.czm.num_zones_; i++) { + Zone z; + initialize_zone(z, params_.czm.num_sectors_each_zone_[i], params_.czm.num_rings_each_zone_[i]); + ConcentricZoneModel_.push_back(z); + } } template inline @@ -364,10 +287,10 @@ void PatchWorkpp::extract_initial_seeds( double sum = 0; int cnt = 0; - int init_idx = 0; + size_t init_idx = 0; if (zone_idx == 0) { - for (int i = 0; i < p_sorted.points.size(); i++) { - if (p_sorted.points[i].z < adaptive_seed_selection_margin_ * sensor_height_) { + for (size_t i = 0; i < p_sorted.points.size(); i++) { + if (p_sorted.points[i].z < params_.adaptive_seed_selection_margin_ * params_.sensor_height_) { ++init_idx; } else { break; @@ -376,14 +299,14 @@ void PatchWorkpp::extract_initial_seeds( } // Calculate the mean height value. - for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { + for (size_t i = init_idx; i < p_sorted.points.size() && cnt < params_.num_lpr_; i++) { sum += p_sorted.points[i].z; cnt++; } double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 - // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ - for (int i = 0; i < p_sorted.points.size(); i++) { + // iterate pointcloud, filter those height is less than lpr.height+params_.th_seeds_ + for (size_t i = 0; i < p_sorted.points.size(); i++) { if (p_sorted.points[i].z < lpr_height + th_seed) { init_seeds.points.push_back(p_sorted.points[i]); } @@ -402,8 +325,8 @@ void PatchWorkpp::extract_initial_seeds( int init_idx = 0; if (zone_idx == 0) { - for (int i = 0; i < p_sorted.points.size(); i++) { - if (p_sorted.points[i].z < adaptive_seed_selection_margin_ * sensor_height_) { + for (size_t i = 0; i < p_sorted.points.size(); i++) { + if (p_sorted.points[i].z < params_.adaptive_seed_selection_margin_ * params_.sensor_height_) { ++init_idx; } else { break; @@ -412,15 +335,15 @@ void PatchWorkpp::extract_initial_seeds( } // Calculate the mean height value. - for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { + for (size_t i = init_idx; i < p_sorted.points.size() && cnt < params_.num_lpr_; i++) { sum += p_sorted.points[i].z; cnt++; } double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 - // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ - for (int i = 0; i < p_sorted.points.size(); i++) { - if (p_sorted.points[i].z < lpr_height + th_seeds_) { + // iterate pointcloud, filter those height is less than lpr.height+params_.th_seeds_ + for (size_t i = 0; i < p_sorted.points.size(); i++) { + if (p_sorted.points[i].z < lpr_height + params_.th_seeds_) { init_seeds.points.push_back(p_sorted.points[i]); } } @@ -429,13 +352,13 @@ void PatchWorkpp::extract_initial_seeds( template inline void PatchWorkpp::reflected_noise_removal(pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_nonground) { - for (int i=0; i::reflected_noise_removal(pcl::PointCloud &cloud } } - if (verbose_) cout << "[ RNR ] Num of noises : " << noise_pc_.points.size() << endl; + if (params_ .verbose_) cout << "[ RNR ] Num of noises : " << noise_pc_.points.size() << endl; +} + +template inline +std::pair PatchWorkpp::topic_changed() { + return params_.topic_changed(); +} + +template inline +std::string PatchWorkpp::topic() { + return params_.topic(); } /* @@ -453,20 +386,31 @@ void PatchWorkpp::reflected_noise_removal(pcl::PointCloud &cloud */ template inline -void PatchWorkpp::estimate_ground( +bool PatchWorkpp::estimate_ground( pcl::PointCloud cloud_in, pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, double &time_taken) { - + unique_lock lock(mutex_); + + if (not params_.params_valid_) { + ROS_WARN_STREAM_THROTTLE(1, "Invalid Parameters..."); + return false; + } + + if (using_reconf_ && params_.czm_changed()) { + ROS_WARN_STREAM("Resetting poly list and concentric zone model"); + reset_poly_list(); + reset_concentric_zone_model(); + } poly_list_.header.stamp = ros::Time::now(); poly_list_.header.frame_id = cloud_in.header.frame_id; if (!poly_list_.polygons.empty()) poly_list_.polygons.clear(); if (!poly_list_.likelihood.empty()) poly_list_.likelihood.clear(); - static double start, t0, t1, t2, end; + static double start, t1, t2, end; double pca_time_ = 0.0; double t_revert = 0.0; @@ -479,7 +423,7 @@ void PatchWorkpp::estimate_ground( cloud_nonground.clear(); // 1. Reflected Noise Removal (RNR) - if (enable_RNR_) reflected_noise_removal(cloud_in, cloud_nonground); + if (params_.enable_RNR_) reflected_noise_removal(cloud_in, cloud_nonground); t1 = ros::Time::now().toSec(); @@ -489,21 +433,21 @@ void PatchWorkpp::estimate_ground( t2 = ros::Time::now().toSec(); - int concentric_idx = 0; + size_t concentric_idx = 0; double t_sort = 0; std::vector> candidates; std::vector ringwise_flatness; - for (int zone_idx = 0; zone_idx < num_zones_; ++zone_idx) { + for (size_t zone_idx = 0; zone_idx < params_.czm.num_zones_; ++zone_idx) { auto zone = ConcentricZoneModel_[zone_idx]; - for (int ring_idx = 0; ring_idx < num_rings_each_zone_[zone_idx]; ++ring_idx) { - for (int sector_idx = 0; sector_idx < num_sectors_each_zone_[zone_idx]; ++sector_idx) { + for (size_t ring_idx = 0; ring_idx < params_.czm.num_rings_each_zone_[zone_idx]; ++ring_idx) { + for (size_t sector_idx = 0; sector_idx < params_.czm.num_sectors_each_zone_[zone_idx]; ++sector_idx) { - if (zone[ring_idx][sector_idx].points.size() < num_min_pts_) + if (zone[ring_idx][sector_idx].points.size() < params_.num_min_pts_) { cloud_nonground += zone[ring_idx][sector_idx]; continue; @@ -533,9 +477,11 @@ void PatchWorkpp::estimate_ground( const double line_variable = singular_values_(1) != 0 ? singular_values_(0)/singular_values_(1) : std::numeric_limits::max(); double heading = 0.0; - for(int i=0; i<3; i++) heading += pc_mean_(i,0)*normal_(i); + for (size_t i = 0; i < 3; i++) { + heading += pc_mean_(i,0)*normal_(i); + } - if (visualize_) { + if (params_.visualize_) { auto polygons = set_polygons(zone_idx, ring_idx, sector_idx, 3); polygons.header = poly_list_.header; poly_list_.polygons.push_back(polygons); @@ -563,10 +509,10 @@ void PatchWorkpp::estimate_ground( Therefore, we only check this value when concentric_idx < num_rings_of_interest ( near condition ) */ - bool is_upright = ground_uprightness > uprightness_thr_; - bool is_not_elevated = ground_elevation < elevation_thr_[concentric_idx]; - bool is_flat = ground_flatness < flatness_thr_[concentric_idx]; - bool is_near_zone = concentric_idx < num_rings_of_interest_; + bool is_upright = ground_uprightness > params_.uprightness_thr_; + bool is_not_elevated = ground_elevation < params_.czm.elevation_thr_[concentric_idx]; + bool is_flat = ground_flatness < params_.czm.flatness_thr_[concentric_idx]; + bool is_near_zone = concentric_idx < params_.num_rings_of_interest_; bool is_heading_outside = heading < 0.0; /* @@ -615,13 +561,13 @@ void PatchWorkpp::estimate_ground( if (!candidates.empty()) { - if (enable_TGR_) + if (params_.enable_TGR_) { temporal_ground_revert(cloud_ground, cloud_nonground, ringwise_flatness, candidates, concentric_idx); } else { - for (size_t i=0; i::estimate_ground( } } - double t_update = ros::Time::now().toSec(); - update_elevation_thr(); update_flatness_thr(); @@ -654,38 +598,38 @@ void PatchWorkpp::estimate_ground( // cout << "Time taken to Revert: " << t_revert << endl; // cout << "Time taken to update : " << end - t_update << endl; - if (visualize_) + if (params_.visualize_) { sensor_msgs::PointCloud2 cloud_ROS; pcl::toROSMsg(revert_pc_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); cloud_ROS.header.frame_id = cloud_in.header.frame_id; - pub_revert_pc.publish(cloud_ROS); + pub_revert_pc_.publish(cloud_ROS); pcl::toROSMsg(reject_pc_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); cloud_ROS.header.frame_id = cloud_in.header.frame_id; - pub_reject_pc.publish(cloud_ROS); + pub_reject_pc_.publish(cloud_ROS); pcl::toROSMsg(normals_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); cloud_ROS.header.frame_id = cloud_in.header.frame_id; - pub_normal.publish(cloud_ROS); + pub_normal_.publish(cloud_ROS); pcl::toROSMsg(noise_pc_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); cloud_ROS.header.frame_id = cloud_in.header.frame_id; - pub_noise.publish(cloud_ROS); + pub_noise_.publish(cloud_ROS); pcl::toROSMsg(vertical_pc_, cloud_ROS); cloud_ROS.header.stamp = ros::Time::now(); cloud_ROS.header.frame_id = cloud_in.header.frame_id; - pub_vertical.publish(cloud_ROS); + pub_vertical_.publish(cloud_ROS); } - if(visualize_) + if(params_.visualize_) { - PlaneViz.publish(poly_list_); + plane_viz_.publish(poly_list_); } revert_pc_.clear(); @@ -693,34 +637,36 @@ void PatchWorkpp::estimate_ground( normals_.clear(); noise_pc_.clear(); vertical_pc_.clear(); + + return true; } template inline void PatchWorkpp::update_elevation_thr(void) { - for (int i=0; i 0) update_elevation_[i].erase(update_elevation_[i].begin(), update_elevation_[i].begin() + exceed_num); } - if (verbose_) + if (params_ .verbose_) { - cout << "sensor height: " << sensor_height_ << endl; - cout << (boost::format("elevation_thr_ : %0.4f, %0.4f, %0.4f, %0.4f") - % elevation_thr_[0] % elevation_thr_[1] % elevation_thr_[2] % elevation_thr_[3]).str() << endl; + cout << "sensor height: " << params_.sensor_height_ << endl; + cout << (boost::format("params_.czm.elevation_thr_ : %0.4f, %0.4f, %0.4f, %0.4f") + % params_.czm.elevation_thr_[0] % params_.czm.elevation_thr_[1] % params_.czm.elevation_thr_[2] % params_.czm.elevation_thr_[3]).str() << endl; } return; @@ -729,25 +675,25 @@ void PatchWorkpp::update_elevation_thr(void) template inline void PatchWorkpp::update_flatness_thr(void) { - for (int i=0; i 0) update_flatness_[i].erase(update_flatness_[i].begin(), update_flatness_[i].begin() + exceed_num); } - if (verbose_) + if (params_ .verbose_) { - cout << (boost::format("flatness_thr_ : %0.4f, %0.4f, %0.4f, %0.4f") - % flatness_thr_[0] % flatness_thr_[1] % flatness_thr_[2] % flatness_thr_[3]).str() << endl; + cout << (boost::format("params_.czm.flatness_thr_ : %0.4f, %0.4f, %0.4f, %0.4f") + % params_.czm.flatness_thr_[0] % params_.czm.flatness_thr_[1] % params_.czm.flatness_thr_[2] % params_.czm.flatness_thr_[3]).str() << endl; } return; @@ -756,14 +702,14 @@ void PatchWorkpp::update_flatness_thr(void) template inline void PatchWorkpp::temporal_ground_revert(pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, std::vector ring_flatness, std::vector> candidates, - int concentric_idx) + size_t concentric_idx) { - if (verbose_) std::cout << "\033[1;34m" << "=========== Temporal Ground Revert (TGR) ===========" << "\033[0m" << endl; + if (params_ .verbose_) std::cout << "\033[1;34m" << "=========== Temporal Ground Revert (TGR) ===========" << "\033[0m" << endl; double mean_flatness = 0.0, stdev_flatness = 0.0; calc_mean_stdev(ring_flatness, mean_flatness, stdev_flatness); - if (verbose_) + if (params_ .verbose_) { cout << "[" << candidates[0].concentric_idx << ", " << candidates[0].sector_idx << "]" << " mean_flatness: " << mean_flatness << ", stdev_flatness: " << stdev_flatness << std::endl; @@ -774,7 +720,7 @@ void PatchWorkpp::temporal_ground_revert(pcl::PointCloud &cloud_ RevertCandidate candidate = candidates[i]; // Debug - if(verbose_) + if(params_ .verbose_) { cout << "\033[1;33m" << candidate.sector_idx << "th flat_sector_candidate" << " / flatness: " << candidate.ground_flatness @@ -786,22 +732,22 @@ void PatchWorkpp::temporal_ground_revert(pcl::PointCloud &cloud_ double mu_flatness = mean_flatness + 1.5*stdev_flatness; double prob_flatness = 1/(1+exp( (candidate.ground_flatness-mu_flatness)/(mu_flatness/10) )); - if (candidate.regionwise_ground.size() > 1500 && candidate.ground_flatness < th_dist_*th_dist_) prob_flatness = 1.0; + if (candidate.regionwise_ground.size() > 1500 && candidate.ground_flatness < params_.th_dist_* params_.th_dist_) prob_flatness = 1.0; double prob_line = 1.0; - if (candidate.line_variable > 8.0 )//&& candidate.line_dir > M_PI/4)// candidate.ground_elevation > elevation_thr_[concentric_idx]) + if (candidate.line_variable > 8.0 )//&& candidate.line_dir > M_PI/4)// candidate.ground_elevation > params_.czm.elevation_thr_[concentric_idx]) { - // if (verbose_) cout << "line_dir: " << candidate.line_dir << endl; + // if (params_ .verbose_) cout << "line_dir: " << candidate.line_dir << endl; prob_line = 0.0; } bool revert = prob_line*prob_flatness > 0.5; - if ( concentric_idx < num_rings_of_interest_ ) + if ( concentric_idx < params_.num_rings_of_interest_ ) { if (revert) { - if (verbose_) + if (params_ .verbose_) { cout << "\033[1;32m" << "REVERT TRUE" << "\033[0m" << endl; } @@ -811,7 +757,7 @@ void PatchWorkpp::temporal_ground_revert(pcl::PointCloud &cloud_ } else { - if (verbose_) + if (params_.verbose_) { cout << "\033[1;31m" << "FINAL REJECT" << "\033[0m" << endl; } @@ -821,7 +767,7 @@ void PatchWorkpp::temporal_ground_revert(pcl::PointCloud &cloud_ } } - if (verbose_) std::cout << "\033[1;34m" << "====================================================" << "\033[0m" << endl; + if (params_ .verbose_) std::cout << "\033[1;34m" << "====================================================" << "\033[0m" << endl; } // For adaptive @@ -841,14 +787,14 @@ void PatchWorkpp::extract_piecewiseground( pcl::PointCloud src_wo_verticals; src_wo_verticals = src; - if (enable_RVPF_) + if (params_.enable_RVPF_) { - for (int i = 0; i < num_iter_; i++) + for (int i = 0; i < params_.num_iter_; i++) { - extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_, th_seeds_v_); + extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_, params_.th_seeds_v_); estimate_plane(ground_pc_); - if (zone_idx == 0 && normal_(2) < uprightness_thr_) + if (zone_idx == 0 && normal_(2) < params_.uprightness_thr_) { pcl::PointCloud src_tmp; src_tmp = src_wo_verticals; @@ -863,7 +809,7 @@ void PatchWorkpp::extract_piecewiseground( Eigen::VectorXf result = points * normal_; for (int r = 0; r < result.rows(); r++) { - if (result[r] < th_dist_v_ - d_ && result[r] > -th_dist_v_ - d_) { + if (result[r] < params_.th_dist_v_ - d_ && result[r] > -params_.th_dist_v_ - d_) { non_ground_dst.points.push_back(src_tmp[r]); vertical_pc_.points.push_back(src_tmp[r]); } else { @@ -888,7 +834,7 @@ void PatchWorkpp::extract_piecewiseground( points.row(j++) << p.x, p.y, p.z; } - for (int i = 0; i < num_iter_; i++) { + for (int i = 0; i < params_.num_iter_; i++) { ground_pc_.clear(); @@ -896,12 +842,12 @@ void PatchWorkpp::extract_piecewiseground( Eigen::VectorXf result = points * normal_; // threshold filter for (int r = 0; r < result.rows(); r++) { - if (i < num_iter_ - 1) { - if (result[r] < th_dist_ - d_ ) { + if (i < params_.num_iter_ - 1) { + if (result[r] < params_.th_dist_ - d_ ) { ground_pc_.points.push_back(src_wo_verticals[r]); } } else { // Final stage - if (result[r] < th_dist_ - d_ ) { + if (result[r] < params_.th_dist_ - d_ ) { dst.points.push_back(src_wo_verticals[r]); } else { non_ground_dst.points.push_back(src_wo_verticals[r]); @@ -909,7 +855,7 @@ void PatchWorkpp::extract_piecewiseground( } } - if (i < num_iter_ -1) estimate_plane(ground_pc_); + if (i < params_.num_iter_ -1) estimate_plane(ground_pc_); else estimate_plane(dst); } } @@ -921,16 +867,16 @@ geometry_msgs::PolygonStamped PatchWorkpp::set_polygons(int zone_idx, in geometry_msgs::Point32 point; // RL - double zone_min_range = min_ranges_[zone_idx]; - double r_len = r_idx * ring_sizes_[zone_idx] + zone_min_range; - double angle = theta_idx * sector_sizes_[zone_idx]; + double zone_min_range = params_.min_ranges_[zone_idx]; + double r_len = r_idx * params_.ring_sizes_[zone_idx] + zone_min_range; + double angle = theta_idx * params_.sector_sizes_[zone_idx]; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); // RU - r_len = r_len + ring_sizes_[zone_idx]; + r_len = r_len + params_.ring_sizes_[zone_idx]; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; @@ -938,21 +884,21 @@ geometry_msgs::PolygonStamped PatchWorkpp::set_polygons(int zone_idx, in // RU -> LU for (int idx = 1; idx <= num_split; ++idx) { - angle = angle + sector_sizes_[zone_idx] / num_split; + angle = angle + params_.sector_sizes_[zone_idx] / num_split; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); } - r_len = r_len - ring_sizes_[zone_idx]; + r_len = r_len - params_.ring_sizes_[zone_idx]; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); for (int idx = 1; idx < num_split; ++idx) { - angle = angle - sector_sizes_[zone_idx] / num_split; + angle = angle - params_.sector_sizes_[zone_idx] / num_split; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; @@ -965,14 +911,14 @@ geometry_msgs::PolygonStamped PatchWorkpp::set_polygons(int zone_idx, in template inline void PatchWorkpp::set_ground_likelihood_estimation_status( const int zone_idx, const int ring_idx, - const int concentric_idx, + const size_t concentric_idx, const double z_vec, const double z_elevation, const double ground_flatness) { - if (z_vec > uprightness_thr_) { //orthogonal - if (concentric_idx < num_rings_of_interest_) { - if (z_elevation > elevation_thr_[concentric_idx]) { - if (flatness_thr_[concentric_idx] > ground_flatness) { + if (z_vec > params_.uprightness_thr_) { //orthogonal + if (concentric_idx < params_.num_rings_of_interest_) { + if (z_elevation > params_.czm.elevation_thr_[concentric_idx]) { + if (params_.czm.flatness_thr_[concentric_idx] > ground_flatness) { poly_list_.likelihood.push_back(FLAT_ENOUGH); } else { poly_list_.likelihood.push_back(TOO_HIGH_ELEVATION); @@ -995,7 +941,7 @@ void PatchWorkpp::calc_mean_stdev(std::vector vec, double &mean, mean = std::accumulate(vec.begin(), vec.end(), 0.0) / vec.size(); - for (int i=0; i::xy2radius(const double &x, const double &y) { template inline void PatchWorkpp::pc2czm(const pcl::PointCloud &src, std::vector &czm, pcl::PointCloud &cloud_nonground) { - for (int i=0; i::pc2czm(const pcl::PointCloud &src, std::vector PointT pt = src.points[i]; double r = xy2radius(pt.x, pt.y); - if ((r <= max_range_) && (r > min_range_)) { + if ((r <= params_.max_range_) && (r > params_.min_range_)) { double theta = xy2theta(pt.x, pt.y); int zone_idx = 0; - if ( r < min_ranges_[1] ) zone_idx = 0; - else if ( r < min_ranges_[2] ) zone_idx = 1; - else if ( r < min_ranges_[3] ) zone_idx = 2; + if ( r < params_.min_ranges_[1] ) zone_idx = 0; + else if ( r < params_.min_ranges_[2] ) zone_idx = 1; + else if ( r < params_.min_ranges_[3] ) zone_idx = 2; else zone_idx = 3; - int ring_idx = min(static_cast(((r - min_ranges_[zone_idx]) / ring_sizes_[zone_idx])), num_rings_each_zone_[zone_idx] - 1); - int sector_idx = min(static_cast((theta / sector_sizes_[zone_idx])), num_sectors_each_zone_[zone_idx] - 1); + size_t ring_idx = min(static_cast(((r - params_.min_ranges_[zone_idx]) / params_.ring_sizes_[zone_idx])), params_.czm.num_rings_each_zone_[zone_idx] - 1); + size_t sector_idx = min(static_cast((theta / params_.sector_sizes_[zone_idx])), params_.czm.num_sectors_each_zone_[zone_idx] - 1); czm[zone_idx][ring_idx][sector_idx].points.emplace_back(pt); } @@ -1048,7 +994,7 @@ void PatchWorkpp::pc2czm(const pcl::PointCloud &src, std::vector } } - if (verbose_) cout << "[ CZM ] Divides pointcloud into the concentric zone model" << endl; + if (params_ .verbose_) cout << "[ CZM ] Divides pointcloud into the concentric zone model" << endl; } #endif diff --git a/include/patchworkpp/utils.hpp b/include/patchworkpp/utils.hpp index 9230b19..2f6115b 100755 --- a/include/patchworkpp/utils.hpp +++ b/include/patchworkpp/utils.hpp @@ -70,7 +70,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZILID, void PointXYZILID2XYZI(pcl::PointCloud& src, - pcl::PointCloud::Ptr dst){ + pcl::PointCloud::Ptr dst) { dst->points.clear(); for (const auto &pt: src.points){ pcl::PointXYZI pt_xyzi; @@ -86,7 +86,7 @@ std::vector ground_classes = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_ std::vector ground_classes_except_terrain = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING}; std::vector traversable_ground_classes = {ROAD, PARKING, LANE_MARKING, OTHER_GROUND}; -int count_num_ground(const pcl::PointCloud& pc){ +int count_num_ground(const pcl::PointCloud& pc) { int num_ground = 0; std::vector::iterator iter; @@ -94,17 +94,19 @@ int count_num_ground(const pcl::PointCloud& pc){ for (auto const& pt: pc.points){ iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); if (iter != ground_classes.end()){ // corresponding class is in ground classes - if (pt.label == VEGETATION){ - if (pt.z < VEGETATION_THR){ + if (pt.label == VEGETATION) { + if (pt.z < VEGETATION_THR) { num_ground ++; } - }else num_ground ++; + } else { + num_ground ++; + } } } return num_ground; } -int count_num_ground_without_vegetation(const pcl::PointCloud& pc){ +int count_num_ground_without_vegetation(const pcl::PointCloud& pc) { int num_ground = 0; std::vector::iterator iter; @@ -113,34 +115,35 @@ int count_num_ground_without_vegetation(const pcl::PointCloud& pc) for (auto const& pt: pc.points){ iter = std::find(classes.begin(), classes.end(), pt.label); - if (iter != classes.end()){ // corresponding class is in ground classes + if (iter != classes.end()) { // corresponding class is in ground classes num_ground ++; } } return num_ground; } -std::map set_initial_gt_counts(std::vector& gt_classes){ +std::map set_initial_gt_counts(std::vector& gt_classes) { map gt_counts; - for (int i = 0; i< gt_classes.size(); ++i){ + for (size_t i = 0; i < gt_classes.size(); ++i) { gt_counts.insert(pair(gt_classes.at(i), 0)); } return gt_counts; } -std::map count_num_each_class(const pcl::PointCloud& pc){ - int num_ground = 0; +std::map count_num_each_class(const pcl::PointCloud& pc) { auto gt_counts = set_initial_gt_counts(ground_classes); std::vector::iterator iter; for (auto const& pt: pc.points){ iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); if (iter != ground_classes.end()){ // corresponding class is in ground classes - if (pt.label == VEGETATION){ - if (pt.z < VEGETATION_THR){ + if (pt.label == VEGETATION) { + if (pt.z < VEGETATION_THR) { gt_counts.find(pt.label)->second++; } - }else gt_counts.find(pt.label)->second++; + } else { + gt_counts.find(pt.label)->second++; + } } } return gt_counts; @@ -165,16 +168,20 @@ void discern_ground(const pcl::PointCloud& src, pcl::PointCloud::iterator iter; - for (auto const& pt: src.points){ + for (auto const& pt: src.points) { if (pt.label == UNLABELED || pt.label == OUTLIER) continue; iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); if (iter != ground_classes.end()){ // corresponding class is in ground classes - if (pt.label == VEGETATION){ - if (pt.z < VEGETATION_THR){ + if (pt.label == VEGETATION) { + if (pt.z < VEGETATION_THR) { ground.push_back(pt); - }else non_ground.push_back(pt); - }else ground.push_back(pt); - }else{ + } else { + non_ground.push_back(pt); + } + } else { + ground.push_back(pt); + } + } else { non_ground.push_back(pt); } } @@ -184,12 +191,12 @@ void discern_ground_without_vegetation(const pcl::PointCloud& src, ground.clear(); non_ground.clear(); std::vector::iterator iter; - for (auto const& pt: src.points){ + for (auto const& pt: src.points) { if (pt.label == UNLABELED || pt.label == OUTLIER) continue; iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); if (iter != ground_classes.end()){ // corresponding class is in ground classes if (pt.label != VEGETATION) ground.push_back(pt); - }else{ + } else { non_ground.push_back(pt); } } @@ -200,16 +207,16 @@ void calculate_precision_recall(const pcl::PointCloud& pc_curr, pcl::PointCloud& ground_estimated, double & precision, double& recall, - bool consider_outliers=true){ + bool consider_outliers=true) { int num_ground_est = ground_estimated.points.size(); int num_ground_gt = count_num_ground(pc_curr); int num_TP = count_num_ground(ground_estimated); - if (consider_outliers){ + if (consider_outliers) { int num_outliers_est = count_num_outliers(ground_estimated); precision = (double)(num_TP)/(num_ground_est - num_outliers_est) * 100; recall = (double)(num_TP)/num_ground_gt * 100; - }else{ + } else { precision = (double)(num_TP)/num_ground_est * 100; recall = (double)(num_TP)/num_ground_gt * 100; } @@ -219,28 +226,27 @@ void calculate_precision_recall_without_vegetation(const pcl::PointCloud& ground_estimated, double & precision, double& recall, - bool consider_outliers=true){ + bool consider_outliers=true) { int num_veg = 0; - for (auto const& pt: ground_estimated.points) - { + for (auto const& pt: ground_estimated.points) { if (pt.label == VEGETATION) num_veg++; } int num_ground_est = ground_estimated.size() - num_veg; int num_ground_gt = count_num_ground_without_vegetation(pc_curr); int num_TP = count_num_ground_without_vegetation(ground_estimated); - if (consider_outliers){ + if (consider_outliers) { int num_outliers_est = count_num_outliers(ground_estimated); precision = (double)(num_TP)/(num_ground_est - num_outliers_est) * 100; recall = (double)(num_TP)/num_ground_gt * 100; - }else{ + } else { precision = (double)(num_TP)/num_ground_est * 100; recall = (double)(num_TP)/num_ground_gt * 100; } } -void save_all_labels(const pcl::PointCloud& pc, string ABS_DIR, string seq, int count){ +void save_all_labels(const pcl::PointCloud& pc, string ABS_DIR, string seq, int count) { std::string count_str = std::to_string(count); std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str; @@ -285,10 +291,10 @@ void save_all_labels(const pcl::PointCloud& pc, string ABS_DIR, st else if (pt.label == 259) labels[33]++; } - for (uint8_t i=0; i < NUM_ALL_CLASSES;++i){ - if (i!=33){ + for (uint8_t i=0; i < NUM_ALL_CLASSES;++i) { + if (i!=33) { sc_output<& pc, string ABS_DIR, st void save_all_accuracy(const pcl::PointCloud& pc_curr, pcl::PointCloud& ground_estimated, string acc_filename, - double& accuracy, map&pc_curr_gt_counts, map&g_est_gt_counts){ + double& accuracy, map&pc_curr_gt_counts, map&g_est_gt_counts) { // std::cout<<"debug: "<& pc_curr, g_est_gt_counts = count_num_each_class(ground_estimated); // save output - for (auto const& class_id: ground_classes){ + for (auto const& class_id: ground_classes) { sc_output2<second<<","<second<<","; } sc_output2<& pc_curr, void pc2pcdfile(const pcl::PointCloud& TP, const pcl::PointCloud& FP, const pcl::PointCloud& FN, const pcl::PointCloud& TN, - std::string pcd_filename){ + std::string pcd_filename) { pcl::PointCloud pc_out; - for (auto const pt: TP.points){ + for (auto const pt: TP.points) { pcl::PointXYZI pt_est; pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; pt_est.intensity = TRUEPOSITIVE; pc_out.points.push_back(pt_est); } - for (auto const pt: FP.points){ + for (auto const pt: FP.points) { pcl::PointXYZI pt_est; pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; pt_est.intensity = FALSEPOSITIVE; pc_out.points.push_back(pt_est); } - for (auto const pt: FN.points){ + for (auto const pt: FN.points) { pcl::PointXYZI pt_est; pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; pt_est.intensity = FALSENEGATIVE; pc_out.points.push_back(pt_est); } - for (auto const pt: TN.points){ + for (auto const pt: TN.points) { pcl::PointXYZI pt_est; pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; pt_est.intensity = TRUENEGATIVE; diff --git a/launch/demo.launch b/launch/demo.launch index b3b6e42..bc6a1be 100755 --- a/launch/demo.launch +++ b/launch/demo.launch @@ -3,7 +3,7 @@ - + diff --git a/launch/demo_reconf.launch b/launch/demo_reconf.launch new file mode 100755 index 0000000..8484730 --- /dev/null +++ b/launch/demo_reconf.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/package.xml b/package.xml index ed6916a..863b0ae 100755 --- a/package.xml +++ b/package.xml @@ -31,7 +31,6 @@ jsk_recognition_msgs laser_geometry - diff --git a/config/params.yaml b/params/params.yaml similarity index 93% rename from config/params.yaml rename to params/params.yaml index 9cd6221..46c1e2a 100755 --- a/config/params.yaml +++ b/params/params.yaml @@ -3,13 +3,14 @@ save_flag: true # patchworkpp: sensor_height: 1.723 +point_topic: /kitti/velo/pointcloud mode: "czm" verbose: false # To check effect of uprightness/elevation/flatness visualize: true # Ground Likelihood Estimation is visualized # Ground Plane Fitting parameters -num_iter: 3 # Number of iterations for ground plane estimation using PCA. +num_iter: 5 # Number of iterations for ground plane estimation using PCA. num_lpr: 20 # Maximum number of points to be selected as lowest points representative. num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch. th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. @@ -22,9 +23,8 @@ uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Es adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered czm: - num_zones: 4 num_sectors_each_zone: [16, 32, 54, 32] - mum_rings_each_zone: [2, 4, 4, 4] + num_rings_each_zone: [2, 4, 4, 4] elevation_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. diff --git a/src/demo.cpp b/src/demo.cpp index 80020d3..88af7dc 100644 --- a/src/demo.cpp +++ b/src/demo.cpp @@ -2,6 +2,7 @@ // For disable PCL complile lib, to use PointXYZIR #define PCL_NO_PRECOMPILE +#include #include #include #include @@ -12,7 +13,7 @@ using PointType = pcl::PointXYZI; using namespace std; -boost::shared_ptr> PatchworkppGroundSeg; +std::unique_ptr> PatchworkppGroundSeg; ros::Publisher pub_cloud; ros::Publisher pub_ground; @@ -37,14 +38,14 @@ void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg) pcl::fromROSMsg(*cloud_msg, pc_curr); - PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); + if (PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken)) { + ROS_INFO_STREAM("\033[1;32m" << "Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() << "/ NonGround: " << pc_non_ground.size() + << " (running_time: " << time_taken << " sec)" << "\033[0m"); - ROS_INFO_STREAM("\033[1;32m" << "Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() << "/ NonGround: " << pc_non_ground.size() - << " (running_time: " << time_taken << " sec)" << "\033[0m"); - - pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.stamp, cloud_msg->header.frame_id)); - pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); - pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + } } int main(int argc, char**argv) { @@ -53,17 +54,14 @@ int main(int argc, char**argv) { ros::NodeHandle nh; ros::NodeHandle pnh("~"); - std::string cloud_topic; - pnh.param("cloud_topic", cloud_topic, "/pointcloud"); - - cout << "Operating patchwork++..." << endl; - PatchworkppGroundSeg.reset(new PatchWorkpp(&pnh)); + ROS_INFO("Operating patchwork++..."); + PatchworkppGroundSeg.reset(new PatchWorkpp(pnh)); pub_cloud = pnh.advertise("cloud", 100, true); pub_ground = pnh.advertise("ground", 100, true); pub_non_ground = pnh.advertise("nonground", 100, true); - ros::Subscriber sub_cloud = nh.subscribe(cloud_topic, 100, callbackCloud); + ros::Subscriber sub_cloud = nh.subscribe(PatchworkppGroundSeg->topic(), 100, callbackCloud); while (ros::ok()) diff --git a/src/demo_reconf.cpp b/src/demo_reconf.cpp new file mode 100644 index 0000000..9cae49a --- /dev/null +++ b/src/demo_reconf.cpp @@ -0,0 +1,90 @@ +#include +// For disable PCL complile lib, to use PointXYZIR +#define PCL_NO_PRECOMPILE + +#include +#include +#include +#include +#include +#include +#include +#include "patchworkpp/patchworkpp.hpp" + +using PointType = pcl::PointXYZI; + +std::unique_ptr> PatchworkppGroundSeg; + +ros::Publisher pub_cloud; +ros::Publisher pub_ground; +ros::Publisher pub_non_ground; + +template +sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud cloud, const ros::Time& stamp, std::string frame_id = "map") { + sensor_msgs::PointCloud2 cloud_ROS; + pcl::toROSMsg(cloud, cloud_ROS); + cloud_ROS.header.stamp = stamp; + cloud_ROS.header.frame_id = frame_id; + return cloud_ROS; +} + +void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg) +{ + double time_taken; + + pcl::PointCloud pc_curr; + pcl::PointCloud pc_ground; + pcl::PointCloud pc_non_ground; + + pcl::fromROSMsg(*cloud_msg, pc_curr); + + if (PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken)) { + ROS_INFO_STREAM("\033[1;32m" << "Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() << "/ NonGround: " << pc_non_ground.size() + << " (running_time: " << time_taken << " sec)" << "\033[0m"); + + pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); + } +} + +int main(int argc, char**argv) { + + ros::init(argc, argv, "Demo"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + + ROS_INFO("Operating patchwork++..."); + PatchworkppGroundSeg.reset(new PatchWorkpp()); + + pub_cloud = pnh.advertise("cloud", 100, true); + pub_ground = pnh.advertise("ground", 100, true); + pub_non_ground = pnh.advertise("nonground", 100, true); + + ros::Subscriber sub_cloud = nh.subscribe(PatchworkppGroundSeg->topic(), 100, callbackCloud); + + while (ros::ok()) + { + auto [topic_changed, topic] = PatchworkppGroundSeg->topic_changed(); + if (topic_changed) { + std::vector topics; + if (ros::master::check()) { + ros::master::getTopics(topics); + + bool topic_exists = std::find_if(topics.begin(), topics.end(), [&topic](const auto& topic_info) { return topic_info.name == topic; }) != topics.end(); + + if (topic_exists) + { + sub_cloud = nh.subscribe(PatchworkppGroundSeg->topic(), 100, callbackCloud); + } + else + { + ROS_WARN_STREAM("Topic " << topic << " doesn't exist"); + } + } + } + ros::spinOnce(); + } + + return 0; +} diff --git a/src/offline_kitti.cpp b/src/offline_kitti.cpp index b0d45b2..ee8788d 100644 --- a/src/offline_kitti.cpp +++ b/src/offline_kitti.cpp @@ -15,7 +15,7 @@ using PointType = PointXYZILID; using namespace std; void signal_callback_handler(int signum) { - cout << "Caught Ctrl + c " << endl; + ROS_INFO("Caught Ctrl + c "); // Terminate program exit(signum); } @@ -43,7 +43,7 @@ int main(int argc, char**argv) { ros::Publisher FPPublisher; ros::Publisher FNPublisher; - boost::shared_ptr > PatchworkppGroundSeg; + std::unique_ptr > PatchworkppGroundSeg; std::string output_csvpath; std::string acc_filename; @@ -80,21 +80,21 @@ int main(int argc, char**argv) { signal(SIGINT, signal_callback_handler); - PatchworkppGroundSeg.reset(new PatchWorkpp(&nh)); + PatchworkppGroundSeg.reset(new PatchWorkpp(nh)); data_path = data_path + "/" + seq; KittiLoader loader(data_path); int N = loader.size(); for (int n = init_idx; n < N; ++n) { - cout << n << "th node come" << endl; + ROS_INFO("th node come"); pcl::PointCloud pc_curr; loader.get_cloud(n, pc_curr); pcl::PointCloud pc_ground; pcl::PointCloud pc_non_ground; static double time_taken; - cout << "Operating patchwork++..." << endl; + ROS_INFO("Operating patchwork++..."); PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); diff --git a/src/video.cpp b/src/video.cpp index aa6360c..0dd7ca0 100644 --- a/src/video.cpp +++ b/src/video.cpp @@ -2,6 +2,7 @@ // For disable PCL complile lib, to use PointXYZIR #define PCL_NO_PRECOMPILE +#include #include #include #include @@ -15,7 +16,7 @@ using PointType = PointXYZILID; using namespace std; void signal_callback_handler(int signum) { - cout << "Caught Ctrl + c " << endl; + ROS_INFO("Caught Ctrl + c "); // Terminate program exit(signum); } @@ -45,7 +46,7 @@ int main(int argc, char**argv) { // ros::Publisher FPPublisher; // ros::Publisher FNPublisher; - boost::shared_ptr > PatchworkppGroundSeg; + std::unique_ptr> PatchworkppGroundSeg; std::string output_csvpath; std::string acc_filename; @@ -88,21 +89,21 @@ int main(int argc, char**argv) { signal(SIGINT, signal_callback_handler); - PatchworkppGroundSeg.reset(new PatchWorkpp(&nh)); + PatchworkppGroundSeg.reset(new PatchWorkpp(nh)); data_path = data_path + "/" + seq; KittiLoader loader(data_path); int N = loader.size(); for (int n = init_idx; n < N; ++n) { - cout << n << "th node come" << endl; + ROS_INFO("th node come"); pcl::PointCloud pc_curr; loader.get_cloud(n, pc_curr); pcl::PointCloud pc_ground; pcl::PointCloud pc_non_ground; static double time_taken; - cout << "Operating patchwork++..." << endl; + ROS_INFO("Operating patchwork++..."); PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken);