v0.0

init
hitbg-yjm · Nov 2, 2020 · 0ed27c4 · 0ed27c4
1 parent 8e42e0a
commit 0ed27c4
Show file tree

Hide file tree

Showing 14 changed files with 2,041 additions and 0 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -0,0 +1,95 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(removert)
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_STANDARD 17) # C++17 to use stdc++fs
+# set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread")
+
+find_package(catkin REQUIRED COMPONENTS
+  tf
+  roscpp
+  rospy
+  cv_bridge
+  # pcl library
+  pcl_conversions
+  # msgs
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  nav_msgs
+  message_generation
+  image_transport
+)
+
+find_package(OpenMP REQUIRED)
+find_package(PCL REQUIRED QUIET)
+find_package(OpenCV REQUIRED QUIET)
+# find_package(GTSAM REQUIRED QUIET)
+
+# add_message_files(
+#   DIRECTORY msg
+#   FILES
+#   # cloud_info.msg
+# )
+
+# generate_messages(
+#   DEPENDENCIES
+#   geometry_msgs
+#   std_msgs
+#   nav_msgs
+#   sensor_msgs
+# )
+
+catkin_package(
+  INCLUDE_DIRS include
+  DEPENDS PCL 
+  # GTSAM
+
+  CATKIN_DEPENDS 
+  std_msgs
+  nav_msgs
+  geometry_msgs
+  sensor_msgs
+  message_runtime 
+  message_generation
+)
+
+# include directories
+include_directories(
+	include
+	${catkin_INCLUDE_DIRS}
+	${PCL_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+	# ${GTSAM_INCLUDE_DIR}
+)
+
+# link directories
+link_directories(
+	include
+	${PCL_LIBRARY_DIRS}
+  ${OpenCV_LIBRARY_DIRS}
+  # ${GTSAM_LIBRARY_DIRS}
+)
+
+###########
+## Build ##
+###########
+
+# Range Image Projection
+add_executable( ${PROJECT_NAME}_removert src/removert_main.cpp
+  src/utility.cpp 
+  src/RosParamServer.cpp
+  src/Removerter.cpp
+)
+
+add_dependencies(${PROJECT_NAME}_removert ${catkin_EXPORTED_TARGETS})
+target_compile_options(${PROJECT_NAME}_removert PRIVATE ${OpenMP_CXX_FLAGS})
+target_link_libraries(${PROJECT_NAME}_removert 
+  ${catkin_LIBRARIES} 
+  ${PCL_LIBRARIES} 
+  ${OpenCV_LIBRARIES}
+  ${OpenMP_CXX_FLAGS}
+  stdc++fs
+)
+
+
diff --git a/config/params.yaml b/config/params.yaml
@@ -0,0 +1,81 @@
+removert:
+
+  # @ NOTE: Recommended test regions and corresponding parameters
+  # - KITTI 09 [downsample_voxel_size: 0.05m, skips: 5, start idx: 1300, end idx: 1600] 
+  # - KITTI 03 [downsample_voxel_size: 0.05m, skips: 5, start idx: 0, end idx: 150] 
+
+
+  # @ save option 
+  saveMapPCD: true 
+  saveCleanScansPCD: true
+  save_pcd_directory: "/home/user/removert/results/" # replace to your path (please use an absolute path)
+
+
+  # @ sequence info (replace to your paths) 
+  #   please follow the KITTI odometry dataset's scan and pose formats (i.e., make them .bin and line-by-line corresponding se3 poses composed of 12 numbers)
+  sequence_scan_dir: "/media/user/GS1TB/KITTI/dataset/sequences/09/velodyne/"
+  sequence_pose_path: "/media/user/GS1TB/KITTI/dataset/sequences/09/poses.txt"
+
+  sequence_vfov: 50 # including upper and lower fovs, for example, KITTI's HDL64E is 2 + 24.9 ~ 27 deg. (so 25 + 25 = 50 is recommended because it is enough)
+  sequence_hfov: 360 # generally for targetting scanning LiDAR but a lidar having restricted hfov also just can use the 360 (because no point regions are considered in the algorithm)
+
+
+  # @ Sequence's BaseToLidar info
+  ExtrinsicLiDARtoPoseBase: # this is for the KITTI (in the removert example, using SuMa poses written in the camera coord)
+      [-1.857e-03, -9.999e-01, -8.039e-03, -4.784e-03,
+       -6.481e-03,  8.0518e-03, -9.999e-01, -7.337e-02,
+        9.999e-01, -1.805e-03, -6.496e-03, -3.339e-01,
+        0.0,        0.0,        0.0,        1.0]
+  # @ If you use the lidar-itself odometry (e.g., from A-LOAM or using odometry saver https://github.com/SMRT-AIST/interactive_slam/wiki/Example2), 
+  #   use the below identity matrix, not the above KITTI pose setting (w.r.t the camera).
+  # ExtrinsicLiDARtoPoseBase: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1]
+
+
+  # @ Sampling nodes 
+  use_keyframe_gap: true 
+  keyframe_gap: 5 # i.e., 5 means every 5 frames // 5 is recommended for fast removing while preserving the static map's quality # if you make all scans clean, use this value = 1
+
+  # use_keyframe_meter: false # TODO
+  # keyframe_meter: 2 # TODO, e.g., every 2 meters (using the given pose relative translations) 
+
+
+  # @ target region (scan idx range) when clean_for_all_scan is false 
+  #   means the removeter runs on all scans in a specified regions of a sequence 
+  #   it means we parse scans every <keyframe_gap> or <keyframe_gap_meter> meter within the idx range [start_idx, end_idx] 
+  #   but for a single batch, < 100 scans are recommended for the computation speed.
+  start_idx: 1300 # change this
+  end_idx: 1500 # change this
+
+
+  # @ auto repeat
+  #   means the removeter runs on all scans in a sequence 
+  clean_for_all_scan: false # default is false
+  batch_size: 150
+  valid_ratio_to_save: 0.75 # i.e., no save cleaned scans located at the boundaries of a batch  
+
+
+  # @ Range image resolution
+  # the below is actually magnifier ratio (i.e., 5 means x5 resolution, the x1 means 1 deg x 1 deg per pixel)
+  # - recommend to use the first removing resolution's magnifier ratio should meet the seonsor vertical fov / number of rays 
+  #     - e.g., HDL 64E of KITTI dataset -> appx 25 deg / 64 ray ~ 0.4 deg per pixel -> the magnifier ratio = 1/0.4 = 2.5
+  # - recommend to use the first reverting resolution's magnifier ratio should lied in 1.0 to 1.5
+  # remove_resolution_list: [2.5, 2.0, 1.5] 
+  remove_resolution_list: [2.5, 1.5] 
+  revert_resolution_list: [1.0, 0.9, 0.8, 0.7] # TODO
+
+
+  # @ Removert params 
+  # about density 
+  downsample_voxel_size: 0.05 # user parameter but recommend to use 0.05 to make sure an enough density (this value is related to the removing resolution's expected performance)
+  # about Static sensitivity
+  num_nn_points_within: 2 # how many - using higher, more strict static 
+  dist_nn_points_within: 0.1 # meter - using smaller, more strict static 
+
+
+  # @ For faster
+  num_omp_cores: 16 # for faster map points projection (to make a map range image)
+
+
+  # @ For visualization of range images (rviz -d removert_visualization.rviz)
+  rimg_color_min: 0.0 # meter
+  rimg_color_max: 20.0 # meter
diff --git a/include/removert/Removerter.h b/include/removert/Removerter.h
@@ -0,0 +1,132 @@
+#pragma once
+
+#include "removert/RosParamServer.h"
+
+class Removerter : public RosParamServer
+{
+private:
+
+    ros::Subscriber subLaserCloud; // TODO, for real-time integration (later)
+
+    const float kFlagNoPOINT = 10000.0; // no point constant, 10000 has no meaning, but must be larger than the maximum scan range (e.g., 200 meters)
+    const float kValidDiffUpperBound = 200.0; // must smaller than kFlagNoPOINT
+
+    // Static sensitivity
+    int kNumKnnPointsToCompare;// static sensitivity (increase this value, less static structure will be removed at the scan-side removal stage)
+    float kScanKnnAndMapKnnAvgDiffThreshold; // static sensitivity (decrease this value, less static structure will be removed at the scan-side removal stage)
+
+    std::vector<std::string> sequence_valid_scan_names_;
+    std::vector<std::string> sequence_valid_scan_paths_;
+    std::vector<pcl::PointCloud<PointType>::Ptr> scans_;
+    std::vector<pcl::PointCloud<PointType>::Ptr> scans_static_;
+    std::vector<pcl::PointCloud<PointType>::Ptr> scans_dynamic_;
+
+    std::string scan_static_save_dir_;
+    std::string scan_dynamic_save_dir_;
+    std::string map_static_save_dir_;
+    std::string map_dynamic_save_dir_;
+
+    std::vector<Eigen::Matrix4d> scan_poses_;
+    std::vector<Eigen::Matrix4d> scan_inverse_poses_;
+
+    pcl::KdTreeFLANN<PointType>::Ptr kdtree_map_global_curr_;
+    pcl::KdTreeFLANN<PointType>::Ptr kdtree_scan_global_curr_;
+
+    pcl::PointCloud<PointType>::Ptr map_global_orig_;
+
+    pcl::PointCloud<PointType>::Ptr map_global_curr_; // the M_i. i.e., removert is: M1 -> S1 + D1, D1 -> M2 , M2 -> S2 + D2 ... repeat ... 
+    pcl::PointCloud<PointType>::Ptr map_local_curr_;
+
+    pcl::PointCloud<PointType>::Ptr map_subset_global_curr_;
+
+    pcl::PointCloud<PointType>::Ptr map_global_curr_static_; // the S_i
+    pcl::PointCloud<PointType>::Ptr map_global_curr_dynamic_;  // the D_i
+
+    pcl::PointCloud<PointType>::Ptr map_global_accumulated_static_; // TODO, the S_i after reverted
+    pcl::PointCloud<PointType>::Ptr map_global_accumulated_dynamic_;  // TODO, the D_i after reverted 
+
+    std::vector<pcl::PointCloud<PointType>::Ptr> static_map_global_history_; // TODO
+    std::vector<pcl::PointCloud<PointType>::Ptr> dynamic_map_global_history_; // TODO
+
+    float curr_res_alpha_; // just for tracking current status
+
+    const int base_node_idx_ = 0;
+
+    unsigned long kPauseTimeForClearStaticScanVisualization = 10000; // microsec
+
+    // NOT recommend to use for under 5 million points map input (becausing not-using is just faster)
+    const bool kUseSubsetMapCloud = false; 
+    const float kBallSize = 80.0; // meter
+
+public:
+    Removerter();
+    ~Removerter();
+
+    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg); // TODO (if removert run online)
+
+    void allocateMemory();
+
+    void parseValidScanInfo();
+    void readValidScans();
+
+    void mergeScansWithinGlobalCoord( 
+            const std::vector<pcl::PointCloud<PointType>::Ptr>& _scans, 
+            const std::vector<Eigen::Matrix4d>& _scans_poses,
+            pcl::PointCloud<PointType>::Ptr& _ptcloud_to_save );
+    void octreeDownsampling(const pcl::PointCloud<PointType>::Ptr& _src, pcl::PointCloud<PointType>::Ptr& _to_save);
+    void makeGlobalMap();
+
+    void run(void);
+
+    void removeOnce(float _res);
+    void revertOnce(float _res);
+
+    void saveCurrentStaticMapHistory(void); // the 0th element is a noisy (original input) (actually not static) map.
+    void saveCurrentDynamicMapHistory(void);
+
+    void takeGlobalMapSubsetWithinBall( int _center_scan_idx );
+    void transformGlobalMapSubsetToLocal(int _base_scan_idx);
+
+    void transformGlobalMapToLocal(int _base_scan_idx);
+    void transformGlobalMapToLocal(int _base_scan_idx, pcl::PointCloud<PointType>::Ptr& _map_local);
+    void transformGlobalMapToLocal(const pcl::PointCloud<PointType>::Ptr& _map_global, int _base_scan_idx, pcl::PointCloud<PointType>::Ptr& _map_local);
+
+    cv::Mat scan2RangeImg(const pcl::PointCloud<PointType>::Ptr& _scan, 
+                        const std::pair<float, float> _fov, /* e.g., [vfov = 50 (upper 25, lower 25), hfov = 360] */
+                        const std::pair<int, int> _rimg_size);
+    std::pair<cv::Mat, cv::Mat> map2RangeImg(const pcl::PointCloud<PointType>::Ptr& _scan, 
+                        const std::pair<float, float> _fov, /* e.g., [vfov = 50 (upper 25, lower 25), hfov = 360] */
+                        const std::pair<int, int> _rimg_size);
+
+    std::vector<int> calcDescrepancyAndParseDynamicPointIdx (const cv::Mat& _scan_rimg, const cv::Mat& _diff_rimg, const cv::Mat& _map_rimg_ptidx);
+    std::vector<int> calcDescrepancyAndParseDynamicPointIdxForEachScan( std::pair<int, int> _rimg_shape );
+
+    std::vector<int> getStaticIdxFromDynamicIdx(const std::vector<int>& _dynamic_point_indexes, int _num_all_points);
+    std::vector<int> getGlobalMapStaticIdxFromDynamicIdx(const std::vector<int>& _dynamic_point_indexes);
+
+    void parsePointcloudSubsetUsingPtIdx( const pcl::PointCloud<PointType>::Ptr& _ptcloud_orig,
+            std::vector<int>& _point_indexes, pcl::PointCloud<PointType>::Ptr& _ptcloud_to_save );
+    void parseMapPointcloudSubsetUsingPtIdx( std::vector<int>& _point_indexes, pcl::PointCloud<PointType>::Ptr& _ptcloud_to_save );
+    void parseStaticMapPointcloudUsingPtIdx( std::vector<int>& _point_indexes );
+    void parseDynamicMapPointcloudUsingPtIdx( std::vector<int>& _point_indexes );
+
+    void saveCurrentStaticAndDynamicPointCloudGlobal( void );
+    void saveCurrentStaticAndDynamicPointCloudLocal( int _base_pose_idx  = 0);
+
+    // void local2global(const pcl::PointCloud<PointType>::Ptr& _ptcloud_global, pcl::PointCloud<PointType>::Ptr& _ptcloud_local_to_save );
+    pcl::PointCloud<PointType>::Ptr local2global(const pcl::PointCloud<PointType>::Ptr& _scan_local, int _scan_idx);
+    pcl::PointCloud<PointType>::Ptr global2local(const pcl::PointCloud<PointType>::Ptr& _scan_global, int _scan_idx);
+
+    // scan-side removal
+    std::pair<pcl::PointCloud<PointType>::Ptr, pcl::PointCloud<PointType>::Ptr> removeDynamicPointsOfScanByKnn ( int _scan_idx );
+    void removeDynamicPointsAndSaveStaticScanForEachScan( void );
+
+    void scansideRemovalForEachScan(void);
+    void saveCleanedScans(void);
+    void saveMapPointcloudByMergingCleanedScans(void);
+    void scansideRemovalForEachScanAndSaveThem(void);
+
+    void saveStaticScan( int _scan_idx, const pcl::PointCloud<PointType>::Ptr& _ptcloud );
+    void saveDynamicScan( int _scan_idx, const pcl::PointCloud<PointType>::Ptr& _ptcloud );
+
+}; // Removerter