From 18563ed532f7489ad8fb4fca511c0615785e3d82 Mon Sep 17 00:00:00 2001
From: Julian Gaal <julian.gaal@anavs.de>
Date: Thu, 25 May 2023 14:12:47 +0200
Subject: [PATCH 1/8] compiles with dynamic reconfigure

---
 CMakeLists.txt                      |   9 +
 cfg/patchwork.cfg                   |  44 +++++
 include/patchworkpp/params.hpp      | 169 +++++++++++++++++++
 include/patchworkpp/patchworkpp.hpp | 253 ++++++++++++++++------------
 launch/demo.launch                  |   2 +-
 {config => params}/params.yaml      |   6 +-
 src/demo.cpp                        |   5 +-
 src/demo_reconfigure.cpp            |  90 ++++++++++
 src/offline_kitti.cpp               |   4 +-
 src/video.cpp                       |   5 +-
 10 files changed, 467 insertions(+), 120 deletions(-)
 create mode 100644 cfg/patchwork.cfg
 create mode 100644 include/patchworkpp/params.hpp
 rename {config => params}/params.yaml (93%)
 create mode 100644 src/demo_reconfigure.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index edaedb1..c9a335e 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -27,6 +27,7 @@ find_package(catkin REQUIRED COMPONENTS
         laser_geometry
         sensor_msgs
         message_generation
+        dynamic_reconfigure
         jsk_recognition_msgs
 )
 
@@ -48,6 +49,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 +74,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_reconfigure src/demo_reconfigure.cpp)
+target_link_libraries(demo_reconfigure ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
+add_dependencies(demo_reconfigure 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/cfg/patchwork.cfg b/cfg/patchwork.cfg
new file mode 100644
index 0000000..f5fb8da
--- /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("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, 0, 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_zones", int_t, 0, "Number of zones in czm mode", 4, 0, 10)
+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..57231ba
--- /dev/null
+++ b/include/patchworkpp/params.hpp
@@ -0,0 +1,169 @@
+#ifndef PATCHWORKPP_PARAMS
+#define PATCHWORKPP_PARAMS
+
+#include <regex>
+#include <vector>
+#include <stdexcept>
+#include <ros/ros.h>
+#include <dynamic_reconfigure/server.h>
+#include <patchworkpp/PatchworkppConfig.h>
+
+class ParamsNh
+{
+public:
+  ParamsNh() {
+    auto f = boost::bind(&ParamsNh::reconfigure_callback, this, _1, _2);
+    server_.setCallback(f);
+    validate();
+  }
+
+  ParamsNh(const ros::NodeHandle& nh) {
+    nh.param("verbose", verbose_, false);
+    nh.param("sensor_height", sensor_height_, 1.723);
+    nh.param("num_iter", num_iter_, 3);
+    nh.param("num_lpr", num_lpr_, 20);
+    nh.param("num_min_pts", num_min_pts_, 10);
+    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
+    nh.getParam("czm/num_zones", czm.num_zones_);
+    nh.getParam("czm/num_sectors_each_zone", czm.num_sectors_each_zone_);
+    nh.getParam("czm/num_rings_each_zone", czm.num_rings_each_zone_);
+    nh.getParam("czm/elevation_thresholds", czm.elevation_thresholds_);
+    nh.getParam("czm/flatness_thresholds", czm.flatness_thresholds_);
+
+    initialized_ = true;
+    validate();
+  }
+
+  void print_params() const {
+    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_);
+  }
+
+    bool validate() {
+    return  check(czm.num_zones_ == czm.num_sectors_each_zone_.size(), "num_zones 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_thresholds_.size(), "num_zones and length of elevation_thresholds must match") &&\
+            check(czm.num_zones_ == czm.flatness_thresholds_.size(), "num_zones and length of flatness_thresholds must match");
+  }
+
+private:
+  void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level) {
+    verbose_ = config.verbose;
+    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;
+
+    czm.num_zones_ = config.czm_num_zones;
+    czm.num_sectors_each_zone_ = convert_string_to_vector<int>(config.czm_num_sectors_each_zone);
+    czm.num_rings_each_zone_ = convert_string_to_vector<int>(config.czm_num_rings_each_zone);
+    czm.elevation_thresholds_ = convert_string_to_vector<double>(config.czm_elevation_thresholds);
+    czm.flatness_thresholds_ = convert_string_to_vector<double>(config.czm_flatness_thresholds);
+
+    ROS_INFO("Updated params");
+    initialized_ = true;
+    validate();
+  }
+
+
+  template <typename T>
+  std::vector<T> convert_string_to_vector(const std::string& str, char separator = ',') {
+      std::vector<T> 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;
+        }
+      }
+      
+      return result;
+  }
+
+
+  bool check(bool assertion, std::string description) {
+    if (not assertion) {
+        ROS_WARN_STREAM(description);
+        return false;
+    }
+
+    return true;
+  }
+
+  std::string mode_;
+  bool initialized_;
+  bool verbose_;
+  double sensor_height_;
+  int num_iter_;
+  int num_lpr_;
+  int 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_;
+  
+  struct CZM
+  {
+    int num_zones_;
+    std::vector<int> num_sectors_each_zone_;
+    std::vector<int> num_rings_each_zone_;
+    std::vector<double> elevation_thresholds_;
+    std::vector<double> flatness_thresholds_;
+  } czm;
+
+  dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server_;
+};
+
+#endif 
\ No newline at end of file
diff --git a/include/patchworkpp/patchworkpp.hpp b/include/patchworkpp/patchworkpp.hpp
index f45e249..e3a9df9 100755
--- a/include/patchworkpp/patchworkpp.hpp
+++ b/include/patchworkpp/patchworkpp.hpp
@@ -21,6 +21,7 @@
 #include <mutex>
 
 #include <patchworkpp/utils.hpp>
+#include <patchworkpp/params.hpp>
 
 #define MARKER_Z_VALUE -2.2
 #define UPRIGHT_ENOUGH 0.55
@@ -63,114 +64,30 @@ class PatchWorkpp {
     typedef std::vector<pcl::PointCloud<PointT>> Ring;
     typedef std::vector<Ring> Zone;
 
-    PatchWorkpp() {};
+    PatchWorkpp() : params_() {
+        params_.print_params();
 
-    PatchWorkpp(ros::NodeHandle *nh) : node_handle_(*nh) {
-        // Init ROS related
-        ROS_INFO("Inititalizing PatchWork++...");
+        plane_viz_       = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true);
+        pub_revert_pc_   = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true);
+        pub_reject_pc_   = node_handle_.advertise<sensor_msgs::PointCloud2>("reject_pc", 100, true);
+        pub_normal_      = node_handle_.advertise<sensor_msgs::PointCloud2>("normals", 100, true);
+        pub_noise_       = node_handle_.advertise<sensor_msgs::PointCloud2>("noise", 100, true);
+        pub_vertical_    = node_handle_.advertise<sensor_msgs::PointCloud2>("vertical", 100, true);
 
-        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");
-        }
-
-        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);
-
-        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<jsk_recognition_msgs::PolygonArray>("plane", 100, true);
-        pub_revert_pc   = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true);
-        pub_reject_pc   = node_handle_.advertise<sensor_msgs::PointCloud2>("reject_pc", 100, true);
-        pub_normal      = node_handle_.advertise<sensor_msgs::PointCloud2>("normals", 100, true);
-        pub_noise       = node_handle_.advertise<sensor_msgs::PointCloud2>("noise", 100, true);
-        pub_vertical    = node_handle_.advertise<sensor_msgs::PointCloud2>("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;
+        ROS_INFO("INITIALIZATION COMPLETE");
+    }
 
-        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)};
+    PatchWorkpp(const ros::NodeHandle &nh) : node_handle_(nh), params_(nh) {
+        params_.print_params();
 
-        cout << "INITIALIZATION COMPLETE" << endl;
+        plane_viz_       = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true);
+        pub_revert_pc_   = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true);
+        pub_reject_pc_   = node_handle_.advertise<sensor_msgs::PointCloud2>("reject_pc", 100, true);
+        pub_normal_      = node_handle_.advertise<sensor_msgs::PointCloud2>("normals", 100, true);
+        pub_noise_       = node_handle_.advertise<sensor_msgs::PointCloud2>("noise", 100, true);
+        pub_vertical_    = node_handle_.advertise<sensor_msgs::PointCloud2>("vertical", 100, true);
 
