diff --git a/README.md b/README.md index dd8f0b9..66609c5 100644 --- a/README.md +++ b/README.md @@ -132,11 +132,13 @@ The commands depend on your CUDA version. You may check the instructions [here]( ### 3. Install other dependency ``` -pip3 install open3d==0.17 scikit-image gtsam wandb tqdm rich roma natsort pyquaternion pypose evo laspy rospkg +pip3 install open3d==0.17 scikit-image gtsam wandb tqdm rich roma natsort pyquaternion pypose evo ``` -Note that `rospkg` is optional. You can install it if you would like to use PIN-SLAM with ROS. - +If you would like to use some specific data loader or use PIN-SLAM with ROS, you can additionally install: +``` +pip3 install kiss-icp laspy rospkg +``` ## Run PIN-SLAM @@ -158,55 +160,87 @@ sh ./scripts/download_kitti_example.sh And then run: ``` -python3 pin_slam.py ./config/lidar_slam/run_demo.yaml +python3 pin_slam.py ./config/lidar_slam/run_demo.yaml -vsm ```
[Details (click to expand)] -Use `run_demo_no_vis.yaml` if you are running on a server without an X service. -Use `run_demo_sem.yaml` if you want to conduct metric-semantic SLAM using semantic segmentation labels. - You can visualize the SLAM process in PIN-SLAM visualizer and check the results in the `./experiments` folder. + +Use `run_demo_sem.yaml` if you want to conduct metric-semantic SLAM using semantic segmentation labels: +``` +python3 pin_slam.py ./config/lidar_slam/run_demo_sem.yaml -vsm +``` + +If you are running on a server without an X service (you may first try `export DISPLAY=:0`), then you can turn off the visualization `-v` flag: +``` +python3 pin_slam.py ./config/lidar_slam/run_demo.yaml -sm +``` + +If you don't have a Nvidia GPU on your device, then you can turn on the CPU-only operation by adding the `-c` flag: +``` +python3 pin_slam.py ./config/lidar_slam/run_demo.yaml -vsmc +``` +
### Run on your datasets -For an arbitrary data sequence, you can run: +For an arbitrary data sequence, you can run with the default config file by: ``` -python3 pin_slam.py path_to_your_config_file.yaml +python3 pin_slam.py -i /path/to/your/point/cloud/folder -vsm ```
[Details (click to expand)] -Generally speaking, you only need to edit in the config file the -`pc_path`, which is the path to the folder containing the point cloud (`.bin`, `.ply`, `.pcd` or `.las` format) for each frame. -For ROS bag, you can use `./scripts/rosbag2ply.py` to extract the point cloud in `.ply` format. +Follow the instructions on how to run PIN-SLAM by typing: +``` +python3 pin_slam.py -h +``` + +To run PIN-SLAM with a specific config file, you can run: +``` +python3 pin_slam.py path_to_your_config_file.yaml -vsm +``` + +The flags `-v`, `-s`, `-m` toggle the visualizer, map saving and mesh saving, respectively. + +To specify the path to the input point cloud folder, you can either set `pc_path` in the config file or set `-i INPUT_PATH` upon running. -For pose estimation evaluation, you may also provide the path `pose_path` to the reference pose file and optionally the path `calib_path` to the extrinsic calibration file. Note the pose file should be in the KITTI format or TUM format. +For pose estimation evaluation, you may also set `pose_path` in the config file to specify the path to the reference pose file (in KITTI or TUM format). -For some popular datasets, you can run: +For some popular datasets, you can also set the dataset name and sequence name upon running. For example: ``` # KITTI dataset sequence 00 -python3 pin_slam.py ./config/lidar_slam/run_kitti.yaml kitti 00 +python3 pin_slam.py ./config/lidar_slam/run_kitti.yaml kitti 00 -vsm # MulRAN dataset sequence KAIST01 -python3 pin_slam.py ./config/lidar_slam/run_mulran.yaml mulran kaist01 +python3 pin_slam.py ./config/lidar_slam/run_mulran.yaml mulran kaist01 -vsm # Newer College dataset sequence 01_short -python3 pin_slam.py ./config/lidar_slam/run_ncd.yaml ncd 01 +python3 pin_slam.py ./config/lidar_slam/run_ncd.yaml ncd 01 -vsm # Replica dataset sequence room0 -python3 pin_slam.py ./config/rgbd_slam/run_replica.yaml replica room0 +python3 pin_slam.py ./config/rgbd_slam/run_replica.yaml replica room0 -vsm ``` -The SLAM results and logs will be output in the `output_root` folder specified in the config file. +We also support loading data from rosbag, mcap or pcap using KISS-ICP data loaders. You need to set the flag `-k` to use such data loaders. For example: +``` +# Run on a rosbag or a folder of rosbags with certain point cloud topic +python3 pin_slam.py ./config/lidar_slam/run.yaml rosbag point_cloud_topic_name -i /path/to/the/rosbag -vsmk + +# If there's only one topic for point cloud in the rosbag, you can omit it +python3 pin_slam.py ./config/lidar_slam/run.yaml rosbag -i /path/to/the/rosbag -vsmk +``` + +The SLAM results and logs will be output in the `output_root` folder set in the config file or specified by the `-o OUTPUT_PATH` flag. You may check [here](https://github.com/PRBonn/PIN_SLAM/blob/main/eval/README.md) for the results that can be obtained with this repository on a couple of popular datasets. -The training logs can be monitored via [Weights & Bias](wandb.ai) online if you turn on the `wandb_vis_on` option in the config file. If it's your first time using Weights & Bias, you will be requested to register and log in to your wandb account. +The training logs can be monitored via [Weights & Bias](wandb.ai) online if you set the flag `-w`. If it's your first time using Weights & Bias, you will be requested to register and log in to your wandb account. You can also set the flag `-l` to turn on the log printing in the terminal.
@@ -215,7 +249,7 @@ The training logs can be monitored via [Weights & Bias](wandb.ai) online if you If you are not using PIN-SLAM as a part of a ROS package, you can avoid the catkin stuff and simply run: ``` -python3 pin_slam_ros.py [path_to_your_config_file] [point_cloud_topic_name] +python3 pin_slam_ros.py path_to_your_config_file.yaml point_cloud_topic_name ```
@@ -224,7 +258,7 @@ python3 pin_slam_ros.py [path_to_your_config_file] [point_cloud_topic_name] For example: ``` -python3 pin_slam_ros.py ./config/lidar_slam/run_ros_general.yaml /os_cloud_node/points +python3 pin_slam_ros.py ./config/lidar_slam/run.yaml /os_cloud_node/points ``` After playing the ROS bag or launching the sensor you can then visualize the results in Rviz by: @@ -246,6 +280,8 @@ If you are running without a powerful GPU, PIN-SLAM may not run at the sensor fr You can also put `pin_slam_ros.py` into a ROS package for `rosrun` or `roslaunch`. +We will add support for ROS2 in the near future. +
@@ -254,30 +290,28 @@ You can also put `pin_slam_ros.py` into a ROS package for `rosrun` or `roslaunch After the SLAM process, you can reconstruct mesh from the PIN map within an arbitrary bounding box with an arbitrary resolution by running: ``` -python3 vis_pin_map.py [path/to/your/result/folder] [marching_cubes_resolution_m] [(cropped)_map_file.ply] [output_mesh_file.ply] [mesh_min_nn] +python3 vis_pin_map.py path/to/your/result/folder [marching_cubes_resolution_m] [(cropped)_map_file.ply] [output_mesh_file.ply] [mesh_min_nn] ```
[Details (click to expand)] -The bounding box of `(cropped)_map_file.ply` will be used for the bounding box for mesh reconstruction. `mesh_min_nn` controls the trade-off between completeness and accuracy. The smaller number (for example `6`) will lead to a more complete mesh with more guessed artifacts. The larger number (for example `15`) will lead to a less complete but more accurate mesh. +The bounding box of `(cropped)_map_file.ply` will be used as the bounding box for mesh reconstruction. `mesh_min_nn` controls the trade-off between completeness and accuracy. The smaller number (for example `6`) will lead to a more complete mesh with more guessed artifacts. The larger number (for example `15`) will lead to a less complete but more accurate mesh. For example, for the case of the sanity test, run: ``` -python3 vis_pin_map.py ./experiments/sanity_test_* 0.2 neural_points.ply mesh_20cm.ply 8 +python3 vis_pin_map.py ./experiments/sanity_test_* 0.2 neural_points.ply mesh_20cm.ply 8 ```
## Visualizer Instructions -We provide a PIN-SLAM visualizer based on [lidar-visualizer](https://github.com/PRBonn/lidar-visualizer) to monitor the SLAM process. - -The keyboard callbacks are listed below. +We provide a PIN-SLAM visualizer based on [lidar-visualizer](https://github.com/PRBonn/lidar-visualizer) to monitor the SLAM process. You can use `-v` flag to turn on it.
- [Details (click to expand)] + [Keyboard callbacks (click to expand)] | Button | Function | |:------:|:------------------------------------------------------------------------------------------:| diff --git a/config/lidar_slam/run.yaml b/config/lidar_slam/run.yaml new file mode 100644 index 0000000..e69ad10 --- /dev/null +++ b/config/lidar_slam/run.yaml @@ -0,0 +1,25 @@ +setting: + name: "test_pin" + output_root: "./experiments" + deskew: True +process: + max_range_m: 80.0 # maximum distance filter for each frame +sampler: + surface_sample_range_m: 0.25 +neuralpoints: + voxel_size_m: 0.4 + search_alpha: 0.5 # increase when you want to be more robust to agressive motions +continual: + batch_size_new_sample: 1000 + pool_capacity: 1e7 + pool_filter_freq: 10 +tracker: + source_vox_down_m: 0.6 + iter_n: 50 + valid_nn_k: 5 +pgo: + map_context: True + context_cosdist: 0.3 +optimizer: # mapper + batch_size: 10000 + adaptive_iters: True \ No newline at end of file diff --git a/config/lidar_slam/run_apollo.yaml b/config/lidar_slam/run_apollo.yaml index 5d15b9b..858b27b 100644 --- a/config/lidar_slam/run_apollo.yaml +++ b/config/lidar_slam/run_apollo.yaml @@ -38,10 +38,6 @@ optimizer: # mapper iters: 15 # iterations per frame batch_size: 16384 eval: - wandb_vis_on: False # log to wandb or not - o3d_vis_on: True # visualize the mapping or not - silence_log: True sensor_cad_path: ./cad/ipb_car.ply mesh_freq_frame: 50 # reconstruct the mesh every x frames - mesh_min_nn: 9 - save_map: True \ No newline at end of file + mesh_min_nn: 9 \ No newline at end of file diff --git a/config/lidar_slam/run_demo.yaml b/config/lidar_slam/run_demo.yaml index f1961b9..249f0b9 100644 --- a/config/lidar_slam/run_demo.yaml +++ b/config/lidar_slam/run_demo.yaml @@ -7,8 +7,4 @@ process: tracker: iter_n: 20 eval: - o3d_vis_on: True # visualize the mapping or not - silence_log: True # output the logs or not - sensor_cad_path: ./cad/kitti_car.ply - save_map: True - save_mesh: True \ No newline at end of file + sensor_cad_path: ./cad/kitti_car.ply \ No newline at end of file diff --git a/config/lidar_slam/run_demo_no_vis.yaml b/config/lidar_slam/run_demo_no_vis.yaml deleted file mode 100644 index 1459272..0000000 --- a/config/lidar_slam/run_demo_no_vis.yaml +++ /dev/null @@ -1,12 +0,0 @@ -setting: - name: "sanity_test" - output_root: "./experiments" - pc_path: "./data/kitti_example/sequences/00/velodyne" -process: - max_range_m: 60.0 -tracker: - iter_n: 20 -eval: - o3d_vis_on: False # visualize the mapping or not - silence_log: True # output the logs or not - save_map: True diff --git a/config/lidar_slam/run_demo_sem.yaml b/config/lidar_slam/run_demo_sem.yaml index e9563ce..d6ea291 100644 --- a/config/lidar_slam/run_demo_sem.yaml +++ b/config/lidar_slam/run_demo_sem.yaml @@ -9,8 +9,4 @@ process: tracker: iter_n: 20 eval: - o3d_vis_on: True # visualize the mapping or not - silence_log: True # output the logs or not - sensor_cad_path: ./cad/kitti_car.ply - save_map: True - save_mesh: True \ No newline at end of file + sensor_cad_path: ./cad/kitti_car.ply \ No newline at end of file diff --git a/config/lidar_slam/run_hilti.yaml b/config/lidar_slam/run_hilti.yaml index 55c1280..086da91 100644 --- a/config/lidar_slam/run_hilti.yaml +++ b/config/lidar_slam/run_hilti.yaml @@ -38,9 +38,5 @@ optimizer: # mapper batch_size: 16384 adaptive_iters: True eval: - wandb_vis_on: False # log to wandb or not - o3d_vis_on: True # visualize the mapping or not - silence_log: True mesh_freq_frame: 50 # reconstruct the mesh every x frames - save_map: True mesh_min_nn: 18 \ No newline at end of file diff --git a/config/lidar_slam/run_ipbcar.yaml b/config/lidar_slam/run_ipbcar.yaml index 0f9235a..f9f97cc 100644 --- a/config/lidar_slam/run_ipbcar.yaml +++ b/config/lidar_slam/run_ipbcar.yaml @@ -17,29 +17,24 @@ neuralpoints: search_alpha: 0.5 weighted_first: False loss: - main_loss_type: bce loss_weight_on: True dist_weight_scale: 0.8 ekional_loss_on: True weight_e: 0.5 continual: batch_size_new_sample: 3000 - pool_capacity: 4e7 + pool_capacity: 2e7 + pool_filter_freq: 10 tracker: iter_n: 100 - eigenvalue_check: False pgo: map_context: True pgo_freq_frame: 30 - with_pose_prior: True virtual_side_count: 6 optimizer: # mapper iters: 15 # iterations per frame batch_size: 16384 eval: - wandb_vis_on: False # log to wandb or not - o3d_vis_on: True # visualize the mapping or not sensor_cad_path: ./cad/ipb_car.ply mesh_freq_frame: 50 # reconstruct the mesh every x frames - mesh_min_nn: 15 - save_map: True \ No newline at end of file + mesh_min_nn: 15 \ No newline at end of file diff --git a/config/lidar_slam/run_kitti.yaml b/config/lidar_slam/run_kitti.yaml index afbff3a..40a49e1 100644 --- a/config/lidar_slam/run_kitti.yaml +++ b/config/lidar_slam/run_kitti.yaml @@ -46,10 +46,8 @@ pgo: optimizer: # mapper batch_size: 16384 eval: - wandb_vis_on: False # log to wandb or not - o3d_vis_on: True # visualize the mapping or not + o3d_vis_on: False # visualize the mapping or not silence_log: True # output the logs or not sensor_cad_path: ./cad/kitti_car.ply mesh_freq_frame: 50 # reconstruct the mesh every x frames - mesh_min_nn: 9 - save_map: True \ No newline at end of file + mesh_min_nn: 9 \ No newline at end of file diff --git a/config/lidar_slam/run_light.yaml b/config/lidar_slam/run_light.yaml index 52374e5..ac8cab7 100644 --- a/config/lidar_slam/run_light.yaml +++ b/config/lidar_slam/run_light.yaml @@ -36,9 +36,7 @@ optimizer: # mappers batch_size: 8192 adaptive_iters: True eval: - wandb_vis_on: False # log to wandb or not - o3d_vis_on: True # visualize the mapping or not + o3d_vis_on: False # visualize the mapping or not silence_log: True # output the logs or not mesh_freq_frame: 50 # reconstruct the mesh every x frames - mesh_min_nn: 9 - save_map: True \ No newline at end of file + mesh_min_nn: 9 \ No newline at end of file diff --git a/config/lidar_slam/run_livox.yaml b/config/lidar_slam/run_livox.yaml index 932406d..7b80a60 100644 --- a/config/lidar_slam/run_livox.yaml +++ b/config/lidar_slam/run_livox.yaml @@ -32,12 +32,9 @@ tracker: source_vox_down_m: 0.2 iter_n: 100 optimizer: - mapping_freq_frame: 2 iters: 15 batch_size: 8192 eval: - wandb_vis_on: False # log to wandb or not - o3d_vis_on: True # visualize the mapping or not + o3d_vis_on: False # visualize the mapping or not mesh_freq_frame: 20 # reconstruct the mesh every x frames - mesh_min_nn: 15 - save_map: True \ No newline at end of file + mesh_min_nn: 15 \ No newline at end of file diff --git a/config/lidar_slam/run_mulran.yaml b/config/lidar_slam/run_mulran.yaml index 170efd5..acdd474 100644 --- a/config/lidar_slam/run_mulran.yaml +++ b/config/lidar_slam/run_mulran.yaml @@ -38,9 +38,6 @@ optimizer: # mapper iters: 15 # iterations per frame batch_size: 16384 eval: - wandb_vis_on: False # log to wandb or not - o3d_vis_on: True # visualize the mapping or not - silence_log: True + o3d_vis_on: False # visualize the mapping or not mesh_freq_frame: 50 # reconstruct the mesh every x frames - mesh_min_nn: 18 - save_map: True \ No newline at end of file + mesh_min_nn: 18 \ No newline at end of file diff --git a/config/lidar_slam/run_ncd.yaml b/config/lidar_slam/run_ncd.yaml index 08b21b3..3affb51 100644 --- a/config/lidar_slam/run_ncd.yaml +++ b/config/lidar_slam/run_ncd.yaml @@ -40,9 +40,6 @@ optimizer: # mapper iters: 15 batch_size: 16384 eval: - wandb_vis_on: False # log to wandb or not - o3d_vis_on: True # visualize the mapping or not - silence_log: True + o3d_vis_on: False # visualize the mapping or not mesh_freq_frame: 50 # reconstruct the mesh every x frames - mesh_min_nn: 18 - save_map: True \ No newline at end of file + mesh_min_nn: 18 \ No newline at end of file diff --git a/config/lidar_slam/run_ncd_128.yaml b/config/lidar_slam/run_ncd_128.yaml index 89e3e62..42e755b 100644 --- a/config/lidar_slam/run_ncd_128.yaml +++ b/config/lidar_slam/run_ncd_128.yaml @@ -40,9 +40,7 @@ optimizer: # mapper batch_size: 16384 adaptive_iters: True eval: - wandb_vis_on: False # log to wandb or not - o3d_vis_on: True # visualize the mapping or not + o3d_vis_on: False # visualize the mapping or not silence_log: True mesh_freq_frame: 50 # reconstruct the mesh every x frames - mesh_min_nn: 15 - save_map: False \ No newline at end of file + mesh_min_nn: 15 \ No newline at end of file diff --git a/config/lidar_slam/run_ncd_128_m.yaml b/config/lidar_slam/run_ncd_128_m.yaml index 6c65668..6aeeb08 100644 --- a/config/lidar_slam/run_ncd_128_m.yaml +++ b/config/lidar_slam/run_ncd_128_m.yaml @@ -40,9 +40,7 @@ optimizer: # mapper batch_size: 16384 adaptive_iters: True eval: - wandb_vis_on: False # log to wandb or not - o3d_vis_on: True # visualize the mapping or not + o3d_vis_on: False # visualize the mapping or not silence_log: True mesh_freq_frame: 50 # reconstruct the mesh every x frames - mesh_min_nn: 15 - save_map: False \ No newline at end of file + mesh_min_nn: 15 \ No newline at end of file diff --git a/config/lidar_slam/run_ncd_128_s.yaml b/config/lidar_slam/run_ncd_128_s.yaml index ace4ba1..02f98d4 100644 --- a/config/lidar_slam/run_ncd_128_s.yaml +++ b/config/lidar_slam/run_ncd_128_s.yaml @@ -41,9 +41,7 @@ optimizer: # mapper ba_freq_frame: 20 lr_pose_ba: 1e-3 eval: - wandb_vis_on: False # log to wandb or not - o3d_vis_on: True # visualize the mapping or not + o3d_vis_on: False # visualize the mapping or not silence_log: True mesh_freq_frame: 50 - mesh_min_nn: 18 - save_map: False \ No newline at end of file + mesh_min_nn: 18 \ No newline at end of file diff --git a/config/lidar_slam/run_ros_general.yaml b/config/lidar_slam/run_ros_general.yaml deleted file mode 100644 index 277ec3f..0000000 --- a/config/lidar_slam/run_ros_general.yaml +++ /dev/null @@ -1,43 +0,0 @@ -setting: - name: "test_ros" - output_root: "./experiments" - deskew: True -process: - min_range_m: 2.5 - max_range_m: 80.0 # maximum distance filter for each frame - adaptive_range_on: True -sampler: - surface_sample_range_m: 0.3 - surface_sample_n: 3 - free_sample_begin_ratio: 0.3 - free_sample_end_dist_m: 1.0 - free_front_sample_n: 2 -neuralpoints: - voxel_size_m: 0.4 - feature_dim: 8 - query_nn_k: 6 - search_alpha: 0.5 # increase when you want to be more robust to agressive motions -loss: - main_loss_type: bce - loss_weight_on: True - dist_weight_scale: 0.8 - ekional_loss_on: True - weight_e: 0.5 -continual: - batch_size_new_sample: 1500 - pool_capacity: 2e7 - pool_filter_freq: 10 -tracker: - source_vox_down_m: 0.8 - iter_n: 50 -pgo: - map_context: True - pgo_freq_frame: 30 - virtual_side_count: 6 -optimizer: # mapper - iters: 15 # iterations per frame - batch_size: 10000 -eval: - wandb_vis_on: False # log to wandb or not - silence_log: False - save_map: True \ No newline at end of file diff --git a/config/lidar_slam/run_vbr.yaml b/config/lidar_slam/run_vbr.yaml index cdb6853..9af6f5c 100644 --- a/config/lidar_slam/run_vbr.yaml +++ b/config/lidar_slam/run_vbr.yaml @@ -9,8 +9,6 @@ process: max_range_m: 60.0 min_z_m: -10.0 vox_down_m: 0.08 - adaptive_range_on: True - dynamic_filter_on: True sampler: surface_sample_range_m: 0.25 surface_sample_n: 4 @@ -42,9 +40,6 @@ optimizer: # mapper batch_size: 16384 adaptive_iters: True eval: - wandb_vis_on: False # log to wandb or not - o3d_vis_on: True # visualize the mapping or not - silence_log: False + o3d_vis_on: False # visualize the mapping or not mesh_freq_frame: 50 # reconstruct the mesh every x frames - mesh_min_nn: 15 - save_map: True \ No newline at end of file + mesh_min_nn: 15 \ No newline at end of file diff --git a/config/rgbd_slam/run_replica.yaml b/config/rgbd_slam/run_replica.yaml index 69ca1c4..460bbe6 100644 --- a/config/rgbd_slam/run_replica.yaml +++ b/config/rgbd_slam/run_replica.yaml @@ -44,11 +44,8 @@ optimizer: ba_freq_frame: 20 lr_pose_ba: 1e-3 eval: - wandb_vis_on: False # log to wandb or not - o3d_vis_on: True # visualize the mapping or not - silence_log: True + o3d_vis_on: False # visualize the mapping or not mesh_freq_frame: 100 # reconstruct the mesh every x frames mesh_min_nn: 9 sensor_cad_path: ./cad/camera.ply - skip_top_voxel: 5 - save_map: True \ No newline at end of file + skip_top_voxel: 5 \ No newline at end of file diff --git a/dataset/dataset_indexing.py b/dataset/dataset_indexing.py index 19ae359..dd17170 100644 --- a/dataset/dataset_indexing.py +++ b/dataset/dataset_indexing.py @@ -9,6 +9,9 @@ def set_dataset_path(config: Config, dataset_name: str = '', seq: str = ''): + if seq is None: + seq = '' + config.name = config.name + '_' + dataset_name + '_' + seq.replace("/", "") if config.use_kiss_dataloader: @@ -72,3 +75,5 @@ def set_dataset_path(config: Config, dataset_name: str = '', seq: str = ''): config.pc_path = os.path.join(base_path, seq, "rgbd_down_ply") # input point cloud folder #config.pc_path = os.path.join(base_path, seq, "rgbd_ply") # input point cloud folder config.pose_path = os.path.join(base_path, seq, "poses.txt") # input pose file + else: + print('The specified dataset has not been supported yet, try using kiss-icp data loader by adding --kiss_loader flag.') diff --git a/dataset/slam_dataset.py b/dataset/slam_dataset.py index a7d96e3..a78089f 100644 --- a/dataset/slam_dataset.py +++ b/dataset/slam_dataset.py @@ -62,11 +62,11 @@ def __init__(self, config: Config) -> None: topic=config.data_loader_seq, ) config.end_frame = min(len(self.loader), config.end_frame) - used_frame_count = int((config.end_frame - config.begin_frame) / config.every_frame) + used_frame_count = int((config.end_frame - config.begin_frame) / config.step_frame) self.total_pc_count = used_frame_count max_frame_number = self.total_pc_count if hasattr(self.loader, 'gt_poses'): - self.gt_poses = self.loader.gt_poses[config.begin_frame:config.end_frame:config.every_frame] + self.gt_poses = self.loader.gt_poses[config.begin_frame:config.end_frame:config.step_frame] self.gt_pose_provided = True else: self.gt_pose_provided = False @@ -80,9 +80,11 @@ def __init__(self, config: Config) -> None: self.pc_filenames = natsorted(os.listdir(config.pc_path)) self.total_pc_count_in_folder = len(self.pc_filenames) config.end_frame = min(config.end_frame, self.total_pc_count_in_folder) - self.pc_filenames = self.pc_filenames[config.begin_frame:config.end_frame:config.every_frame] + self.pc_filenames = self.pc_filenames[config.begin_frame:config.end_frame:config.step_frame] self.total_pc_count = len(self.pc_filenames) max_frame_number = self.total_pc_count + else: + sys.exit("Input point cloud directory is not specified. Either use -i flag or add `pc_path:` to the config file. Check details by `python pin_slam.py -h`") self.gt_pose_provided = True if config.pose_path == "": @@ -95,8 +97,8 @@ def __init__(self, config: Config) -> None: poses_uncalib = read_kitti_format_poses(config.pose_path) if poses_uncalib is None: poses_uncalib, poses_ts = read_tum_format_poses(config.pose_path) - self.poses_ts = np.array(poses_ts[config.begin_frame:config.end_frame:config.every_frame]) - poses_uncalib = np.array(poses_uncalib[config.begin_frame:config.end_frame:config.every_frame]) + self.poses_ts = np.array(poses_ts[config.begin_frame:config.end_frame:config.step_frame]) + poses_uncalib = np.array(poses_uncalib[config.begin_frame:config.end_frame:config.step_frame]) if poses_uncalib is None: sys.exit("Wrong pose file format. Please use either kitti or tum format with *.txt") @@ -197,7 +199,7 @@ def read_frame_with_loader(self, frame_id): self.set_ref_pose(frame_id) - frame_id_in_folder = self.config.begin_frame + frame_id * self.config.every_frame + frame_id_in_folder = self.config.begin_frame + frame_id * self.config.step_frame data = self.loader[frame_id_in_folder] # data loading could be slow # FIXME point_ts = None @@ -327,9 +329,9 @@ def preprocess_frame(self): self.last_pose_ref = self.cur_pose_ref elif frame_id > 0: # pose initial guess - last_translation = np.linalg.norm(self.last_odom_tran[:3, 3]) - # if self.config.uniform_motion_on and not self.lose_track and last_translation > 0.2 * self.config.voxel_size_m: # apply uniform motion model here - if self.config.uniform_motion_on and not self.lose_track: + # last_translation = np.linalg.norm(self.last_odom_tran[:3, 3]) + if self.config.uniform_motion_on and not self.lose_track: + # if self.config.uniform_motion_on: # apply uniform motion model here cur_pose_init_guess = ( self.last_pose_ref @ self.last_odom_tran @@ -629,7 +631,7 @@ def write_results(self): odom_poses = self.odom_poses[:self.processed_frame+1] odom_poses_out = apply_kitti_format_calib(odom_poses, self.calib["Tr"]) write_kitti_format_poses(os.path.join(self.run_path, "odom_poses"), odom_poses_out) - write_tum_format_poses(os.path.join(self.run_path, "odom_poses"), odom_poses_out, self.poses_ts, 0.1*self.config.every_frame) + write_tum_format_poses(os.path.join(self.run_path, "odom_poses"), odom_poses_out, self.poses_ts, 0.1*self.config.step_frame) write_traj_as_o3d(odom_poses, os.path.join(self.run_path, "odom_poses.ply")) if self.config.pgo_on: @@ -639,7 +641,7 @@ def write_results(self): os.path.join(self.run_path, "slam_poses"), slam_poses_out ) write_tum_format_poses( - os.path.join(self.run_path, "slam_poses"), slam_poses_out, self.poses_ts, 0.1*self.config.every_frame + os.path.join(self.run_path, "slam_poses"), slam_poses_out, self.poses_ts, 0.1*self.config.step_frame ) write_traj_as_o3d(pgo_poses, os.path.join(self.run_path, "slam_poses.ply")) @@ -652,12 +654,12 @@ def write_results(self): np.save( os.path.join(self.run_path, "time_table.npy"), time_table ) # save detailed time table - if self.config.o3d_vis_on: - plot_timing_detail( - time_table, - os.path.join(self.run_path, "time_details.png"), - self.config.pgo_on, - ) + + plot_timing_detail( + time_table, + os.path.join(self.run_path, "time_details.png"), + self.config.pgo_on, + ) pose_eval = None @@ -799,7 +801,7 @@ def write_results(self): return pose_eval - def write_merged_point_cloud(self): + def write_merged_point_cloud(self, down_vox_m=None): print("Begin to replay the dataset ...") @@ -840,7 +842,8 @@ def write_merged_point_cloud(self): ), ) # T_last<-cur - down_vox_m = self.config.vox_down_m + if down_vox_m is None: + down_vox_m = self.config.vox_down_m idx = voxel_down_sample_torch(self.cur_point_cloud_torch[:, :3], down_vox_m) frame_down_torch = self.cur_point_cloud_torch[idx] diff --git a/model/decoder.py b/model/decoder.py index 91cc112..0ba43b3 100644 --- a/model/decoder.py +++ b/model/decoder.py @@ -157,4 +157,4 @@ def regress_color(self, features): out = torch.clamp(self.lout(h), 0.0, 1.0) # out = torch.sigmoid(self.lout(h)) # print(out) - return out + return out \ No newline at end of file diff --git a/pin_slam.py b/pin_slam.py index 0bec08b..c073577 100644 --- a/pin_slam.py +++ b/pin_slam.py @@ -3,6 +3,7 @@ # @author Yue Pan [yue.pan@igg.uni-bonn.de] # Copyright (c) 2024 Yue Pan, all rights reserved +import argparse import os import sys @@ -39,32 +40,59 @@ ''' πŸ“PIN-SLAM: LiDAR SLAM Using a Point-Based Implicit Neural Representation for Achieving Global Map Consistency - Y. Pan et al. + Y. Pan et al. from IPB ''' + +parser = argparse.ArgumentParser() +parser.add_argument('config_path', type=str, nargs='?', default='config/lidar_slam/run.yaml', help='[Optional] Path to *.yaml config file, if not set, default config would be used') +parser.add_argument('dataset_name', type=str, nargs='?', help='[Optional] Name of a specific dataset, example: kitti, mulran, or rosbag (when -k is set)') +parser.add_argument('sequence_name', type=str, nargs='?', help='[Optional] Name of a specific data sequence or the rostopic for point cloud (when -k is set)') +parser.add_argument('--seed', type=int, default=42, help='Set the random seed (default 42)') +parser.add_argument('--input_path', '-i', type=str, default=None, help='Path to the point cloud input directory (this will override the pc_path in config file)') +parser.add_argument('--output_path', '-o', type=str, default=None, help='Path to the result output directory (this will override the output_root in config file)') +parser.add_argument('--range', nargs=3, type=int, metavar=('START', 'END', 'STEP'), default=None, help='Specify the start, end and step of the processed frame, for example: --range 10 1000 1') +parser.add_argument('--kiss_loader', '-k', action='store_true', help='Use KISS-ICP data loader (you can use the rosbag, pcap, mcap dataloaders and datasets supported by KISS-ICP)') +parser.add_argument('--visualize', '-v', action='store_true', help='Turn on the visualizer') +parser.add_argument('--cpu_only', '-c', action='store_true', help='Run only on CPU') +parser.add_argument('--log_on', '-l', action='store_true', help='Turn on the logs printing') +parser.add_argument('--wandb_on', '-w', action='store_true', help='Turn on the weight & bias logging') +parser.add_argument('--save_map', '-s', action='store_true', help='Save the PIN map after SLAM') +parser.add_argument('--save_mesh', '-m', action='store_true', help='Save the reconstructed mesh after SLAM') + +args = parser.parse_args() + def run_pin_slam(config_path=None, dataset_name=None, sequence_name=None, seed=None): config = Config() - if config_path is not None: + if config_path is not None: # use as a function config.load(config_path) - set_dataset_path(config, dataset_name, sequence_name) + if dataset_name is not None: + set_dataset_path(config, dataset_name, sequence_name) if seed is not None: config.seed = seed argv = ['pin_slam.py', config_path, dataset_name, sequence_name, str(seed)] run_path = setup_experiment(config, argv) - else: - if len(sys.argv) > 1: - config.load(sys.argv[1]) - else: - sys.exit("Please provide the path to the config file.\nTry: \ - python3 pin_slam.py path_to_config.yaml [dataset_name] [sequence_name] [random_seed]") - # specific dataset [optional] - if len(sys.argv) == 3: - set_dataset_path(config, sys.argv[2]) - if len(sys.argv) > 3: - set_dataset_path(config, sys.argv[2], sys.argv[3]) - if len(sys.argv) > 4: # random seed [optional] - config.seed = int(sys.argv[4]) - run_path = setup_experiment(config, sys.argv) + else: # from args + argv = sys.argv + config.load(args.config_path) + config.use_kiss_dataloader = args.kiss_loader + config.seed = args.seed + config.silence = not args.log_on + config.wandb_vis_on = args.wandb_on + config.o3d_vis_on = args.visualize + config.save_map = args.save_map + config.save_mesh = args.save_mesh + if args.range is not None: + config.begin_frame, config.end_frame, config.step_frame = args.range + if args.cpu_only: + config.device = 'cpu' + if args.input_path is not None: + config.pc_path = args.input_path + if args.output_path is not None: + config.output_root = args.output_path + if args.dataset_name is not None: # specific dataset [optional] + set_dataset_path(config, args.dataset_name, args.sequence_name) + run_path = setup_experiment(config, argv) print("[bold green]PIN-SLAM starts[/bold green]","πŸ“" ) # non-blocking visualizer @@ -94,6 +122,7 @@ def run_pin_slam(config_path=None, dataset_name=None, sequence_name=None, seed=N # mesh reconstructor mesher = Mesher(config, neural_points, geo_mlp, sem_mlp, color_mlp) + cur_mesh = None # pose graph manager (for back-end optimization) initialization pgm = PoseGraphManager(config) @@ -277,6 +306,7 @@ def run_pin_slam(config_path=None, dataset_name=None, sequence_name=None, seed=N print("time for training (ms):", (T6-T5)*1e3) # V: Mesh reconstruction and visualization + cur_mesh = None if config.o3d_vis_on: # if visualizer is off, there's no need to reconstruct the mesh o3d_vis.cur_frame_id = frame_id # frame id in the data folder @@ -296,7 +326,6 @@ def run_pin_slam(config_path=None, dataset_name=None, sequence_name=None, seed=N neural_pcd = neural_points.get_neural_points_o3d(query_global=o3d_vis.vis_global, color_mode=o3d_vis.neural_points_vis_mode, random_down_ratio=1) # select from geo_feature, ts and certainty # reconstruction by marching cubes - cur_mesh = None if config.mesh_freq_frame > 0: if o3d_vis.render_mesh and (frame_id == 0 or frame_id == last_frame or (frame_id+1) % config.mesh_freq_frame == 0 or pgm.last_loop_idx == frame_id): # update map bbx @@ -366,11 +395,16 @@ def run_pin_slam(config_path=None, dataset_name=None, sequence_name=None, seed=N neural_points.recreate_hash(None, None, False, False) # merge the final neural point map neural_points.prune_map(config.max_prune_certainty, 0) # prune uncertain points for the final output + neural_pcd = neural_points.get_neural_points_o3d(query_global=True, color_mode = 0) if config.save_map: - neural_pcd = neural_points.get_neural_points_o3d(query_global=True, color_mode = 0) o3d.io.write_point_cloud(os.path.join(run_path, "map", "neural_points.ply"), neural_pcd) # write the neural point cloud + if config.save_mesh and cur_mesh is None: + output_mc_res_m = config.mc_res_m*0.6 + chunks_aabb = split_chunks(neural_pcd, neural_pcd.get_axis_aligned_bounding_box(), output_mc_res_m * 300) # reconstruct in chunks + mc_cm_str = str(round(output_mc_res_m*1e2)) + mesh_path = os.path.join(run_path, "mesh", "mesh_" + mc_cm_str + "cm.ply") + cur_mesh = mesher.recon_aabb_collections_mesh(chunks_aabb, output_mc_res_m, mesh_path, False, config.semantic_on, config.color_on, filter_isolated_mesh=True, mesh_min_nn=config.mesh_min_nn) neural_points.clear_temp() # clear temp data for output - if config.save_map: save_implicit_map(run_path, neural_points, geo_mlp, color_mlp, sem_mlp) if config.save_merged_pc: diff --git a/utils/config.py b/utils/config.py index f9fd7d4..feaf6ef 100644 --- a/utils/config.py +++ b/utils/config.py @@ -18,7 +18,7 @@ def __init__(self): self.name: str = "dummy" # experiment name self.run_path: str = "" - self.output_root: str = "" # output root folder + self.output_root: str = "experiments" # output root folder self.pc_path: str = "" # input point cloud folder self.pose_path: str = "" # input pose file self.calib_path: str = "" # input calib file (to sensor frame), optional @@ -36,7 +36,7 @@ def __init__(self): # frame as the reference frame self.begin_frame: int = 0 # begin from this frame self.end_frame: int = 100000 # end at this frame - self.every_frame: int = 1 # process every x frame + self.step_frame: int = 1 # process every x frame self.seed: int = 42 # random seed for the experiment self.num_workers: int = 12 # number of worker for the dataloader @@ -186,6 +186,7 @@ def __init__(self): self.bs: int = 16384 # batch size self.lr: float = 0.01 # learning rate for map parameters and MLP self.lr_pose: float = 1e-4 # learning rate for poses during bundle adjustment + self.lr_ba_map: float = 0.01 # learning rate for map during bundle adjustment self.weight_decay: float = 0.0 # weight_decay is only applied to the latent codes for the l2 regularization self.adam_eps: float = 1e-15 self.adaptive_iters: bool = False # adptive map optimization iterations on (train for fewer iterations when there's not much new information to learn) @@ -253,18 +254,18 @@ def __init__(self): # eval self.wandb_vis_on: bool = False # monitor the training on weight and bias or not - self.silence: bool = False # print log in the terminal or not + self.silence: bool = True # print log in the terminal or not self.o3d_vis_on: bool = False # visualize the mesh in-the-fly using o3d visualzier or not [press space to pasue/resume] self.o3d_vis_raw: bool = False # visualize the raw point cloud or the weight source point cloud self.log_freq_frame: int = 0 # save the result log per x frames - self.mesh_freq_frame: int = 10 # do the reconstruction per x frames + self.mesh_freq_frame: int = 20 # do the reconstruction per x frames self.sdfslice_freq_frame: int = 1 # visualize the SDF slice per x frames self.vis_sdf_slice_v: bool = False # also visualize the vertical SDF slice or not (default only horizontal slice) self.sdf_slice_height: float = -1.0 # initial height of the horizontal SDF slice (m) in sensor frame self.eval_traj_align: bool = True # do the SE3 alignment of the trajectory when evaluating the absolute error # mesh reconstruction, marching cubes related - self.mc_res_m: float = 0.1 # resolution for marching cubes + self.mc_res_m: float = 0.3 # resolution for marching cubes self.pad_voxel: int = 2 # pad x voxels on each side self.skip_top_voxel: int = 2 # slip the top x voxels (mainly for visualization indoor, remove the roof) self.mc_mask_on: bool = True # use mask for marching cubes to avoid the artifacts @@ -324,7 +325,7 @@ def load(self, config_file): self.first_frame_ref = config_args["setting"].get("first_frame_ref", self.first_frame_ref) self.begin_frame = config_args["setting"].get("begin_frame", 0) self.end_frame = config_args["setting"].get("end_frame", self.end_frame) - self.every_frame = config_args["setting"].get("every_frame", 1) + self.step_frame = config_args["setting"].get("step_frame", 1) self.seed = config_args["setting"].get("random_seed", self.seed) self.device = config_args["setting"].get("device", "cuda") # or cpu, on cpu it's about 5 times slower @@ -337,7 +338,7 @@ def load(self, config_file): self.deskew = config_args["setting"].get("deskew", self.deskew) # apply motion undistortion or not self.valid_ts_in_points = config_args["setting"].get("valid_ts", self.valid_ts_in_points) - if self.every_frame > 1: + if self.step_frame > 1: self.deskew = False # process @@ -486,6 +487,7 @@ def load(self, config_file): self.ba_freq_frame = config_args["optimizer"].get("ba_freq_frame", 0) # default off self.ba_frame = config_args["optimizer"].get("ba_local_frame", self.ba_frame) self.lr_pose = float(config_args["optimizer"].get("lr_pose_ba", self.lr_pose)) + self.lr_map_ba = float(config_args["optimizer"].get("lr_map_ba", self.lr)) self.ba_iters = int(config_args["optimizer"].get("ba_iters", self.ba_iters)) self.ba_bs = int(config_args["optimizer"].get("ba_bs", self.ba_bs)) diff --git a/utils/loop_detector.py b/utils/loop_detector.py index 37781b7..054ab42 100644 --- a/utils/loop_detector.py +++ b/utils/loop_detector.py @@ -209,9 +209,9 @@ def detect_global_loop( ")", ) # print("[bold red]Candidate global loop event detected: [/bold red]", self.curr_node_idx, "---", loop_id, "(" , loop_cos_dist, ")") - else: - if not self.silence: - print("No global loop") + # else: + # if not self.silence: + # print("No global loop") return loop_id, loop_cos_dist, loop_transform, local_map_context_loop @@ -434,8 +434,8 @@ def detect_local_loop( ) return loop_id, loop_dist, loop_transform else: - if not silence: - print("No local loop") + # if not silence: + # print("No local loop") return None, None, None diff --git a/utils/mapper.py b/utils/mapper.py index 80477ea..d0e6b95 100644 --- a/utils/mapper.py +++ b/utils/mapper.py @@ -12,7 +12,6 @@ import torch import torch.nn.functional as F import wandb -import pandas as pd from rich import print from tqdm import tqdm @@ -354,11 +353,11 @@ def process_frame( self.cur_sample_count = coord.shape[0] self.pool_sample_count = self.coord_pool.shape[0] - if not self.silence: - print( - "# Total sample in pool: ", self.pool_sample_count - ) # including the current samples - print("# Current sample : ", self.cur_sample_count) + # if not self.silence: + # print( + # "# Total sample in pool: ", self.pool_sample_count + # ) # including the current samples + # print("# Current sample : ", self.cur_sample_count) T3_2 = get_time() @@ -410,8 +409,8 @@ def process_frame( ) # new idx in the data pool new_sample_count = self.new_idx.shape[0] - if not self.silence: - print("# New sample : ", new_sample_count) + # if not self.silence: + # print("# New sample : ", new_sample_count) # for determine adaptive mapping iteration self.adaptive_iter_offset = 0 @@ -494,7 +493,7 @@ def get_batch(self, global_coord=False): return coord, sdf_label, ts, normal_label, sem_label, color_label, weight - # get a batch of training samples (only those measured points) and labels for local bundle adjustment + # get a batch of training samples (only those measured end points) and labels for local bundle adjustment def get_ba_samples(self, subsample_count): surface_sample_idx = torch.where(self.sdf_label_pool == 0)[0] @@ -545,6 +544,9 @@ def get_data_pool_o3d(self, down_rate=1, only_cur_data=False): data_pool_pc_o3d = o3d.geometry.PointCloud() data_pool_pc_o3d.points = o3d.utility.Vector3dVector(pool_coord_np) + if self.sdf_label_pool is None: + return data_pool_pc_o3d + if only_cur_data: pool_label_np = ( self.sdf_label_pool[-self.cur_sample_count :: 3] @@ -781,7 +783,7 @@ def mapping(self, iter_count): sem_label > 0 ) # only use the points with labels (even those with free space labels would not be used) sem_pred = sem_pred[label_mask] - sem_label = sem_label[label_mask] + sem_label = sem_label[label_mask].long() sem_loss = loss_nll( sem_pred[:: self.config.sem_label_decimation, :], sem_label[:: self.config.sem_label_decimation], @@ -863,7 +865,8 @@ def bundle_adjustment( # also add the poses as param here, for pose refinement (bundle ajustment) opt = setup_optimizer( - self.config, neural_point_feat, poses=current_poses_se3_opt, lr_ratio=1.0 + self.config, neural_point_feat, + poses=current_poses_se3_opt, lr_ratio=self.config.lr_ba_map/self.config.lr ) for iter in tqdm(range(iter_count), disable=self.silence): diff --git a/utils/pgo.py b/utils/pgo.py index a0099ed..0e13a67 100644 --- a/utils/pgo.py +++ b/utils/pgo.py @@ -325,8 +325,8 @@ def estimate_drift( travel_dist[self.min_loop_idx] + travel_dist[used_frame_id] * correct_ratio ) * drfit_ratio - if not self.silence: - print("Estimated drift (m):", self.drift_radius) + # if not self.silence: + # print("Estimated drift (m):", self.drift_radius) def plot_loops(self, loop_plot_path, vis_now=False): diff --git a/utils/tools.py b/utils/tools.py index 9216e47..c1a2a04 100644 --- a/utils/tools.py +++ b/utils/tools.py @@ -75,7 +75,12 @@ def setup_experiment(config: Config, argv=None, debug_mode: bool = False): # config file and reproducable shell script if argv is not None: - shutil.copy2(argv[1], run_path) # copy the config file to the result folder + if len(argv) > 1 and os.path.exists(argv[1]): + config_path = argv[1] + else: + config_path = "config/lidar_slam/run.yaml" + # copy the config file to the result folder + shutil.copy2(config_path, run_path) git_commit_id = ( subprocess.check_output(["git", "rev-parse", "HEAD"]).decode().strip() diff --git a/utils/tracker.py b/utils/tracker.py index 541d6b3..7459e85 100644 --- a/utils/tracker.py +++ b/utils/tracker.py @@ -204,7 +204,7 @@ def tracking( if cov_mat is not None: cov_mat = cov_mat.detach().cpu().numpy() - if not valid_flag: # NOTE: if not valid, just take the initial guess + if not valid_flag and i < 10: # NOTE: if not valid and without enough iters, just take the initial guess T = init_pose cov_mat = None