hdl_graph_slam support

config/params_hdl_graph_slam.yaml

@@ -0,0 +1,89 @@
+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] 
+
+  # 
+  isScanFileKITTIFormat: false
+  isDumpHdlGraphSlamFormat: true
+
+  # @ save option 
+  saveMapPCD: true 
+  saveCleanScansPCD: true
+  save_pcd_directory: "/home/daniil/Documents/removert/dump/" # 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: "/home/daniil/Documents/removert/dump/"
+  sequence_pose_path: "/home/daniil/Documents/removert/dump/"
+
+  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.0, 0.0,
+                              0.0, 1.0, 0.0, 0.0,
+                              0.0, 0.0, 1.0, 0.0,
+                              0.0, 0.0, 0.0, 1.0]
+
+
+  # @ Sampling nodes 
+  use_keyframe_gap: false
+  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
+  # keyframe_gap: 2 # 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: 1600 # change this
+
+
+  # @ 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
+  #     - e.g., Ouster OS1-64 of MulRan dataset -> appx 45 deg / 64 ray ~ 0.7 deg per pixel -> the magnifier ratio = 1/0.7 = 1.4
+  # - recommend to use the first reverting resolution's magnifier ratio should lied in 1.0 to 1.5
+  remove_resolution_list: [1.25, 1.0] # for HDL 64E of KITTI dataset
+  # remove_resolution_list: [1.4, 1.1] # for Ouster OS1-64 of MulRan dataset
+  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 (you need to tune these below two values, depends on the environement)
+  # - if you use a raw scan 
+  num_nn_points_within: 2 # how many - using higher, more strict static 
+  dist_nn_points_within: 0.1 # meter - using smaller, more strict static 
+
+  # about Static sensitivity (SC-LIO-SAM's feature cloud)
+  # - if you use a SC-LIO-SAM's feature cloud
+  # num_nn_points_within: 2 # how many - using higher, more strict static 
+  # dist_nn_points_within: 0.2 # 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

include/removert/RosParamServer.h

@@ -32,6 +32,7 @@ class RosParamServer: public RosNodeHandle
 
     // sequence bin files
     bool isScanFileKITTIFormat_;
+    bool isDumpHdlGraphSlamFormat_;
 
     std::string sequence_scan_dir_;
     std::vector<std::string> sequence_scan_names_;

include/removert/utility.h

@@ -150,4 +150,6 @@ double ROS_TIME(T msg)
 float pointDistance(PointType p);
 float pointDistance(PointType p1, PointType p2);
 
+Eigen::Matrix4d readPose(const std::string& filename);
+
 #endif

launch/run_hdl_graph_slam.launch

@@ -0,0 +1,21 @@
+<launch>
+
+    <arg name="project" default="removert"/>
+
+    <!--- Run Rviz-->
+    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find removert)/launch/removert_visualization.rviz" />
+
+    <!-- Parameters -->
+    <rosparam file="$(find removert)/config/params_hdl_graph_slam.yaml" command="load" />
+
+    <!--- Removert -->
+    <!-- <node pkg="$(arg project)" type="$(arg project)_removert"   name="$(arg project)_removert"    output="screen" 	respawn="true"/> -->
+    <node pkg="$(arg project)" type="$(arg project)_removert"   name="$(arg project)_removert"   output="screen"/>
+
+    <!--- Run Rqt (show range images-->
+    <!-- <include file="$(find lio_sam)/launch/include/module_rviz.launch" /> -->
+
+    <!--- Run Rviz-->
+    <!-- <include file="$(find lio_sam)/launch/include/module_rviz.launch" /> -->
+
+</launch>