aevainc / doppler-icp

Official code release for Doppler ICP

DICP: Doppler Iterative Closest Point Algorithm

Link to oral presentation at RSS 2022 | Link to paper

Doppler ICP is a novel algorithm for point cloud registration for range sensors capable of measuring per-return instantaneous radial velocity. Existing variants of ICP that solely rely on geometry or other features generally fail to estimate the motion of the sensor correctly in scenarios that have non-distinctive features and/or repetitive geometric structures such as hallways, tunnels, highways, and bridges.

We jointly optimize a new Doppler velocity objective function that exploits the compatibility of each point's Doppler measurement and the sensor's current motion estimate, and the geometric objective function which sufficiently constrains the point cloud alignment problem even in feature-denied environments.

Comparison of tunnel reconstructions using point-to-plane ICP (left) and Doppler ICP (right) with measurements collected by an FMCW LiDAR. Point-to-plane ICP fails in this degenerate case due to the lack of distinctive features in the scene whereas the Doppler ICP algorithm is able to reconstruct the scene with very low error.

Setup and Dependencies

Clone this repository along with the submodules.

git clone --recurse-submodules https://github.com/aevainc/Doppler-ICP.git

To download the sample dataset, pull the LFS files.

git lfs pull

Install the Python dependencies.

pip install -r requirements.txt

The Doppler ICP algorithm is implemented in our fork of Open3D library. You need to build the Open3D wheel file for Python from source and install it in your Python environment. See the Open3D docs for more details.

cd Open3D
mkdir build
cd build
cmake ..
make -j$(nproc) install-pip-package

Dataset

Download link for CARLA-Town04-Straight-Walls and CARLA-Town05-Curved-Walls sequences.

Extract and copy the downloaded sequences to the dataset/ directory in the repository root. A sample sequence has been provided in the dataset. The file structure should look like the following:

REPOSITORY_ROOT/dataset/
├── sample/
│   ├── point_clouds/
│   │   ├── 00001.bin  # NUM_POINTS * (3 + 1) float32 bytes containing XYZ points and Doppler velocities.
│   │   ├── 00002.bin
│   │   └── ...
│   ├── calibration.json
│   └── ref_poses.txt  # NUM_SCANS reference poses with timestamps in TUM format.
├── carla-town04-straight-walls/
│   └── ...
└── ...

Usage

Run Doppler ICP on the sample sequence and display the results. Additional registration results for all pairs of scans and the ICP poses for the sequence (in TUM format) are saved in OUTPUT_DIR.

foo@bar:~/Doppler-ICP/scripts$ python run.py --sequence sample -o OUTPUT_DIR

Run point-to-plane ICP on a sequence downloaded in another directory (for frames 100-150).

foo@bar:~/Doppler-ICP/scripts$ python run.py --sequence /tmp/carla-town05-curved-walls \
    -o /tmp/town05-output -s 100 -e 150 -m point_to_plane

Visualize the sample point cloud sequence (colored by the Doppler velocity channel) using Open3D.

foo@bar:~/Doppler-ICP/scripts$ python visualize.py --sequence sample

Visualize the trajectories generated from the registration algorithms and the reference trajectory from the dataset using evo.

foo@bar:~$ evo_traj tum SEQUENCE_DIR/ref_poses.txt OUTPUT_DIR/icp_poses.txt --plot --plot_mode xyz

Citations

If you use Doppler ICP in your work, please cite the corresponding paper.

@INPROCEEDINGS{Hexsel-RSS-22, 
    AUTHOR    = {Bruno Hexsel AND Heethesh Vhavle AND Yi Chen}, 
    TITLE     = {{DICP: Doppler Iterative Closest Point Algorithm}}, 
    BOOKTITLE = {Proceedings of Robotics: Science and Systems}, 
    YEAR      = {2022}, 
    ADDRESS   = {New York City, NY, USA}, 
    MONTH     = {June}, 
    DOI       = {10.15607/RSS.2022.XVIII.015} 
}

Poster

doppler-icp's Issues

sample data can't be read correctly

Very impressive work, and I just try to reproduce the result in the paper.
But when I try to run python run.py --sequence sample -o OUTPUT_DIR , I got the error:

PID: [1024933]
Loaded sequence: sample with 10 scans
Loaded 10 poses from ref_poses.txt (0.90 secs)
Processing: 11%|███████████████ | 1/9 [00:00<?, ?it/s]
Traceback (most recent call last):
File "run.py", line 234, in
run(args)
File "run.py", line 104, in run
source = utils.load_point_cloud(pcd_files[i])
File "/home/zhiyuan/share_folder/DICP-verificarion/Doppler-ICP/scripts/utils.py", line 166, in load_point_cloud
data = np.fromfile(filename, dtype=np.float32).reshape(-1, 4)
ValueError: cannot reshape array of size 33 into shape (4)

It seems that the sample data's size is not correct. Just want confirm is the sample data is uploaded correctly, or is there any step I missed(I basically just followed the readme...)

Hope for your help !

How to get the other 5 sequences which from real sensor?

Thank your for excellent work!
And I can download 2 sequences which from simulator according the link in your repo.
May I get the other 5 sequences dataset?

Thank your great work again!

Aeve AeriesII ROS driver support ?

Hello, nice job.
May I ask if you provide ROS drivers for Aeve AeriesII ?
Looking for your reply soon ~

Dataset of FMCW lidar

Excellent work! Is there any plan to open source the experimental data? Or is there an open source dataset of related FMCW lidars?

How to calibrate lidar to vehicle?

hi, I wonder that how to calibrate lidar to vehicle? Could I regard wheel odometry as vehicle(body coordinate)?

About Odometry and Relative Pose

Hi! I sorry for that I'm not good at English.

I am begginer at Lidar ICP So I am studying with Your Code. Thank You.

I curious about this code

    results['poses'].append(np.linalg.inv(result.transformation))
    results['ref_poses'].append(np.linalg.inv(ref_transform))

1. Why is inversed? I learned that pose is same as transformation.

    source = utils.load_point_cloud(pcd_files[i])
    target = utils.load_point_cloud(pcd_files[i + 1])
   

2. why target is not Transformed? I learned that transform target to World Coordinate and Find transformation from source to target.

Thank You For Your Code!