-        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);
-        }
+        ROS_INFO("INITIALIZATION COMPLETE");
     }
 
     void estimate_ground(pcl::PointCloud<PointT> cloud_in,
@@ -184,6 +101,10 @@ class PatchWorkpp {
 
     std::recursive_mutex mutex_;
 
+    ParamsNh params_;
+
+    bool initialized_;
+
     int num_iter_;
     int num_lpr_;
     int num_min_pts_;
@@ -240,7 +161,7 @@ class PatchWorkpp {
 
     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<PointT> revert_pc_, reject_pc_, noise_pc_, vertical_pc_;
     pcl::PointCloud<PointT> ground_pc_;
 
@@ -248,6 +169,10 @@ class PatchWorkpp {
 
     pcl::PointCloud<PointT> regionwise_ground_, regionwise_nonground_;
 
+    void initialize();
+
+    void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level);
+
     void initialize_zone(Zone &z, int num_sectors, int num_rings);
 
     void flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings);
@@ -335,6 +260,105 @@ void PatchWorkpp<PointT>::flush_patches(vector<Zone> &czm) {
     if( verbose_ ) cout << "Flushed patches" << endl;
 }
 
+template<typename PointT> inline
+void PatchWorkpp<PointT>::initialize() {
+        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/num_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");
+        }
+
+        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);
+
+        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);
+
+        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)};
+
+        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);
+        }
+
+        initialized_ = true;
+}
+
 template<typename PointT> inline
 void PatchWorkpp<PointT>::estimate_plane(const pcl::PointCloud<PointT> &ground) {
     pcl::computeMeanAndCovarianceMatrix(ground, cov_, pc_mean_);
@@ -458,6 +482,15 @@ void PatchWorkpp<PointT>::estimate_ground(
         pcl::PointCloud<PointT> &cloud_ground,
         pcl::PointCloud<PointT> &cloud_nonground,
         double &time_taken) {
+    
+    if (not initialized_) {
+        ROS_WARN_STREAM("Waiting for initialization...");
+        return;
+    }
+
+    if (not params_.validate()) {
+        return;
+    }
 
     unique_lock<recursive_mutex> lock(mutex_);
 
@@ -660,32 +693,32 @@ void PatchWorkpp<PointT>::estimate_ground(
         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_)
     {
-        PlaneViz.publish(poly_list_);
+        plane_viz_.publish(poly_list_);
     }
 
     revert_pc_.clear();
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 @@
 <arg name="cloud_topic" default="/kitti/velo/pointcloud"/>
 
   <node name="ground_segmentation" pkg="patchworkpp" type="demo" output="screen">
-    <rosparam command="load" file="$(find patchworkpp)/config/params.yaml" />
+    <rosparam command="load" file="$(find patchworkpp)/params/params.yaml" />
     <param name="cloud_topic" value="$(arg cloud_topic)"/>
   </node>
 
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..b3f494f 100755
--- a/config/params.yaml
+++ b/params/params.yaml
@@ -2,14 +2,14 @@ save_flag: true
 
 # patchworkpp:
 
-sensor_height: 1.723
+sensor_height: 1.2
 
 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.
@@ -24,7 +24,7 @@ adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_select
 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..8975caa 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 <memory>
 #include <ros/ros.h>
 #include <signal.h>
 #include <sensor_msgs/PointCloud2.h>
@@ -12,7 +13,7 @@
 using PointType = pcl::PointXYZI;
 using namespace std;
 
-boost::shared_ptr<PatchWorkpp<PointType>> PatchworkppGroundSeg;
+std::unique_ptr<PatchWorkpp<PointType>> PatchworkppGroundSeg;
 
 ros::Publisher pub_cloud;
 ros::Publisher pub_ground;
@@ -57,7 +58,7 @@ int main(int argc, char**argv) {
     pnh.param<string>("cloud_topic", cloud_topic, "/pointcloud");
 
     cout << "Operating patchwork++..." << endl;
-    PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>(&pnh));
+    PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>(pnh));
 
     pub_cloud       = pnh.advertise<sensor_msgs::PointCloud2>("cloud", 100, true);
     pub_ground      = pnh.advertise<sensor_msgs::PointCloud2>("ground", 100, true);
diff --git a/src/demo_reconfigure.cpp b/src/demo_reconfigure.cpp
new file mode 100644
index 0000000..502674f
--- /dev/null
+++ b/src/demo_reconfigure.cpp
@@ -0,0 +1,90 @@
+#include <iostream>
+// For disable PCL complile lib, to use PointXYZIR
+#define PCL_NO_PRECOMPILE
+
+#include <ros/ros.h>
+#include <signal.h>
+#include <dynamic_reconfigure/server.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <patchworkpp/PatchworkppConfig.h>
+#include "patchworkpp/patchworkpp.hpp"
+
+using PointType = pcl::PointXYZI;
+using namespace std;
+
+std::unique_ptr<PatchWorkpp<PointType>> PatchworkppGroundSeg;
+
+ros::Publisher pub_cloud;
+ros::Publisher pub_ground;
+ros::Publisher pub_non_ground;
+
+template<typename T>
+sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud<T> 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;
+}
+
+// Callback function for parameter changes
+void reconfigureCallback(const patchworkpp::PatchworkppConfig& config, uint32_t level)
+{
+  // Update your parameters based on the changes
+  // You can access the parameters using config.param_name
+  // For example: int parameter = config.int_param;
+  // Your code to handle parameter changes goes here
+  ROS_INFO("Updated parameters");
+}
+
+void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg)
+{
+    double time_taken;
+
+    pcl::PointCloud<PointType> pc_curr;
+    pcl::PointCloud<PointType> pc_ground;
+    pcl::PointCloud<PointType> pc_non_ground;
+
+    pcl::fromROSMsg(*cloud_msg, pc_curr);
+
+    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("~");
+
+    std::string cloud_topic;
+    pnh.param<string>("cloud_topic", cloud_topic, "/pointcloud");
+
+    cout << "Operating patchwork++..." << endl;
+    PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>(pnh));
+
+    pub_cloud       = pnh.advertise<sensor_msgs::PointCloud2>("cloud", 100, true);
+    pub_ground      = pnh.advertise<sensor_msgs::PointCloud2>("ground", 100, true);
+    pub_non_ground  = pnh.advertise<sensor_msgs::PointCloud2>("nonground", 100, true);
+
+    ros::Subscriber sub_cloud = nh.subscribe(cloud_topic, 100, callbackCloud);
+
+    // Create a dynamic reconfigure server
+    dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server;
+    dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig>::CallbackType f;
+
+    // Set the callback function
+    f = boost::bind(&reconfigureCallback, _1, _2);
+    server.setCallback(f);
+
+    ros::spin();
+
+    return 0;
+}
diff --git a/src/offline_kitti.cpp b/src/offline_kitti.cpp
index b0d45b2..db12d85 100644
--- a/src/offline_kitti.cpp
+++ b/src/offline_kitti.cpp
@@ -43,7 +43,7 @@ int main(int argc, char**argv) {
     ros::Publisher FPPublisher;
     ros::Publisher FNPublisher;
 
-    boost::shared_ptr<PatchWorkpp<PointType> > PatchworkppGroundSeg;
+    std::unique_ptr<PatchWorkpp<PointType> > PatchworkppGroundSeg;
     std::string output_csvpath;
 
     std::string acc_filename;
@@ -80,7 +80,7 @@ int main(int argc, char**argv) {
 
     signal(SIGINT, signal_callback_handler);
 
-    PatchworkppGroundSeg.reset(new PatchWorkpp<PointXYZILID>(&nh));
+    PatchworkppGroundSeg.reset(new PatchWorkpp<PointXYZILID>(nh));
     data_path = data_path + "/" + seq;
     KittiLoader loader(data_path);
 
diff --git a/src/video.cpp b/src/video.cpp
index aa6360c..e726d21 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 <memory>
 #include <ros/ros.h>
 #include <signal.h>
 #include <sensor_msgs/PointCloud2.h>
@@ -45,7 +46,7 @@ int main(int argc, char**argv) {
     // ros::Publisher FPPublisher;
     // ros::Publisher FNPublisher;
 
-    boost::shared_ptr<PatchWorkpp<PointType> > PatchworkppGroundSeg;
+    std::unique_ptr<PatchWorkpp<PointType>> PatchworkppGroundSeg;
     std::string output_csvpath;
 
     std::string acc_filename;
@@ -88,7 +89,7 @@ int main(int argc, char**argv) {
 
     signal(SIGINT, signal_callback_handler);
 
-    PatchworkppGroundSeg.reset(new PatchWorkpp<PointXYZILID>(&nh));
+    PatchworkppGroundSeg.reset(new PatchWorkpp<PointXYZILID>(nh));
     data_path = data_path + "/" + seq;
     KittiLoader loader(data_path);
 

From 6b1b098c7443994a7fcca605a82fdf37036852bb Mon Sep 17 00:00:00 2001
From: Julian Gaal <julian.gaal@anavs.de>
Date: Thu, 25 May 2023 16:55:42 +0200
Subject: [PATCH 2/8] default reconfigure parameters match rosparam servers

---
 CMakeLists.txt                                |   6 +-
 include/patchworkpp/params.hpp                | 319 ++++++++++--------
 include/patchworkpp/patchworkpp.hpp           | 130 +++----
 launch/demo.launch                            |   2 +-
 launch/demo_reconf.launch                     |  14 +
 src/demo.cpp                                  |   2 +-
 src/{demo_reconfigure.cpp => demo_reconf.cpp} |   5 +-
 src/offline_kitti.cpp                         |   6 +-
 src/video.cpp                                 |   6 +-
 9 files changed, 281 insertions(+), 209 deletions(-)
 create mode 100755 launch/demo_reconf.launch
 rename src/{demo_reconfigure.cpp => demo_reconf.cpp} (95%)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index c9a335e..a4c8877 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -74,9 +74,9 @@ 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_reconfigure src/demo_reconfigure.cpp)
-target_link_libraries(demo_reconfigure ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
-add_dependencies(demo_reconfigure 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})
diff --git a/include/patchworkpp/params.hpp b/include/patchworkpp/params.hpp
index 57231ba..787b903 100644
--- a/include/patchworkpp/params.hpp
+++ b/include/patchworkpp/params.hpp
@@ -8,118 +8,201 @@
 #include <dynamic_reconfigure/server.h>
 #include <patchworkpp/PatchworkppConfig.h>
 
-class ParamsNh
-{
+class ParamsNh {
 public:
-  ParamsNh() {
-    auto f = boost::bind(&ParamsNh::reconfigure_callback, this, _1, _2);
-    server_.setCallback(f);
-    validate();
-  }
+    ParamsNh() {
+        auto f = boost::bind(&ParamsNh::reconfigure_callback, this, _1, _2);
+        server_.setCallback(f);
+        validate();
+    }
 
-  ParamsNh(const ros::NodeHandle& nh) {
-    nh.param("verbose", verbose_, false);
-    nh.param("sensor_height", sensor_height_, 1.723);
-    nh.param("num_iter", num_iter_, 3);
-    nh.param("num_lpr", num_lpr_, 20);
-    nh.param("num_min_pts", num_min_pts_, 10);
-    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
-    nh.getParam("czm/num_zones", czm.num_zones_);
-    nh.getParam("czm/num_sectors_each_zone", czm.num_sectors_each_zone_);
-    nh.getParam("czm/num_rings_each_zone", czm.num_rings_each_zone_);
-    nh.getParam("czm/elevation_thresholds", czm.elevation_thresholds_);
-    nh.getParam("czm/flatness_thresholds", czm.flatness_thresholds_);
-
-    initialized_ = true;
-    validate();
-  }
+    ParamsNh(const ros::NodeHandle& nh) {
+        nh.param("verbose", verbose_, false);
+        nh.param("sensor_height", sensor_height_, 1.723);
+        nh.param("num_iter", num_iter_, 3);
+        nh.param("num_lpr", num_lpr_, 20);
+        nh.param("num_min_pts", num_min_pts_, 10);
+        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
+        nh.getParam("czm/num_zones", czm.num_zones_);
+        nh.getParam("czm/num_sectors_each_zone", czm.num_sectors_each_zone_);
+        nh.getParam("czm/num_rings_each_zone", czm.num_rings_each_zone_);
+        nh.getParam("czm/elevation_thresholds", czm.elevation_thr_);
+        nh.getParam("czm/flatness_thresholds", czm.flatness_thr_);
+
+        validate();
+
+        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)};
+
+        initialized_ = true;
+        
+    }
 
-  void print_params() const {
-    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_);
-  }
+    void print_params() const {
+        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_);
+        ROS_INFO("Num. zones: %d", czm.num_zones_);
+        ROS_INFO_STREAM((boost::format("Num. sectors: %d, %d, %d, %d") % 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: %01d, %01d, %01d, %01d") % 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() {
-    return  check(czm.num_zones_ == czm.num_sectors_each_zone_.size(), "num_zones 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_thresholds_.size(), "num_zones and length of elevation_thresholds must match") &&\
-            check(czm.num_zones_ == czm.flatness_thresholds_.size(), "num_zones and length of flatness_thresholds must match");
-  }
+        return  check(czm.num_zones_ != 4, "Number of zones must be four!") &&\
+                check(czm.num_zones_ == czm.num_sectors_each_zone_.size(), "num_zones 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");
+    }
+
+    std::string mode_;
+    bool initialized_;
+    bool verbose_;
+    double sensor_height_;
+    int num_iter_;
+    int num_lpr_;
+    int 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<double> min_ranges_;
+    std::vector<double> ring_sizes_;
+    std::vector<double> sector_sizes_;
+
+    struct CZM
+    {
+        int num_zones_;
+        std::vector<int> num_sectors_each_zone_;
+        std::vector<int> num_rings_each_zone_;
+        std::vector<double> elevation_thr_;
+        std::vector<double> flatness_thr_;
+    } czm;
 
 private:
-  void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level) {
-    verbose_ = config.verbose;
-    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;
-
-    czm.num_zones_ = config.czm_num_zones;
-    czm.num_sectors_each_zone_ = convert_string_to_vector<int>(config.czm_num_sectors_each_zone);
-    czm.num_rings_each_zone_ = convert_string_to_vector<int>(config.czm_num_rings_each_zone);
-    czm.elevation_thresholds_ = convert_string_to_vector<double>(config.czm_elevation_thresholds);
-    czm.flatness_thresholds_ = convert_string_to_vector<double>(config.czm_flatness_thresholds);
-
-    ROS_INFO("Updated params");
-    initialized_ = true;
-    validate();
-  }
+    void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level) {
+        verbose_ = config.verbose;
+        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;
+
+        czm.num_zones_ = config.czm_num_zones;
+        czm.num_sectors_each_zone_ = convert_string_to_vector<int>(config.czm_num_sectors_each_zone);
+        czm.num_rings_each_zone_ = convert_string_to_vector<int>(config.czm_num_rings_each_zone);
+        czm.elevation_thr_ = convert_string_to_vector<double>(config.czm_elevation_thresholds);
+        czm.flatness_thr_ = convert_string_to_vector<double>(config.czm_flatness_thresholds);
+
+        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;
+
+        validate();
+
+        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)};
+
+        ROS_INFO("Updated params");
+        initialized_ = true;
+    }
 
 
-  template <typename T>
-  std::vector<T> convert_string_to_vector(const std::string& str, char separator = ',') {
-      std::vector<T> 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;
+    template <typename T>
+    std::vector<T> convert_string_to_vector(const std::string& str, char separator = ',') {
+        std::vector<T> 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;
+            }
         }
