P5-T3 — Ground and Plane Estimation

Learning objectives

After this lesson, you should be able to:

• Write a plane as n · p + d = 0 and compute the distance of a point to it and the plane through three points.
• Fit a plane by least squares and by PCA, and say why neither is robust to obstacles.
• Apply RANSAC to plane fitting and compute the iteration count for a given inlier ratio.
• Explain the failure of height thresholds on slopes, the need for a range-dependent tolerance, and when one plane is not enough.

The idea: the ground is the biggest plane

Ask what most of the points in front of a wheeled robot belong to. Usually the ground. Obstacles are a minority of the points, and they do not lie in a common plane. That makes the ground the dominant plane in the cloud, and a method that finds the dominant plane robustly finds the ground without being told its height or tilt.

Why not simply threshold the height? Because the height of the ground is not constant. Slopes, kerbs and the pitching of the robot under acceleration all move it, and the error grows with range (range × tan of the slope). The estimated plane carries the tilt and the height with it.

Fit the surface, then measure height above the fitted surface. An obstacle is something that stands out of the ground, wherever the ground is.

The mathematics: planes, fits and RANSAC

The plane

A plane is the set of points p that satisfy

n

the unit normal (nx, ny, nz); for the ground it points up.

d

the offset: −n · p₀ for any point p₀ on the plane. For a level ground at z = −h, n = (0, 0, 1) and d = h.

dist(q)

the signed distance of q from the plane in metres: positive on the side the normal points to. The height above the ground.

Three points that are not collinear define a plane: n is the normalised cross product (p₂ − p₁) × (p₃ − p₁), flipped to point up, and d = −n · p₁. A slope of angle α along x gives n = (−sin α, 0, cos α).

Fitting many points

PCA (total least squares): the plane through the centroid p̄ whose normal is the eigenvector of the smallest eigenvalue of the covariance (Lesson P5-T2). It minimises the sum of squared perpendicular distances and treats all directions equally.

Ordinary least squares on z = a x + b y + c: minimises vertical errors; simple (a linear system) and good when the ground is not near-vertical, but biased for steep planes.

Both weigh every point equally, so obstacles (a minority that sits above the ground) pull the plane up and tilt it. They fit the ground well only after the obstacles have been removed, which needs the ground… so a robust method is required first.

RANSAC for planes

Draw three points at random, build their plane, count the points within a threshold t of it, and repeat; keep the plane with most inliers, then refit by PCA to all its inliers (Lesson P4-T4 and Lab P5-L2). The minimal sample is s = 3, so the iterations needed are

With a ground fraction of w = 0.7 and p = 0.99: N = 11. Even with w = 0.4 only 70 are needed. Plane fitting is cheap for RANSAC because the minimal sample is small and the ground usually holds most of the points.

Tolerance that grows with range

A range sensor’s error grows with distance, so the set of ground points within a fixed band is thinner far away. A practical rule lets the tolerance grow linearly:

When one plane is not enough

• Changing slope: a road that crests or dips is not a plane. Fit a plane per sector (angular slices) or per range band, or fit a surface on a grid, as ground-segmentation methods do.
• Kerbs, steps, ditches: small height jumps that are the boundary of the drivable surface, not obstacles to remove blindly.
• Vegetation and rough terrain: no clean plane; the ground has to be estimated as a surface that the lowest points support, not as a plane.
• A large wall or flat roof: a plane with more points than the ground. Constrain the normal to be nearly vertical and the plane to be near the expected height.

A worked example: a plane from three points, and the cost of assuming flat

Three points on a rising road, in the sensor frame (z up, sensor at the origin): p₁ = (2, 0, -1.1), p₂ = (6, 0.2, -0.72), p₃ = (5, -2, -0.8).

The road rises at about 5.5°: the normal tilts back from the vertical by that angle. The height of a point q above this ground is n · q + d. For a point at (10, 0, −0.1): 0.24 m above the ground; if instead you assume a flat ground at z = −1.2, the same point is 1.10 m above ground. A true obstacle point only 0.24 m above the road would be classified as a 1.1 m obstacle.

