gisbi-kim / pyicp-slam Goto Github PK

View Code? Open in Web Editor NEW

454.0 454.0 77.0 19.98 MB

Full-python LiDAR SLAM using ICP and Scan Context

Python 100.00%

icp lidar loop odometry place pointcloud recognition robot scan slam

pyicp-slam's Introduction

PyICP SLAM

Full-python LiDAR SLAM.

If you find a C++ version of this repo, go to SC-LeGO-LOAM or SC-A-LOAM.

Purpose

Full-python LiDAR SLAM
- Easy to exchange or connect with any Python-based components (e.g., DL front-ends such as Deep Odometry)
  - Here, ICP, which is a very basic option for LiDAR, and Scan Context (IROS 18) are used for odometry and loop detection, respectively.
Hands-on LiDAR SLAM
- Easy to understand (could be used for educational purpose)
The practical use case of miniSAM
- The miniSAM is easy to use at Python

What is SLAM?

In this repository, SLAM (Simultaneous localization and mapping) is considered as
- SLAM = Odometry + Loop closure
In this repository, the state to be optimized only has robot poses; that is pose-graph SLAM.

Overview of the PyICP SLAM

The pipeline of the PyICP SLAM is composed of three parts
1. Odometry: ICP (iterative closest point)
  - In here, Point-to-point and frame-to-frame (i.e., no local mapping)
2. Loop detection: Scan Context (IROS 18)
  - Reverse loop detection is supported.
3. Back-end (graph optimizer): miniSAM
  - Python API

Features

Thanks to the Scan Context, reverse loops can be successfully closed.
- E.g., see KITTI 14 at Results section below.
Time costs
- (Here, no accelerated and naive) ICP gets 7-10 Hz for randomly downsampled points (7000 points)
- (Here, no accelerated and naive) Scan Context gets 1-2 Hz (when 10 ringkey candidates).
  - if you need the more faster one, consider the C++ version of Scan Context (e.g., https://github.com/irapkaist/SC-LeGO-LOAM)
- miniSAM is enough fast.

How to use

Just run

$ python3 main_icp_slam.py

The details of parameters are eaily found in the argparser in that .py file.

Results (KITTI dataset)

Those results are produced under the same parameter conditions:

ICP used random downsampling, 7000 points.
Scan Context's parameters:
- Ring: 20, Sector: 60
- The number of ringkey candidates: 30
- Correct Loop threshold: 0.17 for 09, 0.15 for 14, and 0.11 for all others

Results (left to right):

00 (loop), 01, 02 (loop), 03

04, 05 (loop), 06 (loop), 09 (loop)

10, 11, 12, 13 (loop)

14 (loop), 15 (loop), 16 (loop), 17

18 (loop), 20

Some of the results are good, and some of them are not enough. Those results are for the study to understand when is the algorithm works or not.

Findings

The Scan Context does not find loops well when there is a lane level change (i.e., KITTI 08, as below figures).
If the loop threshold is too low (0.07 in the below figure), no loops are detected and thus the odometry errors cannot be reduced.
If the loop threshold is high (0.20 in the below figure), false loops are detected and thus the graph optimization failed.
- but using this non-conservative threshold with a robust kernel would be a solution.
  - see https://github.com/irapkaist/SC-LeGO-LOAM/blob/3170ba1de7bb0d3f8de5192742e0bbe267415ab4/SC-LeGO-LOAM/LeGO-LOAM/src/mapOptmization.cpp#L994

Author

  Giseop Kim ([email protected])

Contirbutors

  @JustWon
    - Supports Pangolin-based point cloud visualization along the SLAM poses.
    - Go to https://github.com/JustWon/PyICP-SLAM

pyicp-slam's People

Contributors

Stargazers

Watchers

pyicp-slam's Issues

do you have some benchmark result in kitti odometry lidar dataset ?

module 'minisam' has no attribute 'DiagonalLoss'

I am using PyICP github repo. I built the Sophus from this commit (commit a0fe89a323e20c42d3cecb590937eb7a06b8343a) Reference.
I am using Ubuntu 22 and used virtual environment (venv with python version 3.7.14) to build the PyICP repo, minisam and Sophus repo.

Finally when I run the command python3 main_icp_slam.py it runs in this error.

I am not sure is it caused by Sophus, Eigen, Venv or minisam.

Let me know if anymore information is needed.

Thanks, for any and all the help.

Have a question on a 4x4 matrix parameter in UtilsMisc.py

Hi, im working on a project having registration with only depth images.

Im now trying to understand and modifying to do a job, and I got a simple question on your code in line 64, UtilsMisc.py.

Why did you put 0.1 instead of 1 in the optimized_pos_ith 4x4 matrix. I know there's no difference in the result, but I just wanna know the reason, if there a benefit from it or not.

Some questions about this project

Hi!
This project is really cool. I have some questions about how to using this project and need your help.
Does this project support to output a global map like Le-GO LOAM? I need the final point cloud for some other research.

Regarding the noise models

Hi,

Thank you for this project, it has been very useful.

I was wondering if there is any heuristic for choosing the covariance values that you have used here, and to what extent they affect the optimization.

self.prior_cov = minisam.DiagonalLoss.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) 
self.const_cov = np.array([0.5, 0.5, 0.5, 0.1, 0.1, 0.1])
self.odom_cov = minisam.DiagonalLoss.Sigmas(self.const_cov)
self.loop_cov = minisam.DiagonalLoss.Sigmas(self.const_cov)

Any insight on this would be helpful.

core dumped when i run the script

python3 main_icp_slam.py
Segmentation fault (core dumped)

AttributeError: module 'minisam' has no attribute 'SE3'

Hi,
I have a similar problem as this issue,
File "/home/win/.vscode/extensions/ms-python.python-2022.18.2/pythonFiles/lib/python/debugpy/_vendored/pydevd/_pydevd_bundle/pydevd_runpy.py", line 124, in _run_code
exec(code, run_globals)
File "/home/win/work/PyICP-SLAM/main_icp_slam.py", line 53, in
PGM.addPriorFactor()
File "/home/win/work/PyICP-SLAM/utils/PoseGraphManager.py", line 32, in addPriorFactor
self.graph_initials.add(minisam.key('x', self.curr_node_idx), minisam.SE3(self.curr_se3))
AttributeError: module 'minisam' has no attribute 'SE3'.

how can I solve this?

Can not find DiagonalLoss ?

Hi @kissb2 I have run PyICP-SLAM but after installing minisam, I ran into a problem that is:
AttributeError: module 'minisam' has no attribute 'DiagonalLoss
I have built successfully like the guidance in the minisam repo. Could you help me, please ?

Question about the generated .csv file

First of all, thank you very much for your excellent work. When I use the code to run the kitti dataset, there will be multiple .csv files in the result folder after running a sequence every time. Why is this? In other words, I want to compare the saved pose with groundtruth, but the multiple files generated make me a little confused.
Thanks again!