-      }
-      
-      return result;
-  }
+        
+        return result;
+    }
 
 
   bool check(bool assertion, std::string description) {
@@ -131,38 +214,6 @@ class ParamsNh
     return true;
   }
 
-  std::string mode_;
-  bool initialized_;
-  bool verbose_;
-  double sensor_height_;
-  int num_iter_;
-  int num_lpr_;
-  int 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_;
-  
-  struct CZM
-  {
-    int num_zones_;
-    std::vector<int> num_sectors_each_zone_;
-    std::vector<int> num_rings_each_zone_;
-    std::vector<double> elevation_thresholds_;
-    std::vector<double> flatness_thresholds_;
-  } czm;
-
   dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server_;
 };
 
diff --git a/include/patchworkpp/patchworkpp.hpp b/include/patchworkpp/patchworkpp.hpp
index e3a9df9..b12dfe6 100755
--- a/include/patchworkpp/patchworkpp.hpp
+++ b/include/patchworkpp/patchworkpp.hpp
@@ -64,8 +64,9 @@ class PatchWorkpp {
     typedef std::vector<pcl::PointCloud<PointT>> Ring;
     typedef std::vector<Ring> Zone;
 
-    PatchWorkpp() : params_() {
+    PatchWorkpp() : params_(), using_reconf_(true) {
         params_.print_params();
+        initialize();
 
         plane_viz_       = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true);
         pub_revert_pc_   = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true);
@@ -74,11 +75,17 @@ class PatchWorkpp {
         pub_noise_       = node_handle_.advertise<sensor_msgs::PointCloud2>("noise", 100, true);
         pub_vertical_    = node_handle_.advertise<sensor_msgs::PointCloud2>("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);
+
         ROS_INFO("INITIALIZATION COMPLETE");
     }
 
-    PatchWorkpp(const ros::NodeHandle &nh) : node_handle_(nh), params_(nh) {
+    PatchWorkpp(const ros::NodeHandle &nh) : node_handle_(nh), params_(nh), using_reconf_(false) {
         params_.print_params();
+        initialize();
 
         plane_viz_       = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true);
         pub_revert_pc_   = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true);
@@ -87,6 +94,11 @@ class PatchWorkpp {
         pub_noise_       = node_handle_.advertise<sensor_msgs::PointCloud2>("noise", 100, true);
         pub_vertical_    = node_handle_.advertise<sensor_msgs::PointCloud2>("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);
+
         ROS_INFO("INITIALIZATION COMPLETE");
     }
 
@@ -120,13 +132,11 @@ class PatchWorkpp {
     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 using_reconf_;
     bool enable_RNR_;
     bool enable_RVPF_;
     bool enable_TGR_;
@@ -174,6 +184,8 @@ class PatchWorkpp {
     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<Zone> &czm);
@@ -263,7 +275,6 @@ void PatchWorkpp<PointT>::flush_patches(vector<Zone> &czm) {
 template<typename PointT> inline
 void PatchWorkpp<PointT>::initialize() {
         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);
@@ -284,6 +295,32 @@ void PatchWorkpp<PointT>::initialize() {
         node_handle_.param("enable_RVPF", enable_RVPF_, true);
         node_handle_.param("enable_TGR", enable_TGR_, true);
 
+        // 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/num_rings_each_zone", num_rings_each_zone_);
+        node_handle_.getParam("czm/elevation_thresholds", elevation_thr_);
+        node_handle_.getParam("czm/flatness_thresholds", flatness_thr_);
+        node_handle_.param("visualize", visualize_, true);
+
+        num_rings_of_interest_ = 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_) / 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)};
+
+        reset_poly_list();
+        reset_concentric_zone_model();
+
         ROS_INFO("Sensor Height: %f", sensor_height_);
         ROS_INFO("Num of Iteration: %d", num_iter_);
         ROS_INFO("Num of LPR: %d", num_lpr_);
@@ -294,69 +331,40 @@ void PatchWorkpp<PointT>::initialize() {
         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/num_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");
-        }
-
-        cout << (boost::format("Num. sectors: %d, %d, %d, %d") % num_sectors_each_zone_[0] % num_sectors_each_zone_[1] %
+        ROS_INFO_STREAM((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_sectors_each_zone_[3]).str());
+        ROS_INFO_STREAM((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] %
+                 num_rings_each_zone_[3]).str());
+        ROS_INFO_STREAM((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] %
+                 elevation_thr_[3]).str());
+        ROS_INFO_STREAM((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);
-
-        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);
-
-        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;
+                 flatness_thr_[3]).str());
 
-        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)};
+        initialized_ = true;
+}
 
-        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);
-        }
+template<typename PointT> inline
+void PatchWorkpp<PointT>::reset_poly_list() {
+    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.clear();
+    poly_list_.polygons.reserve(num_polygons);
+}
 
-        initialized_ = true;
+template<typename PointT> inline
+void PatchWorkpp<PointT>::reset_concentric_zone_model() {
+    ConcentricZoneModel_.clear();
+    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);
+    }
 }
 
 template<typename PointT> inline
@@ -488,7 +496,7 @@ void PatchWorkpp<PointT>::estimate_ground(
         return;
     }
 
-    if (not params_.validate()) {
+    if (using_reconf_ && not params_.validate()) {
         return;
     }
 
diff --git a/launch/demo.launch b/launch/demo.launch
index bc6a1be..38d2129 100755
--- a/launch/demo.launch
+++ b/launch/demo.launch
@@ -1,6 +1,6 @@
 <launch>
 
-<arg name="cloud_topic" default="/kitti/velo/pointcloud"/>
+<arg name="cloud_topic" default="/velodyne_points"/>
 
   <node name="ground_segmentation" pkg="patchworkpp" type="demo" output="screen">
     <rosparam command="load" file="$(find patchworkpp)/params/params.yaml" />
diff --git a/launch/demo_reconf.launch b/launch/demo_reconf.launch
new file mode 100755
index 0000000..3cf4554
--- /dev/null
+++ b/launch/demo_reconf.launch
@@ -0,0 +1,14 @@
+<launch>
+
+  <arg name="cloud_topic" default="/velodyne_points"/>
+  
+  <node name="rqt_conf_gui" pkg="rqt_reconfigure" type="rqt_reconfigure" output="screen"/>
+
+  <node name="ground_segmentation" pkg="patchworkpp" type="demo_reconf" output="screen">
+    <rosparam command="load" file="$(find patchworkpp)/params/params.yaml" />
+    <param name="cloud_topic" value="$(arg cloud_topic)"/>
+  </node>
+
+  <node name="$(anon rviz)" pkg="rviz" type="rviz" args="-d $(find patchworkpp)/rviz/demo.rviz"/>
+
+</launch>
diff --git a/src/demo.cpp b/src/demo.cpp
index 8975caa..7da6389 100644
--- a/src/demo.cpp
+++ b/src/demo.cpp
@@ -57,7 +57,7 @@ int main(int argc, char**argv) {
     std::string cloud_topic;
     pnh.param<string>("cloud_topic", cloud_topic, "/pointcloud");
 
-    cout << "Operating patchwork++..." << endl;
+    ROS_INFO("Operating patchwork++...");
     PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>(pnh));
 
     pub_cloud       = pnh.advertise<sensor_msgs::PointCloud2>("cloud", 100, true);
diff --git a/src/demo_reconfigure.cpp b/src/demo_reconf.cpp
similarity index 95%
rename from src/demo_reconfigure.cpp
rename to src/demo_reconf.cpp
index 502674f..c90b40c 100644
--- a/src/demo_reconfigure.cpp
+++ b/src/demo_reconf.cpp
@@ -11,7 +11,6 @@
 #include "patchworkpp/patchworkpp.hpp"
 
 using PointType = pcl::PointXYZI;
-using namespace std;
 
 std::unique_ptr<PatchWorkpp<PointType>> PatchworkppGroundSeg;
 
@@ -67,8 +66,8 @@ int main(int argc, char**argv) {
     std::string cloud_topic;
     pnh.param<string>("cloud_topic", cloud_topic, "/pointcloud");
 
-    cout << "Operating patchwork++..." << endl;
-    PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>(pnh));
+    ROS_INFO("Operating patchwork++...");
+    PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>());
 
     pub_cloud       = pnh.advertise<sensor_msgs::PointCloud2>("cloud", 100, true);
     pub_ground      = pnh.advertise<sensor_msgs::PointCloud2>("ground", 100, true);
diff --git a/src/offline_kitti.cpp b/src/offline_kitti.cpp
index db12d85..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);
 }
@@ -87,14 +87,14 @@ int main(int argc, char**argv) {
     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<PointType> pc_curr;
         loader.get_cloud(n, pc_curr);
         pcl::PointCloud<PointType> pc_ground;
         pcl::PointCloud<PointType> 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 e726d21..0dd7ca0 100644
--- a/src/video.cpp
+++ b/src/video.cpp
@@ -16,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);
 }
@@ -96,14 +96,14 @@ int main(int argc, char**argv) {
     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<PointType> pc_curr;
         loader.get_cloud(n, pc_curr);
         pcl::PointCloud<PointType> pc_ground;
         pcl::PointCloud<PointType> 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);
 

