Remove ground points from a depth point cloud for obstacle detection

A depth camera mounted at a bird's-eye angle sees two things: obstacles and a lot of floor. Feed that cloud straight into a costmap and the planner treats the ground as a wall. The usual first fix — clipping the depth range — quietly deletes low obstacles. This guide shows how to remove ground from a point cloud with a height pre-filter plus a RANSAC plane fit, with the numbers that make it stable on a moving robot.

You will finish with a filtered, time-stamped cloud in the right coordinate frame, ready for a costmap obstacle layer, and a checklist to verify it against real objects at known distances. The method works with any 3D sensor that outputs XYZ, from a ToF camera to a laser galvanometer stereo unit.

Why a distance clip cannot remove the ground

Stereo depth cameras have a minimum sensing distance, commonly around 10 cm. Points closer than that break up or never form. When you set a clip distance of 0.5 m, the camera discards everything inside that shell before your software runs. A 15 cm obstacle two metres ahead survives; the same obstacle at half a metre produces only sub-clip points and vanishes.

So the clip removes the ground near the robot at the cost of removing everything short near the robot — exactly the region where a low obstacle matters most for collision avoidance. Ground removal has to happen after the cloud is built, in your own code, not in the camera's range settings.

The pipeline at a glance

Ground removal is three steps on each frame:

1. Transform the cloud into a frame where the floor is roughly perpendicular to one axis (the robot base frame, Z up).
2. Pre-filter to a band of candidate floor points using a height cut.
3. Fit a plane to that band with RANSAC, then split the full cloud by signed distance to the plane.

Step 1 matters most: if you skip it, the camera optical frame (Z forward, Y down) turns a simple height test into an angled-plane test that changes with camera tilt. Transform depth point clouds between optical frame and camera frame covers the rotation itself; for a mobile robot you want to go one step further, into the base_link frame.

Step 1: Get the cloud into the base frame

Subscribe to the XYZ cloud, look up the transform from the cloud's frame to the robot base frame at the cloud's own stamp — not at time zero, and not at "now" — and apply it. Two classic failures live here:

• An extrapolation error means the transform you asked for is outside the buffered interval. Use the stamp on the cloud message and make sure your TF buffer is being fed.
• Obstacles that map closer than reality usually mean the cloud was transformed with a stale or future transform while the robot was moving.

The filtered cloud you publish must carry the base-frame name and the original stamp, so downstream consumers (the costmap) do not re-stamp it with the arrival time.

Step 2: Pre-filter with a height band

Do not run RANSAC on the whole cloud — walls and shelves are planes too, and the biggest planar patch might not be the floor. First keep only points whose height is plausible floor:

If the camera is 0.5 m above the floor and the floor is level, keep points with z between about −0.05 m and +0.10 m. A 15 cm band is enough to absorb mounting error, tyre compression and sensor noise, while excluding table tops and most wall area. This band is also where the height of the camera enters the maths: express your cut in the base frame, where the floor sits near z = 0 regardless of mount height, and the magic constants disappear.

Step 3: Fit the plane and split the cloud

Run RANSAC with a plane model on the band. It returns coefficients (A, B, C, D) such that Ax + By + Cz + D = 0 for floor points. Then classify every point in the full cloud by its signed perpendicular distance:

Pick the threshold t from your floor's roughness: 0.01 m on smooth concrete or tile, up to 0.03 m on textured or carpeted floors. Publish the two clouds separately so you can visually confirm the split before the costmap consumes it.

A worked example shows how the numbers interact. Camera at 0.5 m height, floor flat, so after the base-frame transform the fitted plane is roughly z = 0 and RANSAC returns (A, B, C, D) ≈ (0, 0, 1, 0). A point on a 10 cm curb maps to d = 0.10 m; a floor point with 5 mm of noise maps to d = 0.005 m.

With t = 0.02 m the curb is an obstacle and the noise is ground — with t = 0.15 m, exactly the mistake made when people crank the threshold up to "fix" a wobbly fit, the 10 cm curb is absorbed into the floor and the robot drives over it. If you need t above about 0.05 m to get a stable fit, the problem is the pre-filter band or the frame, not the threshold.

Choosing the sensor

The method is sensor-agnostic, but the camera decides what you can detect. For a downward-tilted mount, check minimum working distance, field of view and depth noise against the smallest obstacle you need to detect. The MV-DT01WMU is a ToF sensing camera.

For stereo or structured-light sensing, check the documented working range and acquisition timing against the intended obstacle field and robot motion. The MV-DLS300P-04 is a laser galvanometer stereo camera.

Browse the full range of 3D sensors if your mounting distance or field of view is not yet fixed; the finder filters by type (laser profiler, stereo, ToF) and range class. If you would rather build a complete station, the vision system configurator walks camera, lens and mount choices in order.

Feeding the costmap without ghost obstacles

The plane split gives you an obstacle cloud; the costmap adds three more failure modes:

1. Add the filtered cloud to the obstacle layer of the costmap, and give the raytrace layer the same topic so cleared cells actually clear. Feeding one but not the other leaves stale obstacles behind.
2. Check the observation_persistence and expected_update_rate settings: a cloud that arrives slower than expected makes the layer discard it, and obstacles flicker.
3. Lay out a box at a measured 2 m and confirm the occupied cell lands at 2 m. If it reads closer, revisit Step 1 — this is the classic symptom of a frame or stamp error, and no amount of threshold tuning fixes it.

When several sensors feed the same map, per-camera timestamps must agree before merging, or each sensor smears its obstacles differently. Multi-camera point cloud synchronization for mobile robots covers clock alignment; merge multiple depth frames into one gap-free point cloud covers the merge itself, including filling holes left by ground removal.

For tracking obstacles across frames rather than re-detecting them each time, a Kalman filter with proper innovation gating stops a latched ground remnant from being tracked as a persistent phantom wall — Kalman filter obstacle detection gating explains the gating rules.

When plane fitting is the wrong answer

• Highly cluttered floors (cables, gravel, pallet debris): the plane fit works, but everything above 2 cm becomes an obstacle and the robot refuses to move. Add a minimum obstacle height (ignore points whose height above the plane is under your traversal limit, e.g. 3 cm) and a minimum cluster size.
• Ramps and sloped floors: one plane per frame misclassifies the far end of a ramp. Bin the cloud into forward 1 m slices and fit one plane per bin, accepting a point as ground if the nearest bin's plane claims it.
• Vibration or a flexing mount: the pre-filter band wanders frame to frame. Widen the band and let RANSAC refit, rather than hard-coding the floor height.

Verification checklist

• Cloud transformed to base frame using the cloud's own stamp — no extrapolation errors at startup.
• Plane fitted only on the pre-filtered band; full cloud classified separately.
• Distance threshold swept from 5 mm to 30 mm; lowest stable value chosen, not the highest.
• A 10 cm test object at 0.5 m and at 2 m both appear in the obstacle cloud.
• Floor-only scene publishes an obstacle cloud that is empty (or below the cluster minimum).
• Box at measured 2 m maps to a cell 2 m from the robot in the costmap.
• Obstacle and raytrace layers subscribe to the same filtered topic.

Once the split is clean, the ground cloud itself is useful: it is your traversability map. Occupied cells come from the obstacle topic; free space can be cleared along rays through the ground cloud, giving the planner a floor it trusts instead of unknown space behind the robot.

If you are still choosing hardware for the job, tell us the mounting height, detection distance and minimum obstacle size you need, and we will size the sensor: the contact page reaches the applications team directly.