Skip to content

Commit

Permalink
[UPDATE] drop kiss-loader dependency, update readme
Browse files Browse the repository at this point in the history
  • Loading branch information
Yue Pan committed Jun 5, 2024
1 parent 3c273d2 commit dfa5b17
Show file tree
Hide file tree
Showing 29 changed files with 2,279 additions and 137 deletions.
26 changes: 19 additions & 7 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
<h1 align="center">📍PIN-SLAM: LiDAR SLAM Using a Point-Based Implicit Neural Representation for Achieving Global Map Consistency</h1>

<p align="center">
<a href="https://github.com/PRBonn/PIN_SLAM/releases"><img src="https://img.shields.io/github/v/release/PRBonn/PIN_SLAM?label=version" /></a>
<a href="https://github.com/PRBonn/PIN_SLAM#run-pin-slam"><img src="https://img.shields.io/badge/python-3670A0?style=flat-square&logo=python&logoColor=ffdd54" /></a>
<a href="https://github.com/PRBonn/PIN_SLAM#installation"><img src="https://img.shields.io/badge/Linux-FCC624?logo=linux&logoColor=black" /></a>
<a href="https://arxiv.org/pdf/2401.09101v1.pdf"><img src="https://img.shields.io/badge/Paper-pdf-<COLOR>.svg?style=flat-square" /></a>
Expand Down Expand Up @@ -114,6 +115,7 @@ PIN-SLAM can run at the sensor frame rate on a moderate GPU.

* Windows/MacOS with CPU-only mode


### 1. Set up conda environment

```
Expand All @@ -137,9 +139,11 @@ pip3 install open3d==0.17 scikit-image gtsam wandb tqdm rich roma natsort pyquat

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
pip3 install laspy ouster-sdk rospkg
```

----

## Run PIN-SLAM

### Clone the repository
Expand Down Expand Up @@ -227,18 +231,26 @@ python3 pin_slam.py ./config/lidar_slam/run_ncd.yaml ncd 01 -vsm
python3 pin_slam.py ./config/rgbd_slam/run_replica.yaml replica room0 -vsm
```

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:
We also support loading data from rosbag, mcap or pcap using specific data loaders (originally from [KISS-ICP](https://github.com/PRBonn/kiss-icp)). You need to set the flag `-d` 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
python3 pin_slam.py ./config/lidar_slam/run.yaml rosbag point_cloud_topic_name -i /path/to/the/rosbag -vsmd
# 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
python3 pin_slam.py ./config/lidar_slam/run.yaml rosbag -i /path/to/the/rosbag -vsmd
```

The data loaders for some specific datasets are also available. For example, you can run on Replica RGB-D dataset without preprocessing the data by:
```
# Download data
sh scripts/download_replica.sh
# Run PIN-SLAM
python3 pin_slam.py ./config/rgbd_slam/run_replica.yaml replica room0 -i data/Replica -vsmd
```

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.
For evaluation, 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 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.

Expand Down Expand Up @@ -296,9 +308,9 @@ python3 vis_pin_map.py path/to/your/result/folder [marching_cubes_resolution_m]
<details>
<summary>[Details (click to expand)]</summary>

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.
The bounding box of `(cropped)_map_file.ply` will be used as the bounding box for mesh reconstruction. This file should be stored in the `map` subfolder of the result folder. You may directly use the original `neural_points.ply` or crop the neural points in software such as CloudCompare. The argument `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 reconstructed mesh would be saved as `output_mesh_file.ply` in the `mesh` subfolder of the result folder.

For example, for the case of the sanity test, run:
For example, for the case of the sanity test described above, run:

```
python3 vis_pin_map.py ./experiments/sanity_test_* 0.2 neural_points.ply mesh_20cm.ply 8
Expand Down
5 changes: 4 additions & 1 deletion config/lidar_slam/run.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -22,4 +22,7 @@ pgo:
context_cosdist: 0.3
optimizer: # mapper
batch_size: 10000
adaptive_iters: True
adaptive_iters: True
eval:
wandb_vis_on: False # log to wandb or not
silence_log: True
3 changes: 1 addition & 2 deletions config/rgbd_slam/run_replica.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,7 @@ optimizer:
ba_freq_frame: 20
lr_pose_ba: 1e-3
eval:
o3d_vis_on: False # visualize the mapping or not
mesh_freq_frame: 100 # reconstruct the mesh every x frames
mesh_freq_frame: 50 # reconstruct the mesh every x frames
mesh_min_nn: 9
sensor_cad_path: ./cad/camera.ply
skip_top_voxel: 5
83 changes: 83 additions & 0 deletions dataset/dataloaders/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
# MIT License
#
# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
# Stachniss.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
from pathlib import Path
from typing import Dict, List


def supported_file_extensions():
return [
"bin",
"pcd",
"ply",
"xyz",
"obj",
"ctm",
"off",
"stl",
]


def sequence_dataloaders():
# TODO: automatically infer this
return ["kitti", "kitti_raw", "nuscenes", "helipr", "replica"]


def available_dataloaders() -> List:
import os.path
import pkgutil

pkgpath = os.path.dirname(__file__)
return [name for _, name, _ in pkgutil.iter_modules([pkgpath])]


def jumpable_dataloaders():
_jumpable_dataloaders = available_dataloaders()
_jumpable_dataloaders.remove("mcap")
_jumpable_dataloaders.remove("ouster")
_jumpable_dataloaders.remove("rosbag")
return _jumpable_dataloaders


def dataloader_types() -> Dict:
import ast
import importlib