From 38e00a637925a27d34e6ed1e184b09383b99128a Mon Sep 17 00:00:00 2001
From: Julian Gaal <julian.gaal@anavs.de>
Date: Thu, 25 May 2023 17:39:59 +0200
Subject: [PATCH 3/8] move to ParamsHandler for regular demo

---
 cfg/patchwork.cfg                   |   2 +-
 include/patchworkpp/params.hpp      |  31 +--
 include/patchworkpp/patchworkpp.hpp | 317 +++++++++-------------------
 src/demo.cpp                        |  14 +-
 src/demo_reconf.cpp                 |  16 +-
 5 files changed, 139 insertions(+), 241 deletions(-)

diff --git a/cfg/patchwork.cfg b/cfg/patchwork.cfg
index f5fb8da..28b75eb 100644
--- a/cfg/patchwork.cfg
+++ b/cfg/patchwork.cfg
@@ -12,7 +12,7 @@ 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, 0, 10)
+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)
diff --git a/include/patchworkpp/params.hpp b/include/patchworkpp/params.hpp
index 787b903..6fb8fb2 100644
--- a/include/patchworkpp/params.hpp
+++ b/include/patchworkpp/params.hpp
@@ -2,22 +2,25 @@
 #define PATCHWORKPP_PARAMS
 
 #include <regex>
+#include <mutex>
 #include <vector>
 #include <stdexcept>
 #include <ros/ros.h>
 #include <dynamic_reconfigure/server.h>
 #include <patchworkpp/PatchworkppConfig.h>
 
