Path Planning — Demo
A* on Stereo Depth Occupancy Grid
A* global path search running directly on a 3D occupancy grid built from the SGBM stereo depth map. The pipeline goes from raw stereo frames to a driveable ground plane and obstacle voxels in a single pass, then runs A* to find the shortest collision-free route — all on the Raspberry Pi 5 CPU.
Frame 10 — Stereo Vision 3D Reconstruction. SGBM computation: 0.089s
Pipeline
From Stereo Frames to Path
Stereo Rectification
Left and right frames are rectified using the Kalibr-calibrated intrinsics and extrinsics, aligning epipolar lines horizontally for accurate disparity computation.
SGBM Disparity + WLS Filter
Semi-Global Block Matching produces a raw disparity map. A Weighted Least Squares filter smooths edges and fills holes, giving the dense colored disparity map shown above.
3D Point Cloud
Each disparity pixel is back-projected into 3D space using the stereo baseline and focal length. Two viewing angles of the resulting point cloud are shown in the top and bottom left panels.
Occupancy Grid
The point cloud is voxelized into a 3D occupancy grid colored by depth. Occupied voxels represent obstacles; free voxels define the traversable space fed to the planner.
Ground Plane Extraction
RANSAC fits a ground plane to the bottom portion of the point cloud. Points within a height threshold are classified as driveable (green shaded region, 2 m max depth).
A* Path Search
A* runs on the 2D projection of the occupancy grid, using Euclidean distance as the heuristic. It finds the shortest collision-free path from the robot's current position to the goal.