Range-Based Feature Extraction

Feature extraction for range sensor can be very useful. This usually deals with geometric primitives such as lines, circles, etc. We will look at line extraction techniques as these are the simplest features to extract and provide a basis for more complex feature extraction techniques.

For line extraction, we need to answer:

1. How many lines are present in the scan?
2. Which points belong to which line?
3. Given these points, what is the line equation?

Least Squares Line Fit

First, we convert range data (such as from LiDAR) from polar coordinates to Cartesian:

$$({\rho }_i,{\theta }_{i)}⟹(x_i,y_i)=({\rho }_i\cos {\theta }_i,{\rho }_i\sin {\theta }_i)$$

We want to fit a line:

$$x_i\cos \alpha +y_i\sin \alpha -r=0$$

which is equivalent to slope-intercept $y=mx+b$.

We can expand this to write:

$$\begin{array}{rl}{\rho }_i\cos {\theta }_i\cos \alpha +{\rho }_i\sin {\theta }_i\sin \alpha -r& =0\\ {\rho }_i\cos ({\theta }_i-\alpha )-r& =0\end{array}$$

Taking the orthogonal distance $d_i$ between the measured point and the line, we have:

$${\rho }_i\cos ({\theta }_i-\alpha )-r=d_i$$

To find the best line, we minimize the total squared error:

$$S=\sum _i{d}_i^2=\sum _i({\rho }_i\cos ({\theta }_i-\alpha )-r{)}^2$$

which can be solved by the roots of $\frac{\partial \sqrt{y}}{\partial \alpha }=0,\frac{\partial S}{\partial r}=0$.

Unfortunately, this least squares method is not robust to uncertain data (i.e. outliers).

A more typical approach is to use RANSAC, which is more robust to noise.