Robust Ego and Object 6-DoF Motion Estimation and Tracking

Jun Zhang, Mina Henein, Robert Mahony, Viorela Ila

TL;DR

This paper introduces a novel framework for robustly estimating both the camera's motion (ego-motion) and the motion of multiple dynamic objects within a scene, using only visual data. By jointly optimizing motion estimation and optical flow, and leveraging semantic instance segmentation, their approach significantly improves tracking accuracy and robustness in dynamic environments, crucial for applications like autonomous driving.

What Problem Does This Paper Address?

Visual Odometry (VO) is a fundamental technique for robot navigation, typically excelling in static environments. However, real-world scenarios, especially in autonomous driving, are inherently dynamic, with moving objects like cars and pedestrians. Standard VO methods struggle with these dynamic elements, often failing or producing inaccurate trajectory estimates. While some approaches try to simply remove dynamic points, this paper argues that this information is valuable and can be leveraged. The challenge is to accurately track both the camera's own movement and the 6-DoF (six degrees of freedom: translation and rotation) motion of multiple objects simultaneously and robustly, even with occlusions and varying distances.

Methods

The core of the proposed system lies in a novel formulation that jointly optimizes the camera's SE(3) motion and the optical flow, alongside the 6-DoF motion of individual objects.

  • Instance-Level Semantic Segmentation: The pipeline begins by using advanced semantic instance segmentation (specifically Mask R-CNN) to identify and separate individual objects from the background. This is crucial for treating each object independently.
  • Dense Optical Flow: To ensure robust tracking of points on moving objects, which can be small or far away, the system utilizes dense optical flow (e.g., PWC-Net). This provides a much richer set of correspondences than sparse feature detection alone.
  • Joint Optimization: The key innovation is a unified optimization framework. Instead of estimating optical flow and then using it for motion estimation, the paper proposes to refine the optical flow simultaneously with the SE(3) motion estimation for both the camera and each dynamic object. This is achieved by minimizing a cost function that includes terms for re-projection error (using the refined optical flow) and a prior on the optical flow itself. This joint approach helps to correct errors in both estimations.
  • Dynamic Object Classification: To distinguish between static and dynamic objects, the system computes "scene flow" (the 3D motion of points) and uses a thresholding mechanism on the magnitude of this flow. Objects with a significant proportion of "dynamic" points are classified as such.
  • Object Tracking: Instance segmentation provides per-frame object masks. To track objects across frames, optical flow is used to associate point labels. A finite tracking label set is maintained, and objects are assigned new tracking labels if their most frequent correspondence label in the previous frame is "background" (indicating a new object appearance or re-appearance).

Datasets

  • Virtual KITTI Dataset: Used for detailed analysis and to study the influence of optical flow and depth accuracy on motion estimation. This dataset provides ground truth for ego-motion, object poses, depth, optical flow, and instance-level segmentation.
  • Real KITTI Dataset: Used to demonstrate the applicability of the algorithm in real-world scenarios, specifically the KITTI tracking dataset.

Key Insights and Ideas

  • Leveraging Dynamic Information: The paper strongly advocates for using dynamic object information rather than discarding it, demonstrating that it can significantly improve overall scene understanding and localization.
  • Joint Optimization is Key: The most impactful insight is that jointly optimizing motion and optical flow leads to substantial improvements. This mutual refinement helps correct errors that would otherwise propagate through separate estimation steps.
  • Instance Segmentation as a Foundation: The reliance on instance-level semantic segmentation provides a robust way to isolate and track individual objects, which is a critical prerequisite for multi-body motion estimation.
  • Dense Flow for Robustness: Using dense optical flow, especially for dynamic objects, is shown to be crucial for maintaining trackability and accuracy, particularly for objects that are small or far from the camera.

Key Results

  • Improved Accuracy: The proposed method shows significant improvements in both camera ego-motion and object motion estimation compared to baseline methods that only optimize motion. For instance, camera motion accuracy improved by about 300% and object motion by 10-20% when jointly optimizing with optical flow.
  • Robustness to Noise: The system demonstrates robustness to depth noise, with translation errors remaining low (Et < 0.02m) even with moderate noise levels.
  • Accurate Object Velocity Estimation: For object motion, the system achieves an average velocity error of around 2-5 km/h, which is considered highly accurate for typical driving speeds.
  • Comparison with ORB-SLAM2: On the real KITTI dataset, the proposed method produced smoother and more consistent camera trajectories compared to ORB-SLAM2, particularly in dynamic sequences.
  • Computational Cost: The algorithm runs at approximately 6fps on a laptop with an i7 2.6Ghz processor, with the main cost attributed to dense point tracking on multiple objects.

Limitations and Future Work

  • Object Size and Distance Sensitivity: The paper acknowledges that estimation accuracy for objects that are very far away or occupy a very small image area (<1%) can be sensitive to optical flow errors. To mitigate this, the system currently filters objects beyond 25m or those occupying less than 0.5% of the image.
  • Computational Cost: While achieving 6fps is respectable, further improvements for real-time performance could be achieved through parallel implementation.
  • Future Work: The authors plan to integrate the proposed motion estimation framework within a full SLAM system, which would allow for long-term localization and mapping in dynamic environments.

Summary updated

Abstract

The problem of tracking self-motion as well as motion of objects in the scene using information from a camera is known as multi-body visual odometry and is a challenging task. This paper proposes a robust solution to achieve accurate estimation and consistent track-ability for dynamic multi-body visual odometry. A compact and effective framework is proposed leveraging recent advances in semantic instance-level segmentation and accurate optical flow estimation. A novel formulation, jointly optimizing SE(3) motion and optical flow is introduced that improves the quality of the tracked points and the motion estimation accuracy. The proposed approach is evaluated on the virtual KITTI Dataset and tested on the real KITTI Dataset, demonstrating its applicability to autonomous driving applications. For the benefit of the community, we make the source code public.
BibTeX
@misc{zhang2020robust,
  author = {Jun Zhang and Mina Henein and Robert Mahony and Viorela Ila},
  title = {{Robust Ego and Object 6-DoF Motion Estimation and Tracking}},
  year = {2020},
  eprint = {2007.13993},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2007.13993},
}
Figure