This is a package for the generation of the optimal shape, described in paper: Optimal Target Shape for LiDAR Pose Estimation (PDF)(arXiv). This work is submitted to IEEE Robotics and Automation Letters.
Both the simulation and the experimental results (verified by a motion capture system) confirm that by using the optimal shape and the global solver, we achieve centimeter error in translation and a few degrees in rotation even when a partially illuminated target is placed 30 meters away.
- Author: Jiunn-Kai (Bruce) Huang, William Clark, and Jessy W. Grizzle
- Maintainer: Bruce JK Huang, brucejkh[at]gmail.com
- Affiliation: The Biped Lab, the University of Michigan
This package has been tested under MATLAB2020a and Ubuntu 18.04.
[Note] More detailed introduction will be updated shortly. Sorry for the inconvenient!
[Issues] If you encounter any issues, I would be happy to help. If you cannot find a related one in the existing issues, please open a new one. I will try my best to help!
- Global Pose Estimation for Optimal Target Shapes
- Overview
- Table of Contents
- Abstract
- Video
- Quick View
- Why Optimal Shape?
- How is the Optimal Generated?
- Pose Definition and Estimation
- Algorithm Evaluations
- Required Toolboxes / Library / Packages
- Paper Results for the Target's Pose
- Paper Results for Target Shape Generation and Make your Own Optimal Target Shape
- Citations
Targets are essential in problems such as object tracking in cluttered or textureless environments, camera (and multi-sensor) calibration tasks, and simultaneous localization and mapping (SLAM). Target shapes for these tasks typically are symmetric (square, rectangular, or circular) and work well for structured, dense sensor data such as pixel arrays (i.e., image). However, symmetric shapes lead to pose ambiguity when using sparse sensor data such as LiDAR point clouds and suffer from the quantization uncertainty of the LiDAR. This paper introduces the concept of optimizing target shape to remove pose ambiguity for LiDAR point clouds. A target is designed to induce large gradients at edge points under rotation and translation relative to the LiDAR to ameliorate the quantization uncertainty associated with point cloud sparseness. Moreover, given a target shape, we present a means that leverages the target’s geometry to estimate the target’s vertices while globally estimating the pose. Both the simulation and the experimental results (verified by a motion capture system) confirm that by using the optimal shape and the global solver, we achieve centimeter error in translation and a few degrees in rotation even when a partially illuminated target is placed 30 meters away.
Please checkout the introduction video. It highlights some important keypoints in the paper!
Both the simulation and the experimental results (verified by a motion capture system) confirm that by using the optimal shape and the global solver, we achieve centimeter error in translation and a few degrees in rotation even when a partially illuminated target is placed 30 meters away.
Symmetric shapes lead to pose ambiguity when using sparse sensor data such as LiDAR point clouds and suffer from the quantization uncertainty of the LiDAR. This paper introduces the concept of optimizing target shape to remove pose ambiguity for LiDAR point clouds. A target is designed to induce large gradients at edge points under rotation and translation relative to the LiDAR to ameliorate the quantization uncertainty associated with point cloud sparseness.
This target can be used in tandem with fiducial markers such as LiDARTag (paper, GitHub) and LiDAR-camera extrinsic calibration (paper, GitHub).
The following shows the resulting optimal shape in arbitrary units.
Projective transformations applied to a nominal quadrilateral generate candidate convex quadrilaterals (targets). Edge points are intersections of LiDAR rings with the target boundaries. The objective is to maximize the gradient of edge points under actions of SE(2) applied to the target. To enhance robustness, the gradients are computed for n-discrete rotations of the quadrilateral under partial illumination, and the score is the worst case.
A coordinate frame for the template (target shown in black) is defined by aligning the plane of the template with the y-z plane of the LiDAR frame and also aligning the mean of its vertices with the origin of the LiDAR.
Let (blue arrow) be an estimate of the rigid-body transformation from target to LiDAR, projecting the edge points of the target back to the template. The estimated pose of the target is then given by the inverse transformation,
(green arrow). The optimal
is obtained by minimizing (18) (based on point-to-line distance). This figure also shows a fitting result of a target at 2 meters in the Ford Robotics Building. The red frame is the template re-projected onto the LiDAR point cloud by
.
We evaluate the proposed algorithm in both simulation and experiments. Both quantitative and qualitative results are provided. We do not compare against standard targets, such as unpatterned rectangles, diamonds, or circles, because their symmetry properties result in pose ambiguity. At large distances, a pattern would not be discernible.
Before carrying out experiments with the new target shape, we used a MATLAB-based LiDAR simulator introduced in GitHub to extensively evaluate the pose and vertex estimation of the optimal shape.
Simulation results of the noise-free dataset of the pose estimation at various distances (10, 20, 30, 40 m). LiDAR returns (blue dots) on the target are provided by the LiDAR simulatorGitHub. Black indicates the ground truth pose from the simulator, and red is the estimated pose and vertices. The top and bottom show the front view and a side view of the fitting results, respectively.
- Subset of Quantitative Results of the Pose and Vertex Estimation Using the Noise-free Dataset: Pose and vertex accuracy of the simulation results at various distances.
- Complete Quantitative Results of Distances and Noise Levels: Simulation results with a target placed at distances from 2 to 40 meters in 2 m increments in the LiDAR simulatorGitHub. At each distance, the simulation data are collected with the target face-on to the LiDAR as an easy case (solid line), and for the other, the target is rotated by the Euler angle (roll = 20 deg , pitch = 30 deg, yaw = 30 deg) as a challenging case (dashed line). In addition, we induce two different levels of noises to each dataset, as indicated by the different colors.
We now present experimental evaluations of the pose and vertex estimation of the optimal shape. All the experiments are conducted with a 32-Beam Velodyne ULTRA Puck LiDAR and an Intel RealSense camera rigidly attached to the torso of a Cassie-series bipedal robot. The Qualisys motion capture system in M-Air Website is used as a proxy for ground truth poses and vertices. The setup consists of 33 motion capture cameras with passive markers attached to the target, the LiDAR and the camera.
For distances beyond 16 meters (the distance limit in M-Air), we present qualitative results from the atrium to support the simulation-based analysis.
- No Missing Points: Fitting results of the optimal shape at various distances (20, 24, 28, 32 meters) in the atrium of the Ford Robotics Building at the University of Michigan. The blue dots are the LiDAR returns on the target and the red frame are the fitting results. The top and bottom show the front view and a side view of the results, respectively.
- Green Points are Considered Missing: Fitting results of the partially illuminated target at various distances (4, 10, 22, 30 meters) in the atrium of the Ford Robotics Building at the University of Michigan. The selected distances are different from No Missing Pionts to show more results. The red frames are the fitting results. The blue dots are the LiDAR returns on the targets while the green dots are considered missing. The top and bottom show the front view and a side view of the fitting results, respectively.
Pose and vertex accuracy of the experimental results. The ground truth is provided by a motion capture system with 33 cameras. Similar to the simulation environment, the optimal-shape target is placed at distances from 2 to 16 meters (maximum possible in M-Air) in 2 meter increments. At each distance, data is collected with a target face-on to the LiDAR and another dataset with the target roughly rotated by the Euler angles (roll = 20 deg, pitch = 30 deg, yaw = 30 deg) as a challenging case.
- Required MATLAB Toolboxes
- Optimization Toolbox
- Phased Array System Toolbox
- Statistics and Machine Learning Toolbox'
- Parallel Computing Toolbox
- Polyspace Bug Finder
- Required Library
- CVX:
- Download CVX for your platform
- Install CVX: Run
cvx_setup.mfrom Matlab
- Please also make sure that you have it installed on your path
- CVX:
- Required Packages
- To generate Fig. 8: double-click
draw_pose_illustration.mand run! - To generate Table 1 and Fig. 10: double-click
gen_sim_table.mand run! - To generate Table 2: double-click
gen_exp_table.mand run! - To generate Fig. 9, 13, and 14:
- double-click
estimate_pose_of_optimal_target.m opts.dataset: 0 for simulation; 6 for experimentstarget_num_list:- Simulation: which list of angle and translation to use
- NOTE: Given the angle and translatoin, we first transform the optimal shape and the use the lidar_simulator to generate the points on the target
- Experiments: which datasets/targets to use
- Simulation: which list of angle and translation to use
opts.target_occlusion: to remove points on rings or not (ONLY SUPPORT FOR EXPERIMENTAL DATA FOR NOW)opts.remove_rings: this will remove the points on rings (ONLY SUPPORT FOR EXPERIMENTAL DATA FOR NOW)- For example: opts.remove_rings = 21:22 will remove all the points on ring 21 and ring 22.
- double-click
To generate Fig. 3, 4, and 7: Please go to another GitHub and follow the introduction there.
The detail is described in: Optimal Target Shape for LiDAR Pose Estimation, Jiunn-Kai Huang, William Clark, and Jessy W. Grizzle. (PDF) (arXiv)
@article{huang2021optimal,
title={Optimal Target Shape for LiDAR Pose Estimation},
author={Jiunn-Kai Huang and William Clark and Jessy W. Grizzle},
year={2021},
journal={arXiv preprint arXiv:2109.01181}
}