dataloaders = available_dataloaders()
_types = {}
for dataloader in dataloaders:
script = importlib.util.find_spec(f".{dataloader}", __name__).origin
with open(script) as f:
tree = ast.parse(f.read(), script)
classes = [cls for cls in tree.body if isinstance(cls, ast.ClassDef)]
_types[dataloader] = classes[0].name # assuming there is only 1 class
return _types


def dataset_factory(dataloader: str, data_dir: Path, *args, **kwargs):
import importlib

dataloader_type = dataloader_types()[dataloader]
module = importlib.import_module(f".{dataloader}", __name__)
assert hasattr(module, dataloader_type), f"{dataloader_type} is not defined in {module}"
dataset = getattr(module, dataloader_type)
return dataset(data_dir=data_dir, *args, **kwargs)
71 changes: 71 additions & 0 deletions dataset/dataloaders/apollo.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,71 @@
# MIT License
#
# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
# Stachniss.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
import glob
import importlib
import os
import sys
from pathlib import Path

import natsort
import numpy as np
from pyquaternion import Quaternion


class ApolloDataset:
def __init__(self, data_dir: Path, *_, **__):
try:
self.o3d = importlib.import_module("open3d")
except ModuleNotFoundError:
print(
'pcd files requires open3d and is not installed on your system run "pip install open3d"'
)
sys.exit(1)

self.scan_files = natsort.natsorted(glob.glob(f"{data_dir}/pcds/*.pcd"))
self.gt_poses = self.read_poses(f"{data_dir}/poses/gt_poses.txt")
self.sequence_id = os.path.basename(data_dir)

def __len__(self):
return len(self.scan_files)

def __getitem__(self, idx):
return self.get_scan(self.scan_files[idx])

def get_scan(self, scan_file: str):
points = np.asarray(self.o3d.io.read_point_cloud(scan_file).points, dtype=np.float64)
return points.astype(np.float64)

@staticmethod
def read_poses(file):
data = np.loadtxt(file)
_, _, translations, qxyzw = np.split(data, [1, 2, 5], axis=1)
rotations = np.array(
[Quaternion(x=x, y=y, z=z, w=w).rotation_matrix for x, y, z, w in qxyzw]
)
poses = np.zeros([rotations.shape[0], 4, 4])
poses[:, :3, -1] = translations
poses[:, :3, :3] = rotations
poses[:, -1, -1] = 1
# Convert from global coordinate poses to local poses
first_pose = poses[0, :, :]
return np.linalg.inv(first_pose) @ poses
89 changes: 89 additions & 0 deletions dataset/dataloaders/boreas.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
# MIT License
#
# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
# Stachniss.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
import glob
import os
from pathlib import Path

import natsort
import numpy as np


class BoreasDataset:
def __init__(self, data_dir: Path, *_, **__):
self.root_dir = os.path.realpath(data_dir)
self.scan_files = natsort.natsorted(glob.glob(f"{data_dir}/lidar/*.bin"))
self.gt_poses = self.load_poses(f"{data_dir}/applanix/lidar_poses.csv")
self.sequence_id = os.path.basename(data_dir)
assert len(self.scan_files) == self.gt_poses.shape[0]

def __len__(self):
return len(self.scan_files)

def __getitem__(self, idx):
return self.read_point_cloud(self.scan_files[idx])

def read_point_cloud(self, scan_file: str):
points = np.fromfile(scan_file, dtype=np.float32).reshape((-1, 6))[:, :3]
return points.astype(np.float64), self.get_timestamps(points)

def load_poses(self, poses_file):
data = np.loadtxt(poses_file, delimiter=",", skiprows=1)
n, m = data.shape
t, x, y, z, vx, vy, vz, r, p, ya, wz, wy, wx = data[0, :]
first_pose = self.get_transformation_matrix(x, y, z, ya, p, r)
poses = np.empty((n, 4, 4), dtype=np.float32)
poses[0, :, :] = np.identity(4, dtype=np.float32)
for i in range(n):
t, x, y, z, vx, vy, vz, r, p, ya, wz, wy, wx = data[i, :]
current_pose = self.get_transformation_matrix(x, y, z, ya, p, r)
poses[i, :, :] = np.linalg.inv(first_pose) @ current_pose
return poses

@staticmethod
def get_timestamps(points):
x = points[:, 0]
y = points[:, 1]
yaw = -np.arctan2(y, x)
timestamps = 0.5 * (yaw / np.pi + 1.0)
return timestamps

@staticmethod
def get_transformation_matrix(x, y, z, yaw, pitch, roll):
T_enu_sensor = np.identity(4, dtype=np.float64)
R_yaw = np.array(
[[np.cos(yaw), np.sin(yaw), 0], [-np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]],
dtype=np.float64,
)
R_pitch = np.array(
[[np.cos(pitch), 0, -np.sin(pitch)], [0, 1, 0], [np.sin(pitch), 0, np.cos(pitch)]],
dtype=np.float64,
)
R_roll = np.array(
[[1, 0, 0], [0, np.cos(roll), np.sin(roll)], [0, -np.sin(roll), np.cos(roll)]],
dtype=np.float64,
)
C_enu_sensor = R_roll @ R_pitch @ R_yaw
T_enu_sensor[:3, :3] = C_enu_sensor
r_sensor_enu_in_enu = np.array([x, y, z]).reshape(3, 1)
T_enu_sensor[:3, 3:] = r_sensor_enu_in_enu
return T_enu_sensor
Loading

0 comments on commit dfa5b17

Please sign in to comment.