Cost of the flat assumption

A slope of 6° changes the ground height by 0.1051 m per metre of range: 1.05 m at 10 m and 2.10 m at 20 m. A threshold margin of 0.3 m lets the flat assumption work only to a range of 2.9 m on this slope. Beyond that, every ground point is above the threshold and appears to be an obstacle. The failure is not noise: it is systematic, grows with range and is entirely avoided by fitting a plane.

Iterations and the effect of obstacles

Suppose a cloud of 1 000 points has 700 ground points. A random triple is all ground with probability 0.7³ = 0.343, so about 11 samples give a clean one with 99% confidence. The best plane’s inlier count then identifies ground. An ordinary least-squares fit to all 1 000 points would instead tilt towards the 300 obstacle points: if they sit on average 0.8 m above the ground, the fitted plane rises by roughly 0.3 × 0.8 = 0.24 m on average and tilts, which is a larger error than the noise and is entirely due to the obstacles.

Engineering implications and limitations

Constrain the plane. The ground normal is nearly vertical (within, say, 15° of the expected up direction from the IMU) and the plane is near the expected height. A large wall can win the RANSAC vote otherwise.

Use the IMU. The pitch and roll of the vehicle change the apparent ground plane from frame to frame. Rotating the cloud into a gravity-aligned frame first removes most of that and leaves only real terrain slope for the estimate.

Refit and re-test. After RANSAC, refit by PCA to all inliers and re-evaluate with the range-dependent tolerance. This is the cheap step that improves accuracy and reduces false obstacles at long range.

Handle non-planar ground. Use sectors or a grid of local planes, or a ground model such as a Gaussian process or a lowest-point surface, and check the boundaries between pieces. Keep the previous frame’s estimate as a prior for the next.

Do not discard what stands on the ground. Points close to the plane include the bottoms of obstacles, kerbs and low objects. A removal tolerance that is too generous removes real small obstacles; test with objects of the smallest height that matters (for example 15 cm).

Monitor the estimate. A sudden change in the fitted normal, an unusually small inlier fraction or a plane far from the expected height means something is wrong (a wall in front, a failed scan). Fall back to the previous plane and slow down.

Common misconceptions

Engineering takeaways

1. A plane is n · p + d = 0 with ‖n‖ = 1; the signed distance of q is n · q + d; three non-collinear points define a plane.
2. PCA and least squares fit planes but are pulled by obstacles; RANSAC with three-point samples finds the dominant plane, followed by a PCA refit on its inliers.
3. N ≥ ln(1 − p)/ln(1 − w³): 11 iterations for w = 0.7 and p = 0.99.
4. A slope of α moves the ground by range × tan α: 1.05 m at 10 m for 6°. Height thresholds on flat ground fail systematically with range.
5. Use a range-dependent tolerance t0 + k·range, constrain the plane, use the IMU, and use sectors or grids when the ground is not a single plane.

References and further reading

Standard references; the plane through three points and the slope numbers were computed for this lesson.

• Fischler, M. A. and Bolles, R. C. (1981). Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography. Communications of the ACM 24(6).
• Himmelsbach, M., Hundelshausen, F. v. and Wuensche, H.-J. (2010). Fast segmentation of 3D point clouds for ground vehicles. IEEE Intelligent Vehicles Symposium: a ground model made of local line fits per angular sector.
• Zermas, D., Izzat, I. and Papanikolopoulos, N. (2017). Fast segmentation of 3D point clouds: a paradigm on LiDAR data for autonomous vehicle applications. ICRA: ground plane fitting and scan-line segmentation.
• Rusu, R. B. and Cousins, S. (2011). 3D is here: Point Cloud Library (PCL). ICRA: plane segmentation with RANSAC.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Next: grouping what remains after the ground is removed into separate objects.

Related content

Nothing below is required to finish this lesson.

• Before
  P5-T2: Neighborhoods and Geometric Features

  Compute normals and planarity from a local neighbourhood.

• Lab
  P5-L2: Fit a Ground Plane Robustly

  Estimate a sloped ground plane with RANSAC where a height threshold fails.