-class ParamsNh {
+class ParamsHandler {
 public:
-    ParamsNh() {
-        auto f = boost::bind(&ParamsNh::reconfigure_callback, this, _1, _2);
+    ParamsHandler(std::recursive_mutex& mutex) : mutex_(mutex)  {
+        auto f = boost::bind(&ParamsHandler::reconfigure_callback, this, _1, _2);
         server_.setCallback(f);
+        std::unique_lock<std::recursive_mutex> lock(mutex_);
         validate();
     }
 
-    ParamsNh(const ros::NodeHandle& nh) {
+    ParamsHandler(const ros::NodeHandle& nh, std::recursive_mutex& mutex) : mutex_(mutex) {
         nh.param("verbose", verbose_, false);
+        nh.param("verbose", visualize_, true);
         nh.param("sensor_height", sensor_height_, 1.723);
         nh.param("num_iter", num_iter_, 3);
         nh.param("num_lpr", num_lpr_, 20);
@@ -104,6 +107,7 @@ class ParamsNh {
     std::string mode_;
     bool initialized_;
     bool verbose_;
+    bool visualize_;
     double sensor_height_;
     int num_iter_;
     int num_lpr_;
@@ -139,7 +143,9 @@ class ParamsNh {
 
 private:
     void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level) {
+        std::unique_lock<std::recursive_mutex> lock(mutex_);
         verbose_ = config.verbose;
+        visualize_ = config.visualize;
         sensor_height_ = config.sensor_height;
         num_iter_ = config.num_iter;
         num_lpr_ = config.num_lpr;
@@ -205,16 +211,17 @@ class ParamsNh {
     }
 
 
-  bool check(bool assertion, std::string description) {
-    if (not assertion) {
-        ROS_WARN_STREAM(description);
-        return false;
-    }
+    bool check(bool assertion, std::string description) {
+        if (not assertion) {
+            ROS_WARN_STREAM(description);
+            return false;
+        }
 
-    return true;
-  }
+        return true;
+    }
 
-  dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server_;
+    std::recursive_mutex& mutex_;
+    dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server_;
 };
 
 #endif 
\ No newline at end of file
diff --git a/include/patchworkpp/patchworkpp.hpp b/include/patchworkpp/patchworkpp.hpp
index b12dfe6..eabc4a7 100755
--- a/include/patchworkpp/patchworkpp.hpp
+++ b/include/patchworkpp/patchworkpp.hpp
@@ -64,9 +64,8 @@ class PatchWorkpp {
     typedef std::vector<pcl::PointCloud<PointT>> Ring;
     typedef std::vector<Ring> Zone;
 
-    PatchWorkpp() : params_(), using_reconf_(true) {
+    PatchWorkpp() : mutex_(), params_(mutex_), using_reconf_(true) {
         params_.print_params();
-        initialize();
 
         plane_viz_       = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true);
         pub_revert_pc_   = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true);
@@ -80,12 +79,14 @@ class PatchWorkpp {
         regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);
         regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);
 
+        reset_poly_list();
+        reset_concentric_zone_model();
+
         ROS_INFO("INITIALIZATION COMPLETE");
     }
 
-    PatchWorkpp(const ros::NodeHandle &nh) : node_handle_(nh), params_(nh), using_reconf_(false) {
+    PatchWorkpp(const ros::NodeHandle &nh) : node_handle_(nh), mutex_(), params_(nh, mutex_), using_reconf_(false) {
         params_.print_params();
-        initialize();
 
         plane_viz_       = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true);
         pub_revert_pc_   = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true);
@@ -99,10 +100,13 @@ class PatchWorkpp {
         regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);
         regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);
 
+        reset_poly_list();
+        reset_concentric_zone_model();
+
         ROS_INFO("INITIALIZATION COMPLETE");
     }
 
-    void estimate_ground(pcl::PointCloud<PointT> cloud_in,
+    bool estimate_ground(pcl::PointCloud<PointT> cloud_in,
                          pcl::PointCloud<PointT> &cloud_ground, pcl::PointCloud<PointT> &cloud_nonground, double &time_taken);
 
 private:
@@ -113,35 +117,10 @@ class PatchWorkpp {
 
     std::recursive_mutex mutex_;
 
-    ParamsNh params_;
-
-    bool initialized_;
-
-    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 RNR_ver_angle_thr_;
-    double RNR_intensity_thr_;
-
-    bool verbose_;
+    ParamsHandler params_;
+
     bool using_reconf_;
-    bool enable_RNR_;
-    bool enable_RVPF_;
-    bool enable_TGR_;
 
-    int max_flatness_storage_, max_elevation_storage_;
     std::vector<double> update_flatness_[4];
     std::vector<double> update_elevation_[4];
 
@@ -153,18 +132,6 @@ class PatchWorkpp {
     Eigen::Matrix3f cov_;
     Eigen::Vector4f pc_mean_;
 
-    // For visualization
-    bool visualize_;
-
-    vector<int> num_sectors_each_zone_;
-    vector<int> num_rings_each_zone_;
-
-    vector<double> sector_sizes_;
-    vector<double> ring_sizes_;
-    vector<double> min_ranges_;
-    vector<double> elevation_thr_;
-    vector<double> flatness_thr_;
-
     queue<int> noise_idxs_;
 
     vector<Zone> ConcentricZoneModel_;
@@ -179,8 +146,6 @@ class PatchWorkpp {
 
     pcl::PointCloud<PointT> regionwise_ground_, regionwise_nonground_;
 
-    void initialize();
-
     void reconfigure_callback(const patchworkpp::PatchworkppConfig& config, uint32_t level);
 
     void initialize_zone(Zone &z, int num_sectors, int num_rings);
@@ -261,97 +226,20 @@ void PatchWorkpp<PointT>::flush_patches_in_zone(Zone &patches, int num_sectors,
 
 template<typename PointT> inline
 void PatchWorkpp<PointT>::flush_patches(vector<Zone> &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 (int k = 0; k < params_.czm.num_zones_; k++) {
+        for (int i = 0; i < params_.czm.num_rings_each_zone_[k]; i++) {
+            for (int 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;
-}
-
-template<typename PointT> inline
-void PatchWorkpp<PointT>::initialize() {
-        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);
-
-        // 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/num_rings_each_zone", num_rings_each_zone_);
-        node_handle_.getParam("czm/elevation_thresholds", elevation_thr_);
-        node_handle_.getParam("czm/flatness_thresholds", flatness_thr_);
-        node_handle_.param("visualize", visualize_, true);
-
-        num_rings_of_interest_ = 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_) / 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)};
-
-        reset_poly_list();
-        reset_concentric_zone_model();
-
-        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_);
-        ROS_INFO("Num. zones: %d", num_zones_);
-        ROS_INFO_STREAM((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());
-        ROS_INFO_STREAM((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());
-        ROS_INFO_STREAM((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());
-        ROS_INFO_STREAM((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());
-
-        initialized_ = true;
+    if( params_ .verbose_ ) cout << "Flushed patches" << endl;
 }
 
 template<typename PointT> inline
 void PatchWorkpp<PointT>::reset_poly_list() {
-    int num_polygons = std::inner_product(num_rings_each_zone_.begin(), num_rings_each_zone_.end(), num_sectors_each_zone_.begin(), 0);
+    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);
@@ -360,9 +248,9 @@ void PatchWorkpp<PointT>::reset_poly_list() {
 template<typename PointT> inline
 void PatchWorkpp<PointT>::reset_concentric_zone_model() {
     ConcentricZoneModel_.clear();
-    for (int i = 0; i < num_zones_; i++) {
+    for (int i = 0; i < params_.czm.num_zones_; i++) {
         Zone z;
-        initialize_zone(z, num_sectors_each_zone_[i], num_rings_each_zone_[i]);
+        initialize_zone(z, params_.czm.num_sectors_each_zone_[i], params_.czm.num_rings_each_zone_[i]);
         ConcentricZoneModel_.push_back(z);
     }
 }
@@ -399,7 +287,7 @@ void PatchWorkpp<PointT>::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_) {
+            if (p_sorted.points[i].z < params_.adaptive_seed_selection_margin_ * params_.sensor_height_) {
                 ++init_idx;
             } else {
                 break;
@@ -408,13 +296,13 @@ void PatchWorkpp<PointT>::extract_initial_seeds(
     }
 
     // Calculate the mean height value.
-    for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) {
+    for (int 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_
+    // iterate pointcloud, filter those height is less than lpr.height+params_.th_seeds_
     for (int 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]);
@@ -435,7 +323,7 @@ void PatchWorkpp<PointT>::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_) {
+            if (p_sorted.points[i].z < params_.adaptive_seed_selection_margin_ * params_.sensor_height_) {
                 ++init_idx;
             } else {
                 break;
@@ -444,15 +332,15 @@ void PatchWorkpp<PointT>::extract_initial_seeds(
     }
 
     // Calculate the mean height value.
-    for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) {
+    for (int 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_
+    // iterate pointcloud, filter those height is less than lpr.height+params_.th_seeds_
     for (int i = 0; i < p_sorted.points.size(); i++) {
-        if (p_sorted.points[i].z < lpr_height + th_seeds_) {
+        if (p_sorted.points[i].z < lpr_height + params_.th_seeds_) {
             init_seeds.points.push_back(p_sorted.points[i]);
         }
     }
@@ -467,7 +355,7 @@ void PatchWorkpp<PointT>::reflected_noise_removal(pcl::PointCloud<PointT> &cloud
         double z = cloud_in[i].z;
         double ver_angle_in_deg = atan2(z, r)*180/M_PI;
 
-        if ( ver_angle_in_deg < RNR_ver_angle_thr_ && z < -sensor_height_-0.8 && cloud_in[i].intensity < RNR_intensity_thr_)
+        if ( ver_angle_in_deg < params_.RNR_ver_angle_thr_ && z < -params_.sensor_height_-0.8 && cloud_in[i].intensity < params_.RNR_intensity_thr_)
         {
             cloud_nonground.push_back(cloud_in[i]);
             noise_pc_.push_back(cloud_in[i]);
@@ -475,7 +363,7 @@ void PatchWorkpp<PointT>::reflected_noise_removal(pcl::PointCloud<PointT> &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;
 }
 
 /*
@@ -485,23 +373,24 @@ void PatchWorkpp<PointT>::reflected_noise_removal(pcl::PointCloud<PointT> &cloud
 */
 
 template<typename PointT> inline
-void PatchWorkpp<PointT>::estimate_ground(
+bool PatchWorkpp<PointT>::estimate_ground(
         pcl::PointCloud<PointT> cloud_in,
         pcl::PointCloud<PointT> &cloud_ground,
         pcl::PointCloud<PointT> &cloud_nonground,
         double &time_taken) {
     
-    if (not initialized_) {
+    unique_lock<recursive_mutex> lock(mutex_);
+    
+    if (not params_.initialized_ || params_.num_iter_ < 1) {
         ROS_WARN_STREAM("Waiting for initialization...");
-        return;
+        return false;
     }
 
     if (using_reconf_ && not params_.validate()) {
-        return;
+        ROS_WARN_STREAM("Invalid parameters");
+        return false;
     }
 
-    unique_lock<recursive_mutex> lock(mutex_);
-
     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();
@@ -520,7 +409,7 @@ void PatchWorkpp<PointT>::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();
 
@@ -537,14 +426,14 @@ void PatchWorkpp<PointT>::estimate_ground(
     std::vector<RevertCandidate<PointT>> candidates;
     std::vector<double> ringwise_flatness;
 
-    for (int zone_idx = 0; zone_idx < num_zones_; ++zone_idx) {
+    for (int 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 (int ring_idx = 0; ring_idx < params_.czm.num_rings_each_zone_[zone_idx]; ++ring_idx) {
+            for (int 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;
@@ -576,7 +465,7 @@ void PatchWorkpp<PointT>::estimate_ground(
                 double heading = 0.0;
                 for(int 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);
@@ -604,10 +493,10 @@ void PatchWorkpp<PointT>::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;
 
                 /*
@@ -656,7 +545,7 @@ void PatchWorkpp<PointT>::estimate_ground(
 
             if (!candidates.empty())
             {
-                if (enable_TGR_)
+                if (params_.enable_TGR_)
                 {
                     temporal_ground_revert(cloud_ground, cloud_nonground, ringwise_flatness, candidates, concentric_idx);
                 }
@@ -695,7 +584,7 @@ void PatchWorkpp<PointT>::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);
@@ -724,7 +613,7 @@ void PatchWorkpp<PointT>::estimate_ground(
         pub_vertical_.publish(cloud_ROS);
     }
 
-    if(visualize_)
+    if(params_.visualize_)
     {
         plane_viz_.publish(poly_list_);
     }
@@ -734,34 +623,36 @@ void PatchWorkpp<PointT>::estimate_ground(
     normals_.clear();
     noise_pc_.clear();
     vertical_pc_.clear();
+
+    return true;
 }
 
 template<typename PointT> inline
 void PatchWorkpp<PointT>::update_elevation_thr(void)
 {
-    for (int i=0; i<num_rings_of_interest_; i++)
+    for (int i=0; i<params_.num_rings_of_interest_; i++)
     {
         if (update_elevation_[i].empty()) continue;
 
         double update_mean = 0.0, update_stdev = 0.0;
         calc_mean_stdev(update_elevation_[i], update_mean, update_stdev);
         if (i==0) {
-            elevation_thr_[i] = update_mean + 3*update_stdev;
-            sensor_height_ = -update_mean;
+            params_.czm.elevation_thr_[i] = update_mean + 3*update_stdev;
+            params_.sensor_height_ = -update_mean;
         }
-        else elevation_thr_[i] = update_mean + 2*update_stdev;
+        else params_.czm.elevation_thr_[i] = update_mean + 2*update_stdev;
 
-        // if (verbose_) cout << "elevation threshold [" << i << "]: " << elevation_thr_[i] << endl;
+        // if (params_ .verbose_) cout << "elevation threshold [" << i << "]: " << params_.czm.elevation_thr_[i] << endl;
 
-        int exceed_num = update_elevation_[i].size() - max_elevation_storage_;
+        int exceed_num = update_elevation_[i].size() - params_.max_elevation_storage_;
         if (exceed_num > 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;
@@ -770,25 +661,25 @@ void PatchWorkpp<PointT>::update_elevation_thr(void)
 template<typename PointT> inline
 void PatchWorkpp<PointT>::update_flatness_thr(void)
 {
-    for (int i=0; i<num_rings_of_interest_; i++)
+    for (int i=0; i<params_.num_rings_of_interest_; i++)
     {
         if (update_flatness_[i].empty()) break;
         if (update_flatness_[i].size() <= 1) break;
 
         double update_mean = 0.0, update_stdev = 0.0;
         calc_mean_stdev(update_flatness_[i], update_mean, update_stdev);
-        flatness_thr_[i] = update_mean+update_stdev;
+        params_.czm.flatness_thr_[i] = update_mean+update_stdev;
 
-        // if (verbose_) { cout << "flatness threshold [" << i << "]: " << flatness_thr_[i] << endl; }
+        // if (params_ .verbose_) { cout << "flatness threshold [" << i << "]: " << params_.czm.flatness_thr_[i] << endl; }
 
-        int exceed_num = update_flatness_[i].size() - max_flatness_storage_;
+        int exceed_num = update_flatness_[i].size() - params_.max_flatness_storage_;
         if (exceed_num > 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;
@@ -799,12 +690,12 @@ void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &cloud_
                                                std::vector<double> ring_flatness, std::vector<RevertCandidate<PointT>> candidates,
                                                int 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;
@@ -815,7 +706,7 @@ void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &cloud_
         RevertCandidate<PointT> candidate = candidates[i];
 
         // Debug
-        if(verbose_)
+        if(params_ .verbose_)
         {
             cout << "\033[1;33m" << candidate.sector_idx << "th flat_sector_candidate"
                  << " / flatness: " << candidate.ground_flatness
@@ -827,22 +718,22 @@ void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &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;
                 }
@@ -852,7 +743,7 @@ void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &cloud_
             }
             else
             {
-                if (verbose_)
+                if (params_ .verbose_)
                 {
                     cout << "\033[1;31m" << "FINAL REJECT" << "\033[0m" << endl;
                 }
@@ -862,7 +753,7 @@ void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &cloud_
         }
     }
 
-    if (verbose_) std::cout << "\033[1;34m" << "====================================================" << "\033[0m" << endl;
+    if (params_ .verbose_) std::cout << "\033[1;34m" << "====================================================" << "\033[0m" << endl;
 }
 
 // For adaptive
@@ -882,14 +773,14 @@ void PatchWorkpp<PointT>::extract_piecewiseground(
     pcl::PointCloud<PointT> 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<PointT> src_tmp;
                 src_tmp = src_wo_verticals;
@@ -904,7 +795,7 @@ void PatchWorkpp<PointT>::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 {
@@ -929,7 +820,7 @@ void PatchWorkpp<PointT>::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();
 
@@ -937,12 +828,12 @@ void PatchWorkpp<PointT>::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]);
@@ -950,7 +841,7 @@ void PatchWorkpp<PointT>::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);
     }
 }
@@ -962,16 +853,16 @@ geometry_msgs::PolygonStamped PatchWorkpp<PointT>::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;
@@ -979,21 +870,21 @@ geometry_msgs::PolygonStamped PatchWorkpp<PointT>::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;
@@ -1010,10 +901,10 @@ void PatchWorkpp<PointT>::set_ground_likelihood_estimation_status(
         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);
@@ -1070,17 +961,17 @@ void PatchWorkpp<PointT>::pc2czm(const pcl::PointCloud<PointT> &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<int>(((r - min_ranges_[zone_idx]) / ring_sizes_[zone_idx])), num_rings_each_zone_[zone_idx] - 1);
-            int sector_idx = min(static_cast<int>((theta / sector_sizes_[zone_idx])), num_sectors_each_zone_[zone_idx] - 1);
+            int ring_idx = min(static_cast<int>(((r - params_.min_ranges_[zone_idx]) / params_.ring_sizes_[zone_idx])), params_.czm.num_rings_each_zone_[zone_idx] - 1);
+            int sector_idx = min(static_cast<int>((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);
         }
@@ -1089,7 +980,7 @@ void PatchWorkpp<PointT>::pc2czm(const pcl::PointCloud<PointT> &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/src/demo.cpp b/src/demo.cpp
index 7da6389..0628f1a 100644
--- a/src/demo.cpp
+++ b/src/demo.cpp
@@ -38,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) {
diff --git a/src/demo_reconf.cpp b/src/demo_reconf.cpp
index c90b40c..13bb42d 100644
--- a/src/demo_reconf.cpp
+++ b/src/demo_reconf.cpp
@@ -47,14 +47,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);
-
-    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));
+    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) {

From d8031a8c886805f21c9b933da8bd41b2c9c838ef Mon Sep 17 00:00:00 2001
From: Julian Gaal <julian.gaal@anavs.de>
Date: Fri, 26 May 2023 12:10:39 +0200
Subject: [PATCH 4/8] full support for dynamic_reconfigure

---
 CMakeLists.txt                      |   1 +
 cfg/patchwork.cfg                   |   2 +-
 include/patchworkpp/params.hpp      | 128 +++++++++++++++++++---------
 include/patchworkpp/patchworkpp.hpp |  96 ++++++++++++---------
 include/patchworkpp/utils.hpp       |  88 ++++++++++---------
 launch/demo.launch                  |   2 +-
 launch/demo_reconf.launch           |   7 +-
 package.xml                         |   1 -
 params/params.yaml                  |   4 +-
 src/demo.cpp                        |   5 +-
 src/demo_reconf.cpp                 |  49 +++++------
 11 files changed, 224 insertions(+), 159 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index a4c8877..015bf17 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -76,6 +76,7 @@ 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})
+target_compile_options(demo_reconf PRIVATE -Wsign-compare -Wunused-variable)
 add_dependencies(demo_reconf patchworkpp_generate_messages_cpp)
 
 add_executable(video src/video.cpp)
diff --git a/cfg/patchwork.cfg b/cfg/patchwork.cfg
index 28b75eb..d661024 100644
--- a/cfg/patchwork.cfg
+++ b/cfg/patchwork.cfg
@@ -8,6 +8,7 @@ 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)
 
@@ -25,7 +26,6 @@ gen.add("uprightness_thr", double_t, 0, "Threshold of uprightness using in Groun
 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_zones", int_t, 0, "Number of zones in czm mode", 4, 0, 10)
 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")
diff --git a/include/patchworkpp/params.hpp b/include/patchworkpp/params.hpp
index 6fb8fb2..c555877 100644
--- a/include/patchworkpp/params.hpp
+++ b/include/patchworkpp/params.hpp
@@ -11,16 +11,15 @@
 
 class ParamsHandler {
 public:
-    ParamsHandler(std::recursive_mutex& mutex) : mutex_(mutex)  {
+    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);
-        std::unique_lock<std::recursive_mutex> lock(mutex_);
-        validate();
     }
 
-    ParamsHandler(const ros::NodeHandle& nh, std::recursive_mutex& mutex) : mutex_(mutex) {
+    ParamsHandler(const ros::NodeHandle& nh, std::recursive_mutex& mutex) : mutex_(mutex), czm() {
         nh.param("verbose", verbose_, false);
-        nh.param("verbose", visualize_, true);
+        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);
@@ -42,13 +41,15 @@ class ParamsHandler {
         nh.param("enable_TGR", enable_TGR_, true);
 
         // CZM denotes 'Concentric Zone Model'. Please refer to our paper
-        nh.getParam("czm/num_zones", czm.num_zones_);
         nh.getParam("czm/num_sectors_each_zone", czm.num_sectors_each_zone_);
         nh.getParam("czm/num_rings_each_zone", czm.num_rings_each_zone_);
         nh.getParam("czm/elevation_thresholds", czm.elevation_thr_);
         nh.getParam("czm/flatness_thresholds", czm.flatness_thr_);
 
-        validate();
+        nh.param<std::string>("cloud_topic", cloud_topic_, "/pointcloud");
+
+        czm.num_zones_ = 4;
+        params_valid_ = validate();
 
         num_rings_of_interest_ = czm.elevation_thr_.size();
 
@@ -65,22 +66,28 @@ class ParamsHandler {
                         2 * M_PI / czm.num_sectors_each_zone_.at(2),
                         2 * M_PI / czm.num_sectors_each_zone_.at(3)};
 
-        initialized_ = true;
-        
     }
 
     void print_params() const {
+        std::unique_lock<std::recursive_mutex> 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: %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("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: %d", czm.num_zones_);
+        ROS_INFO("Num. zones: %ld", czm.num_zones_);
         ROS_INFO_STREAM((boost::format("Num. sectors: %d, %d, %d, %d") % 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());
@@ -96,16 +103,39 @@ class ParamsHandler {
                  czm.flatness_thr_[3]).str());
     }
 
-    bool validate() {
-        return  check(czm.num_zones_ != 4, "Number of zones must be four!") &&\
-                check(czm.num_zones_ == czm.num_sectors_each_zone_.size(), "num_zones and length of num_sectors_each_zone must match") &&\
+    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<std::recursive_mutex> lock(mutex_);
+        auto result = czm_changed_;
+        czm_changed_ = false;
+        return result;
+    }
+
+    std::pair<bool, std::string> topic_changed() {
+        std::unique_lock<std::recursive_mutex> lock(mutex_);
+        auto result = topic_changed_;
+        topic_changed_ = false;
+        return { result, cloud_topic_ };
+    }
+
+    std::string topic() {
+        std::unique_lock<std::recursive_mutex> lock(mutex_);
+        return cloud_topic_;
+    }
+
     std::string mode_;
-    bool initialized_;
+    std::string cloud_topic_;
+    bool params_valid_;
     bool verbose_;
     bool visualize_;
     double sensor_height_;
@@ -134,7 +164,7 @@ class ParamsHandler {
 
     struct CZM
     {
-        int num_zones_;
+        size_t num_zones_;
         std::vector<int> num_sectors_each_zone_;
         std::vector<int> num_rings_each_zone_;
         std::vector<double> elevation_thr_;
@@ -161,32 +191,51 @@ class ParamsHandler {
         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;
 
-        czm.num_zones_ = config.czm_num_zones;
-        czm.num_sectors_each_zone_ = convert_string_to_vector<int>(config.czm_num_sectors_each_zone);
-        czm.num_rings_each_zone_ = convert_string_to_vector<int>(config.czm_num_rings_each_zone);
-        czm.elevation_thr_ = convert_string_to_vector<double>(config.czm_elevation_thresholds);
-        czm.flatness_thr_ = convert_string_to_vector<double>(config.czm_flatness_thresholds);
+        if (cloud_topic_ != config.cloud_topic) {
+            topic_changed_ = true;
+        }
+        cloud_topic_ = config.cloud_topic;
 
-        num_rings_of_interest_ = czm.elevation_thr_.size();
+        auto num_sectors_each_zone = convert_string_to_vector<int>(config.czm_num_sectors_each_zone);
+        auto num_rings_each_zone = convert_string_to_vector<int>(config.czm_num_rings_each_zone);
+        auto elevation_thr = convert_string_to_vector<double>(config.czm_elevation_thresholds);
+        auto flatness_thr = convert_string_to_vector<double>(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;
+        }
 
-        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;
+        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;
 
-        validate();
+        params_valid_ = validate();
+        if (params_valid_)
+        {
+            num_rings_of_interest_ = czm.elevation_thr_.size();
 
-        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)};
+            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;
 
-        ROS_INFO("Updated params");
-        initialized_ = true;
+            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)};
+            ROS_INFO("Updated params");
+        } else {
+            ROS_WARN("Parameter update failed");
+        }
     }
 
 
@@ -205,13 +254,14 @@ class ParamsHandler {
                 ROS_WARN_STREAM("Can't convert " << token << " to number");
                 return result;
             }
+            result.push_back(num);
         }
         
         return result;
     }
 
 
-    bool check(bool assertion, std::string description) {
+    bool check(bool assertion, std::string description) const {
         if (not assertion) {
             ROS_WARN_STREAM(description);
             return false;
@@ -220,6 +270,8 @@ class ParamsHandler {
         return true;
     }
 
+    bool czm_changed_;
+    bool topic_changed_;
     std::recursive_mutex& mutex_;
     dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server_;
 };
diff --git a/include/patchworkpp/patchworkpp.hpp b/include/patchworkpp/patchworkpp.hpp
index eabc4a7..d427e49 100755
--- a/include/patchworkpp/patchworkpp.hpp
+++ b/include/patchworkpp/patchworkpp.hpp
@@ -46,7 +46,7 @@ bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; }
 template <typename PointT>
 struct RevertCandidate
 {
-    int concentric_idx;
+    size_t concentric_idx;
     int sector_idx;
     double ground_flatness;
     double line_variable;
@@ -65,8 +65,6 @@ class PatchWorkpp {
     typedef std::vector<Ring> Zone;
 
     PatchWorkpp() : mutex_(), params_(mutex_), using_reconf_(true) {
-        params_.print_params();
-
         plane_viz_       = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("plane", 100, true);
         pub_revert_pc_   = node_handle_.advertise<sensor_msgs::PointCloud2>("revert_pc", 100, true);
         pub_reject_pc_   = node_handle_.advertise<sensor_msgs::PointCloud2>("reject_pc", 100, true);
@@ -79,10 +77,13 @@ class PatchWorkpp {
         regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);
         regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);
 
-        reset_poly_list();
-        reset_concentric_zone_model();
-
-        ROS_INFO("INITIALIZATION COMPLETE");
+        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) {
@@ -109,6 +110,8 @@ class PatchWorkpp {
     bool estimate_ground(pcl::PointCloud<PointT> cloud_in,
                          pcl::PointCloud<PointT> &cloud_ground, pcl::PointCloud<PointT> &cloud_nonground, double &time_taken);
 
+    std::pair<bool, std::string> topic_changed();
+    std::string topic();
 private:
 
     // Every private member variable is written with the undescore("_") in its end.
@@ -161,7 +164,7 @@ class PatchWorkpp {
 
     void temporal_ground_revert(pcl::PointCloud<PointT> &cloud_ground, pcl::PointCloud<PointT> &cloud_nonground,
                                 std::vector<double> ring_flatness, std::vector<RevertCandidate<PointT>> candidates,
-                                int concentric_idx);
+                                size_t concentric_idx);
 
     void calc_mean_stdev(std::vector<double> vec, double &mean, double &stdev);
 
@@ -194,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);
@@ -226,9 +229,9 @@ void PatchWorkpp<PointT>::flush_patches_in_zone(Zone &patches, int num_sectors,
 
 template<typename PointT> inline
 void PatchWorkpp<PointT>::flush_patches(vector<Zone> &czm) {
-    for (int k = 0; k < params_.czm.num_zones_; k++) {
-        for (int i = 0; i < params_.czm.num_rings_each_zone_[k]; i++) {
-            for (int j = 0; j < params_.czm.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();
             }
         }
@@ -248,7 +251,7 @@ void PatchWorkpp<PointT>::reset_poly_list() {
 template<typename PointT> inline
 void PatchWorkpp<PointT>::reset_concentric_zone_model() {
     ConcentricZoneModel_.clear();
-    for (int i = 0; i < params_.czm.num_zones_; i++) {
+    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);
@@ -284,9 +287,9 @@ void PatchWorkpp<PointT>::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++) {
+        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 {
@@ -296,14 +299,14 @@ void PatchWorkpp<PointT>::extract_initial_seeds(
     }
 
     // Calculate the mean height value.
-    for (int i = init_idx; i < p_sorted.points.size() && cnt < params_.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+params_.th_seeds_
-    for (int i = 0; i < p_sorted.points.size(); i++) {
+    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]);
         }
@@ -322,7 +325,7 @@ void PatchWorkpp<PointT>::extract_initial_seeds(
 
     int init_idx = 0;
     if (zone_idx == 0) {
-        for (int i = 0; i < p_sorted.points.size(); i++) {
+        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 {
@@ -332,14 +335,14 @@ void PatchWorkpp<PointT>::extract_initial_seeds(
     }
 
     // Calculate the mean height value.
-    for (int i = init_idx; i < p_sorted.points.size() && cnt < params_.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+params_.th_seeds_
-    for (int i = 0; i < p_sorted.points.size(); i++) {
+    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]);
         }
@@ -349,7 +352,7 @@ void PatchWorkpp<PointT>::extract_initial_seeds(
 template<typename PointT> inline
 void PatchWorkpp<PointT>::reflected_noise_removal(pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_nonground)
 {
-    for (int i=0; i<cloud_in.size(); i++)
+    for (size_t i = 0; i < cloud_in.size(); i++)
     {
         double r = sqrt( cloud_in[i].x*cloud_in[i].x + cloud_in[i].y*cloud_in[i].y );
         double z = cloud_in[i].z;
@@ -366,6 +369,16 @@ void PatchWorkpp<PointT>::reflected_noise_removal(pcl::PointCloud<PointT> &cloud
     if (params_ .verbose_) cout << "[ RNR ] Num of noises : " << noise_pc_.points.size() << endl;
 }
 
+template<typename PointT> inline
+std::pair<bool, std::string> PatchWorkpp<PointT>::topic_changed() {
+    return params_.topic_changed();
+}
+
+template<typename PointT> inline
+std::string PatchWorkpp<PointT>::topic() {
+    return params_.topic();
+}
+
 /*
     @brief Velodyne pointcloud callback function. The main GPF pipeline is here.
     PointCloud SensorMsg -> Pointcloud -> z-value sorted Pointcloud
@@ -381,14 +394,15 @@ bool PatchWorkpp<PointT>::estimate_ground(
     
     unique_lock<recursive_mutex> lock(mutex_);
     
-    if (not params_.initialized_ || params_.num_iter_ < 1) {
-        ROS_WARN_STREAM("Waiting for initialization...");
+    if (not params_.params_valid_) {
+        ROS_WARN_STREAM_THROTTLE(1, "Invalid Parameters...");
         return false;
     }
 
-    if (using_reconf_ && not params_.validate()) {
-        ROS_WARN_STREAM("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();
@@ -396,7 +410,7 @@ bool PatchWorkpp<PointT>::estimate_ground(
     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;
@@ -419,19 +433,19 @@ bool PatchWorkpp<PointT>::estimate_ground(
 
     t2 = ros::Time::now().toSec();
 
-    int concentric_idx = 0;
+    size_t concentric_idx = 0;
 
     double t_sort = 0;
 
     std::vector<RevertCandidate<PointT>> candidates;
     std::vector<double> ringwise_flatness;
 
-    for (int zone_idx = 0; zone_idx < params_.czm.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 < params_.czm.num_rings_each_zone_[zone_idx]; ++ring_idx) {
-            for (int sector_idx = 0; sector_idx < params_.czm.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() < params_.num_min_pts_)
                 {
@@ -463,7 +477,9 @@ bool PatchWorkpp<PointT>::estimate_ground(
                 const double line_variable      = singular_values_(1) != 0 ? singular_values_(0)/singular_values_(1) : std::numeric_limits<double>::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 (params_.visualize_) {
                     auto polygons = set_polygons(zone_idx, ring_idx, sector_idx, 3);
@@ -551,7 +567,7 @@ bool PatchWorkpp<PointT>::estimate_ground(
                 }
                 else
                 {
-                    for (size_t i=0; i<candidates.size(); i++)
+                    for (size_t i = 0; i < candidates.size(); i++)
                     {
                         cloud_nonground += candidates[i].regionwise_ground;
                     }
@@ -569,8 +585,6 @@ bool PatchWorkpp<PointT>::estimate_ground(
         }
     }
 
-    double t_update = ros::Time::now().toSec();
-
     update_elevation_thr();
     update_flatness_thr();
 
@@ -630,7 +644,7 @@ bool PatchWorkpp<PointT>::estimate_ground(
 template<typename PointT> inline
 void PatchWorkpp<PointT>::update_elevation_thr(void)
 {
-    for (int i=0; i<params_.num_rings_of_interest_; i++)
+    for (size_t i=0; i < params_.num_rings_of_interest_; i++)
     {
         if (update_elevation_[i].empty()) continue;
 
@@ -661,7 +675,7 @@ void PatchWorkpp<PointT>::update_elevation_thr(void)
 template<typename PointT> inline
 void PatchWorkpp<PointT>::update_flatness_thr(void)
 {
-    for (int i=0; i<params_.num_rings_of_interest_; i++)
+    for (size_t i=0; i < params_.num_rings_of_interest_; i++)
     {
         if (update_flatness_[i].empty()) break;
         if (update_flatness_[i].size() <= 1) break;
@@ -688,7 +702,7 @@ void PatchWorkpp<PointT>::update_flatness_thr(void)
 template<typename PointT> inline
 void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &cloud_ground, pcl::PointCloud<PointT> &cloud_nonground,
                                                std::vector<double> ring_flatness, std::vector<RevertCandidate<PointT>> candidates,
-                                               int concentric_idx)
+                                               size_t concentric_idx)
 {
     if (params_ .verbose_) std::cout << "\033[1;34m" << "=========== Temporal Ground Revert (TGR) ===========" << "\033[0m" << endl;
 
@@ -743,7 +757,7 @@ void PatchWorkpp<PointT>::temporal_ground_revert(pcl::PointCloud<PointT> &cloud_
             }
             else
             {
-                if (params_ .verbose_)
+                if (params_.verbose_)
                 {
                     cout << "\033[1;31m" << "FINAL REJECT" << "\033[0m" << endl;
                 }
@@ -897,7 +911,7 @@ geometry_msgs::PolygonStamped PatchWorkpp<PointT>::set_polygons(int zone_idx, in
 template<typename PointT> inline
 void PatchWorkpp<PointT>::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) {
@@ -927,7 +941,7 @@ void PatchWorkpp<PointT>::calc_mean_stdev(std::vector<double> vec, double &mean,
 
     mean = std::accumulate(vec.begin(), vec.end(), 0.0) / vec.size();
 
-    for (int i=0; i<vec.size(); i++) { stdev += (vec.at(i)-mean)*(vec.at(i)-mean); }
+    for (size_t i=0; i < vec.size(); i++) { stdev += (vec.at(i)-mean)*(vec.at(i)-mean); }
     stdev /= vec.size()-1;
     stdev = sqrt(stdev);
 }
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<PointXYZILID>& src,
-                       pcl::PointCloud<pcl::PointXYZI>::Ptr dst){
+                       pcl::PointCloud<pcl::PointXYZI>::Ptr dst) {
   dst->points.clear();
   for (const auto &pt: src.points){
     pcl::PointXYZI pt_xyzi;
@@ -86,7 +86,7 @@ std::vector<int> ground_classes = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_
 std::vector<int> ground_classes_except_terrain = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING};
 std::vector<int> traversable_ground_classes = {ROAD, PARKING, LANE_MARKING, OTHER_GROUND};
 
-int count_num_ground(const pcl::PointCloud<PointXYZILID>& pc){
+int count_num_ground(const pcl::PointCloud<PointXYZILID>& pc) {
   int num_ground = 0;
 
   std::vector<int>::iterator iter;
@@ -94,17 +94,19 @@ int count_num_ground(const pcl::PointCloud<PointXYZILID>& 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<PointXYZILID>& pc){
+int count_num_ground_without_vegetation(const pcl::PointCloud<PointXYZILID>& pc) {
   int num_ground = 0;
 
   std::vector<int>::iterator iter;
@@ -113,34 +115,35 @@ int count_num_ground_without_vegetation(const pcl::PointCloud<PointXYZILID>& 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<int, int> set_initial_gt_counts(std::vector<int>& gt_classes){
+std::map<int, int> set_initial_gt_counts(std::vector<int>& gt_classes) {
   map<int, int> 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<int,int>(gt_classes.at(i), 0));
   }
   return gt_counts;
 }
 
-std::map<int, int> count_num_each_class(const pcl::PointCloud<PointXYZILID>& pc){
-  int num_ground = 0;
+std::map<int, int> count_num_each_class(const pcl::PointCloud<PointXYZILID>& pc) {
   auto gt_counts = set_initial_gt_counts(ground_classes);
   std::vector<int>::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<PointXYZILID>& src, pcl::PointCloud<Po
   ground.clear();
   non_ground.clear();
   std::vector<int>::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<PointXYZILID>& src,
   ground.clear();
   non_ground.clear();
   std::vector<int>::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<PointXYZILID>& pc_curr,
                                 pcl::PointCloud<PointXYZILID>& 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<PointXY
                                                    pcl::PointCloud<PointXYZILID>& 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<PointXYZILID>& pc, string ABS_DIR, string seq, int count){
+void save_all_labels(const pcl::PointCloud<PointXYZILID>& 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<PointXYZILID>& 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<<labels[i]<<",";
-    }else{
+    } else {
       sc_output<<labels[i]<<endl;
     }
   }
@@ -297,7 +303,7 @@ void save_all_labels(const pcl::PointCloud<PointXYZILID>& pc, string ABS_DIR, st
 
 void save_all_accuracy(const pcl::PointCloud<PointXYZILID>& pc_curr,
                       pcl::PointCloud<PointXYZILID>& ground_estimated, string acc_filename,
-                      double& accuracy, map<int, int>&pc_curr_gt_counts, map<int, int>&g_est_gt_counts){
+                      double& accuracy, map<int, int>&pc_curr_gt_counts, map<int, int>&g_est_gt_counts) {
 
 
 //  std::cout<<"debug: "<<acc_filename<<std::endl;
@@ -319,7 +325,7 @@ void save_all_accuracy(const pcl::PointCloud<PointXYZILID>& 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<<g_est_gt_counts.find(class_id)->second<<","<<pc_curr_gt_counts.find(class_id)->second<<",";
   }
   sc_output2<<accuracy<<endl;
@@ -329,28 +335,28 @@ void save_all_accuracy(const pcl::PointCloud<PointXYZILID>& pc_curr,
 
 void pc2pcdfile(const pcl::PointCloud<PointXYZILID>& TP, const pcl::PointCloud<PointXYZILID>& FP,
                 const pcl::PointCloud<PointXYZILID>& FN, const pcl::PointCloud<PointXYZILID>& TN,
-                std::string pcd_filename){
+                std::string pcd_filename) {
   pcl::PointCloud<pcl::PointXYZI> 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 38d2129..bc6a1be 100755
--- a/launch/demo.launch
+++ b/launch/demo.launch
@@ -1,6 +1,6 @@
 <launch>
 
-<arg name="cloud_topic" default="/velodyne_points"/>
+<arg name="cloud_topic" default="/kitti/velo/pointcloud"/>
 
   <node name="ground_segmentation" pkg="patchworkpp" type="demo" output="screen">
     <rosparam command="load" file="$(find patchworkpp)/params/params.yaml" />
diff --git a/launch/demo_reconf.launch b/launch/demo_reconf.launch
index 3cf4554..8484730 100755
--- a/launch/demo_reconf.launch
+++ b/launch/demo_reconf.launch
@@ -1,13 +1,8 @@
 <launch>
 
-  <arg name="cloud_topic" default="/velodyne_points"/>
-  
   <node name="rqt_conf_gui" pkg="rqt_reconfigure" type="rqt_reconfigure" output="screen"/>
 
-  <node name="ground_segmentation" pkg="patchworkpp" type="demo_reconf" output="screen">
-    <rosparam command="load" file="$(find patchworkpp)/params/params.yaml" />
-    <param name="cloud_topic" value="$(arg cloud_topic)"/>
-  </node>
+  <node name="ground_segmentation" pkg="patchworkpp" type="demo_reconf" output="screen"/>
 
   <node name="$(anon rviz)" pkg="rviz" type="rviz" args="-d $(find patchworkpp)/rviz/demo.rviz"/>
 
diff --git a/package.xml b/package.xml
index ed6916a..863b0ae 100755
--- a/package.xml
+++ b/package.xml
@@ -31,7 +31,6 @@
   <build_depend>jsk_recognition_msgs</build_depend>
   <build_depend>laser_geometry</build_depend>
 
-
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
diff --git a/params/params.yaml b/params/params.yaml
index b3f494f..46c1e2a 100755
--- a/params/params.yaml
+++ b/params/params.yaml
@@ -2,7 +2,8 @@ save_flag: true
 
 # patchworkpp:
 
-sensor_height: 1.2
+sensor_height: 1.723
+point_topic: /kitti/velo/pointcloud
 
 mode: "czm"
 verbose: false   # To check effect of uprightness/elevation/flatness
@@ -22,7 +23,6 @@ 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]
     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.
diff --git a/src/demo.cpp b/src/demo.cpp
index 0628f1a..88af7dc 100644
--- a/src/demo.cpp
+++ b/src/demo.cpp
@@ -54,9 +54,6 @@ int main(int argc, char**argv) {
     ros::NodeHandle nh;
     ros::NodeHandle pnh("~");
 
-    std::string cloud_topic;
-    pnh.param<string>("cloud_topic", cloud_topic, "/pointcloud");
-
     ROS_INFO("Operating patchwork++...");
     PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>(pnh));
 
@@ -64,7 +61,7 @@ int main(int argc, char**argv) {
     pub_ground      = pnh.advertise<sensor_msgs::PointCloud2>("ground", 100, true);
     pub_non_ground  = pnh.advertise<sensor_msgs::PointCloud2>("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
index 13bb42d..9cae49a 100644
--- a/src/demo_reconf.cpp
+++ b/src/demo_reconf.cpp
@@ -3,6 +3,7 @@
 #define PCL_NO_PRECOMPILE
 
 #include <ros/ros.h>
+#include <ros/master.h>
 #include <signal.h>
 #include <dynamic_reconfigure/server.h>
 #include <sensor_msgs/PointCloud2.h>
@@ -27,16 +28,6 @@ sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud<T> cloud, const ros::Time& st
     return cloud_ROS;
 }
 
-// Callback function for parameter changes
-void reconfigureCallback(const patchworkpp::PatchworkppConfig& config, uint32_t level)
-{
-  // Update your parameters based on the changes
-  // You can access the parameters using config.param_name
-  // For example: int parameter = config.int_param;
-  // Your code to handle parameter changes goes here
-  ROS_INFO("Updated parameters");
-}
-
 void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg)
 {
     double time_taken;
@@ -63,9 +54,6 @@ int main(int argc, char**argv) {
     ros::NodeHandle nh;
     ros::NodeHandle pnh("~");
 
-    std::string cloud_topic;
-    pnh.param<string>("cloud_topic", cloud_topic, "/pointcloud");
-
     ROS_INFO("Operating patchwork++...");
     PatchworkppGroundSeg.reset(new PatchWorkpp<PointType>());
 
@@ -73,17 +61,30 @@ int main(int argc, char**argv) {
     pub_ground      = pnh.advertise<sensor_msgs::PointCloud2>("ground", 100, true);
     pub_non_ground  = pnh.advertise<sensor_msgs::PointCloud2>("nonground", 100, true);
 
-    ros::Subscriber sub_cloud = nh.subscribe(cloud_topic, 100, callbackCloud);
-
-    // Create a dynamic reconfigure server
-    dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig> server;
-    dynamic_reconfigure::Server<patchworkpp::PatchworkppConfig>::CallbackType f;
-
-    // Set the callback function
-    f = boost::bind(&reconfigureCallback, _1, _2);
-    server.setCallback(f);
-
-    ros::spin();
+    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<ros::master::TopicInfo> 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;
 }

From 0fc543ceaa2ca6751f894e1569655eb4a8b62542 Mon Sep 17 00:00:00 2001
From: Julian Gaal <julian.gaal@anavs.de>
Date: Fri, 26 May 2023 12:24:35 +0200
Subject: [PATCH 5/8] fix sign issues

---
 CMakeLists.txt                      |  1 -
 include/patchworkpp/params.hpp      | 28 +++++++++++++++++-----------
 include/patchworkpp/patchworkpp.hpp | 10 +++++-----
 3 files changed, 22 insertions(+), 17 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 015bf17..a4c8877 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -76,7 +76,6 @@ 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})
-target_compile_options(demo_reconf PRIVATE -Wsign-compare -Wunused-variable)
 add_dependencies(demo_reconf patchworkpp_generate_messages_cpp)
 
 add_executable(video src/video.cpp)
diff --git a/include/patchworkpp/params.hpp b/include/patchworkpp/params.hpp
index c555877..941608e 100644
--- a/include/patchworkpp/params.hpp
+++ b/include/patchworkpp/params.hpp
@@ -23,7 +23,9 @@ class ParamsHandler {
         nh.param("sensor_height", sensor_height_, 1.723);
         nh.param("num_iter", num_iter_, 3);
         nh.param("num_lpr", num_lpr_, 20);
-        nh.param("num_min_pts", num_min_pts_, 10);
+        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);
@@ -41,8 +43,12 @@ class ParamsHandler {
         nh.param("enable_TGR", enable_TGR_, true);
 
         // CZM denotes 'Concentric Zone Model'. Please refer to our paper
-        nh.getParam("czm/num_sectors_each_zone", czm.num_sectors_each_zone_);
-        nh.getParam("czm/num_rings_each_zone", czm.num_rings_each_zone_);
+        std::vector<int> 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<size_t>(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<size_t>(value); });
         nh.getParam("czm/elevation_thresholds", czm.elevation_thr_);
         nh.getParam("czm/flatness_thresholds", czm.flatness_thr_);
 
@@ -80,7 +86,7 @@ class ParamsHandler {
         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: %d", num_min_pts_);
+        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_);
@@ -88,10 +94,10 @@ class ParamsHandler {
         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: %d, %d, %d, %d") % czm.num_sectors_each_zone_[0] % czm.num_sectors_each_zone_[1] %
+        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: %01d, %01d, %01d, %01d") % czm.num_rings_each_zone_[0] %
+        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());
@@ -141,7 +147,7 @@ class ParamsHandler {
     double sensor_height_;
     int num_iter_;
     int num_lpr_;
-    int num_min_pts_;
+    size_t num_min_pts_;
     double th_seeds_;
     double th_dist_;
     double th_seeds_v_;
@@ -165,8 +171,8 @@ class ParamsHandler {
     struct CZM
     {
         size_t num_zones_;
-        std::vector<int> num_sectors_each_zone_;
-        std::vector<int> num_rings_each_zone_;
+        std::vector<size_t> num_sectors_each_zone_;
+        std::vector<size_t> num_rings_each_zone_;
         std::vector<double> elevation_thr_;
         std::vector<double> flatness_thr_;
     } czm;
@@ -199,8 +205,8 @@ class ParamsHandler {
         }
         cloud_topic_ = config.cloud_topic;
 
-        auto num_sectors_each_zone = convert_string_to_vector<int>(config.czm_num_sectors_each_zone);
-        auto num_rings_each_zone = convert_string_to_vector<int>(config.czm_num_rings_each_zone);
+        auto num_sectors_each_zone = convert_string_to_vector<size_t>(config.czm_num_sectors_each_zone);
+        auto num_rings_each_zone = convert_string_to_vector<size_t>(config.czm_num_rings_each_zone);
         auto elevation_thr = convert_string_to_vector<double>(config.czm_elevation_thresholds);
         auto flatness_thr = convert_string_to_vector<double>(config.czm_flatness_thresholds);;
         if (czm.num_sectors_each_zone_ != num_sectors_each_zone || \
diff --git a/include/patchworkpp/patchworkpp.hpp b/include/patchworkpp/patchworkpp.hpp
index d427e49..cbf3a50 100755
--- a/include/patchworkpp/patchworkpp.hpp
+++ b/include/patchworkpp/patchworkpp.hpp
@@ -135,7 +135,7 @@ class PatchWorkpp {
     Eigen::Matrix3f cov_;
     Eigen::Vector4f pc_mean_;
 
-    queue<int> noise_idxs_;
+    queue<size_t> noise_idxs_;
 
     vector<Zone> ConcentricZoneModel_;
 
@@ -966,8 +966,8 @@ double PatchWorkpp<PointT>::xy2radius(const double &x, const double &y) {
 template<typename PointT> inline
 void PatchWorkpp<PointT>::pc2czm(const pcl::PointCloud<PointT> &src, std::vector<Zone> &czm, pcl::PointCloud<PointT> &cloud_nonground) {
 
-    for (int i=0; i<src.size(); i++) {
-        if ((!noise_idxs_.empty()) &&(i == noise_idxs_.front())) {
+    for (size_t i = 0; i < src.size(); i++) {
+        if ((!noise_idxs_.empty()) && (i == noise_idxs_.front())) {
             noise_idxs_.pop();
             continue;
         }
@@ -984,8 +984,8 @@ void PatchWorkpp<PointT>::pc2czm(const pcl::PointCloud<PointT> &src, std::vector
             else if ( r < params_.min_ranges_[3] ) zone_idx = 2;
             else zone_idx = 3;
 
-            int ring_idx = min(static_cast<int>(((r - params_.min_ranges_[zone_idx]) / params_.ring_sizes_[zone_idx])), params_.czm.num_rings_each_zone_[zone_idx] - 1);
-            int sector_idx = min(static_cast<int>((theta / params_.sector_sizes_[zone_idx])), params_.czm.num_sectors_each_zone_[zone_idx] - 1);
+            size_t ring_idx = min(static_cast<size_t>(((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<size_t>((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);
         }

From 1a7512a964adea910220dfb23e465e1785047e56 Mon Sep 17 00:00:00 2001
From: Julian Gaal <julian.gaal@anavs.de>
Date: Fri, 26 May 2023 12:34:07 +0200
Subject: [PATCH 6/8] update instructions

---
 README.md | 9 +++++++++
 1 file changed, 9 insertions(+)

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

From 6c25aeb848f2583c1f711ee4978ede1f9b5056a0 Mon Sep 17 00:00:00 2001
From: Julian Gaal <julian.gaal@anavs.de>
Date: Fri, 26 May 2023 12:39:01 +0200
Subject: [PATCH 7/8] no redundant setting of c++ standard in cmake

---
 CMakeLists.txt | 17 +++++++----------
 1 file changed, 7 insertions(+), 10 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index a4c8877..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

From 87621dfeb89082dd8798c3d847c53dc3a3642025 Mon Sep 17 00:00:00 2001
From: Julian Gaal <julian.gaal@anavs.de>
Date: Fri, 26 May 2023 12:48:19 +0200
Subject: [PATCH 8/8] share code to set ranges, rings and sectors between
 reconf/!reconf

---
 include/patchworkpp/params.hpp | 51 ++++++++++++++--------------------
 1 file changed, 21 insertions(+), 30 deletions(-)

diff --git a/include/patchworkpp/params.hpp b/include/patchworkpp/params.hpp
index 941608e..4598fa3 100644
--- a/include/patchworkpp/params.hpp
+++ b/include/patchworkpp/params.hpp
@@ -56,22 +56,9 @@ class ParamsHandler {
 
         czm.num_zones_ = 4;
         params_valid_ = validate();
-
-        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)};
-
+        if (params_valid_) {
+            set_ranges_rings_sectors();
+        }
     }
 
     void print_params() const {
@@ -224,20 +211,7 @@ class ParamsHandler {
         params_valid_ = validate();
         if (params_valid_)
         {
-            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)};
+            set_ranges_rings_sectors();
             ROS_INFO("Updated params");
         } else {
             ROS_WARN("Parameter update failed");
@@ -266,6 +240,23 @@ class ParamsHandler {
         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) {