GeoFusion: Geometric Consistency informed Scene Estimation in Dense Clutter.

Zhiqiang Sui, Haonan Chang, Ning Xu, Odest Chadwicke Jenkins

Abstract

We propose GeoFusion, a SLAM-based scene estimation method for building an object-level semantic map in dense clutter. In dense clutter, objects are often in close contact and severe occlusions, which brings more false detections and noisy pose estimates from existing perception methods. To solve these problems, our key insight is to consider geometric consistency at the object level within a general SLAM framework. The geometric consistency is defined in two parts: geometric consistency score and geometric relation. The geometric consistency score describes the compatibility between object geometry model and observation point cloud. Meanwhile, it provides a reliable measure to filter out false positives in data association. The geometric relation represents the relationship (e.g. contact) between geometric features (e.g. planes) among objects. The geometric relation makes the graph optimization for poses more robust and accurate. GeoFusion can robustly and efficiently infer the object labels, 6D object poses, and spatial relations from continuous noisy semantic measurements. We quantitatively evaluate our method using observations from a Fetch mobile manipulation robot. Our results demonstrate greater robustness against false estimates than frame-by-frame pose estimation from the state-of-the-art convolutional neural network.

Introduction

For robots to perform complex manipulation tasks like setting a table or clearing a desk, they require precise knowledge of the objects present and their 6D poses. While deep learning has significantly advanced object detection, "dense clutter"—where objects are in close contact, overlapping, or heavily occluded—remains a major challenge. In such environments, standard neural networks often produce false positives or noisy pose estimates.

Experimental setup with a Fetch mobile manipulation robot observing a dense tabletop clutter scene.

GeoFusion addresses these issues by integrating geometric consistency into a SLAM-based framework. Unlike traditional systems that treat objects independently, GeoFusion leverages physical constraints—such as the fact that objects sit on surfaces or lean against each other—to filter out "ghost" detections and refine noisy sensor data into a coherent semantic map.

Methodology

GeoFusion employs a two-stage optimization process built on a factor graph, utilizing Mask R-CNN for detection and DenseFusion for initial pose estimation.

Geometric Consistency Score

To validate detections, GeoFusion "back-projects" known 3D CAD models onto the observed point cloud. It calculates a consistency score based on inliers (matching points), outliers, and occlusions. This score acts as a gatekeeper, filtering out false detections during data association.

Geometric Relation Inference

The system identifies physical contacts between objects, specifically focusing on three types of relations: Plane-to-Plane (e.g., a box on a table), Plane-to-Curve (e.g., a can on a table), and Curve-to-Curve.

Two-Stage Optimization

  1. Stage I: A standard SLAM optimization uses robot odometry and raw semantic measurements to generate an initial rough map.
  2. Stage II: The inferred geometric relations are applied as hard constraints. This "snaps" objects into physically plausible positions, ensuring they are flush with surfaces rather than intersecting or hovering.
Overview of the GeoFusion pipeline: from real-world cluttered scene to geometric point cloud and final semantic map reconstruction.

Experimental Evaluation

The system was evaluated using the YCB-Video Dataset for model training and a new Fetch Robot Dataset collected by the authors. This custom dataset features six RGB-D video streams of tabletop scenes with approximately 18 objects in dense clutter, providing 200 keyframes with ground-truth labels.

Key Results

  • Detection Accuracy: GeoFusion achieved a Mean Average Precision (mAP@50) of 73.4, significantly outperforming the frame-by-frame baseline (58.4) and standard SLAM without geometric consistency (56.0).
  • Pose Precision: Approximately 90% of objects in dense scenes achieved a pose error of under 1cm.
  • Efficiency: The system runs on a CPU in under 200ms per frame, making it suitable for real-time robotics.

Discussion and Conclusion

The core innovation of GeoFusion is using geometry as both a filter and a constraint. By being "conservative" and only accepting detections that fit the physical reality of the scene, the system achieves higher accuracy than methods that attempt to process every noisy frame.

However, limitations remain. The system currently requires pre-existing 3D models and is limited to basic geometric primitives (planes and curves). Future work could extend this to unknown or non-rigid objects. Additionally, the current implementation assumes at least one "grounded" object, like a table, to serve as a global reference. Despite these constraints, GeoFusion represents a significant step toward robust semantic mapping in complex, real-world environments.

Summary updated

Abstract

We propose GeoFusion, a SLAM-based scene estimation method for building an object-level semantic map in dense clutter. In dense clutter, objects are often in close contact and severe occlusions, which brings more false detections and noisy pose estimates from existing perception methods. To solve these problems, our key insight is to consider geometric consistency at the object level within a general SLAM framework. The geometric consistency is defined in two parts: geometric consistency score and geometric relation. The geometric consistency score describes the compatibility between object geometry model and observation point cloud. Meanwhile, it provides a reliable measure to filter out false positives in data association. The geometric relation represents the relationship (e.g. contact) between geometric features (e.g. planes) among objects. The geometric relation makes the graph optimization for poses more robust and accurate. GeoFusion can robustly and efficiently infer the object labels, 6D object poses, and spatial relations from continuous noisy semantic measurements. We quantitatively evaluate our method using observations from a Fetch mobile manipulation robot. Our results demonstrate greater robustness against false estimates than frame-by-frame pose estimation from the state-of-the-art convolutional neural network.
BibTeX
@misc{sui2020geofusion,
  author = {Zhiqiang Sui and Haonan Chang and Ning Xu and Odest Chadwicke Jenkins},
  title = {{GeoFusion: Geometric Consistency informed Scene Estimation in Dense Clutter.}},
  year = {2020},
  eprint = {2003.12610},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2003.12610},
}
Figure