cornellev / icp

Ethan's implementation of scan-matching.

icp

Last updated 2024-05-05 02:20:55.300469.
Made by Ethan Uppal.

Introduction

This repository hosts Ethan's implementation of Iterative Closest Points (ICP) as applied to scan matching. It is a first step in my project to implement Simultaneous Localization and Mapping (SLAM).

Install

You can view installation instructions at INSTALL.md.

Usage and Documentation

I host the usage information and documentation at cornellev.github.io/icp/. Please see there for information on how to download and how to use the library.

You can build the documentation yourself locally with make docs. The main page will be located at docs/index.html relative to the project root.

v1.2.1

Started work on a new implementation (read this paper). It passes all make test tests. Here is the performance currently:

ICP ALGORITHM BENCHMARKING
=======================================
* Method name: test1
* Number of trials: 50
* Burn-in period: 0
* Ideal convergence threshold: 20
* Min cost: 19.4709
* Max cost: 19.4709
* Median cost: 19.4709
* Mean cost: 19.4709
* Min iterations: 7 (real: 7)
* Max iterations: 7 (real: 7)
* Median iterations: 7 (real: 7)
* Mean iterations: 7 (real: 7)
* Average time per invocation: 0.00413281s

v1.1.x

The algorithm runs extremely fast now. We only need it to run at 6hz with our current LiDAR.

It also matches quite well. Below is the result of running this ICP implementation on two point clouds obtained within the workspace.

However, there is still remove for improvement with regard to outlier rejection and other parts of the algorithm (for instance, adopting a point-to-line metric).

icp's Issues

Test ICP on actual scan matching instance

You have been testing ICP on two point clouds that have been separated, which is not the actual use case. Construct a sin wave wall, segment it, and overlay. The ICP algorithm should successfully slide the two sin waves together. If it gets trapped in a local minimum already and does not move to align, there is a serious issue.

TODO

https://github.com/ethanuppal/cev-log/blob/main/log/CEV%204-17-24.md

ICP converges but not accurate in general

Two strategies (both of which I have downloaded papers for:

• Outlier rejection
• Point to line

Other option

• I have an error with my ICP implementation
• If the other two don't work then I will have to believe this

Arbitrary Rigid-Body Transformation

Currently, rotations are only around the center of mass. Update the transform, visualization, instance construction, and optimization code to handle an arbitrary rigid-body transformation.

Clean up code

main.cpp and some other code needs to be cleaned up. Remove code in old/ folders.

Optimize Gradient Descent

Possible options to increase speed of convergence using gradient descent:

• fine tune the learning rate up (this is dangerous if gradients can grow very large)
• use something like adagrad to guarantee convergence on all data sets (even with exterme gradients)
• use newtons method when descent slows past some metric (defining this will be data dependent and may be hard to find) to converge to a local min instantly

Generally fix code quality + improve ICP modularity

• You need to fix your code quality, which might involve editing sdl-wrapper
• You also need to make ICP more modular; instead of taking in fixed point sets, for example, you allow dynamic setting of point sets (but the $n$ stays the same)
• You should try to see if quad trees let you speed up performance
  • Lidar range is bounded, so it works maybe?

Major cleanups

• Rename repo and update references
• Factor out messy build and install code
• more stuff as comes to mind

Implement ICP with translation and point-to-point

Implement rotation into ICP