Cube Pose Estimation

This project implements a 6D pose estimation pipeline for cubic objects using RGB-D data within a ROS2 Humble environment. The system processes depth images into point clouds, segments objects from the table plane, and estimates their position and orientation using a multi-stage geometric approach refined by ICP (Iterative Closest Point).

🔥 Features

• RGB-D to Point Cloud Conversion: Projects depth images into 3D space using camera intrinsics.
• Plane Segmentation: Utilizes RANSAC to separate the table plane from object clusters.
• Robust Pose Estimation: Implements three progressive algorithms to handle occlusion and noise:
  • Naive 3D Bounding Box: Direct minimal oriented bounding box estimation
  • 2D Projection & Extrusion: Projects points to the table plane to solve rotation constraints, then extrudes height.
  • ICP Refinement (Best Performance): Uses a known geometric model ($4\times4\times4$ cm cube) and aligns it to the point cloud using ICP to correct dimensional distortions caused by camera calibration mismatches.
• Visualization: Real-time marker visualization in RViz2.
• Dockerized Environment: Fully reproducible setup .

⚠️ Prerequisites

• OS: Linux (Ubuntu 22.04 recommended)
• GPU: NVIDIA GPU (For CUDA acceleration)
• Drivers: NVIDIA Driver supporting CUDA 12.9 (Driver version 570+)
• Software: Docker & Docker Compose

⚡ Quick Start

Only the ICP refinement method is provided in this repository.

1. Build the docker image and run the container

export USER_ID=$(id -u)
cd docker
docker compose up -d --build

2. Attach to the running container

docker exec -it cube-pose-estimation-ws bash

3. Build the workspace

cd ~/CubePoseEstimation
colcon build --symlink-install

Note: After building the workspace, exit the terminal and re-enter the container to source the workspace automatically.

4. Launch the nodes

# Launch the rgbd_image_publisher, depth_to_pointcloud, and RViz
ros2 launch object_pose_estimation run.launch.py

# Launch the object_pose_estimation
ros2 launch object_pose_estimation object_pose_estimation.launch.py

You should see the estimated cube poses visualized in RViz2.

📝 Methodologies

The estimation process is divided into three versions, evolved to solve specific challenges found during development:

Version 1: Naive 3D Bounding Box

Directly calculates the minimal oriented bounding box on object clusters.

Limitation: Due to single-view occlusion, boxes were often "floating" or not orthogonal to the table.

Version 2: 2D Projection

Enforces a hard constraint that the object's z-axis must align with the table's normal vector.

1. Project object point cloud to the 2D plane defined by the table.
2. Compute cv2.minAreaRect to find the yaw (rotation).
3. Project back to 3D.

Limitation: Solved orientation issues, but bounding box dimensions were incorrect due to point cloud noise and camera calibration errors.

Version 3: ICP Refinement (Final Solution)

Assumes known object dimensions ($4cm^3$) to fix the scaling error.

1. Use the pose from Version 2 as an initial guess.
2. Apply Iterative Closest Point (ICP) to align a generated ideal cube model with the observed point cloud.

Result: Significantly improved orientation accuracy.

📊 Results

Below are the visualization results from RViz. The pipeline successfully detects multiple cubes and estimates their 6D pose despite occlusion.

Input RGB Image

Input Depth Image

ICP Refinement Result - 1	ICP Refinement Result - 2
ICP Refinement Result - 3	Top-Down View

The system outputs the estimated coordinates and orientation (roll/pitch/yaw) for each detected cube.

🔎 Known Issues

• Border Occlusion: Objects located at the very edge of the image frame may still yield unstable pose estimations.