Towards Accurate Ground Plane Normal Estimation from Ego-Motion

TL;DR

This paper presents a real-time, odometry-based method for accurately estimating ground plane normals in autonomous driving, improving robustness in dynamic road conditions using an invariant Kalman filter.

Contribution

It introduces a novel approach utilizing ego-motion and an invariant Kalman filter to estimate ground plane normals, adaptable to camera and inertial odometry, with state-of-the-art accuracy.

Findings

Achieves 0.39° vector error on KITTI dataset.

Supports both camera- and inertial-based odometry.

Enhances robustness of autonomous driving tasks.

Abstract

In this paper, we introduce a novel approach for ground plane normal estimation of wheeled vehicles. In practice, the ground plane is dynamically changed due to braking and unstable road surface. As a result, the vehicle pose, especially the pitch angle, is oscillating from subtle to obvious. Thus, estimating ground plane normal is meaningful since it can be encoded to improve the robustness of various autonomous driving tasks (e.g., 3D object detection, road surface reconstruction, and trajectory planning). Our proposed method only uses odometry as input and estimates accurate ground plane normal vectors in real time. Particularly, it fully utilizes the underlying connection between the ego pose odometry (ego-motion) and its nearby ground plane. Built on that, an Invariant Extended Kalman Filter (IEKF) is designed to estimate the normal vector in the sensor's coordinate. Thus, our proposed method is simple yet efficient and supports both camera- and inertial-based odometry algorithms. Its usability and the marked improvement of robustness are validated through multiple experiments on public datasets. For instance, we achieve state-of-the-art accuracy on KITTI dataset with the estimated vector error of 0.39{\deg}. Our code is available at github.com/manymuch/ground_normal_filter.