Feature-Based Sensor Models

The beam model and the likelihood field explain a complete range scan one beam at a time. A feature-based model uses much less data. A feature extractor reduces each scan or image to a short list of features, one for each detected landmark. The measurement model then explains those features. Section 6.6 of Probabilistic Robotics (Thrun, Burgard and Fox) gives the main reason for this approach: inference over a few features can be orders of magnitude cheaper than inference over the full measurement.

​

Range, bearing and signature

A landmark is a distinct object that the robot uses for navigation, such as a pole, a door post or a tree trunk. In the most common landmark model, the sensor measures the landmark’s range $r$ and bearing $\phi$ in the robot’s frame. The extractor may also report a signature. This describes how the landmark looks, such as its color, size or type. Thrun, Burgard and Fox write the signature as $s$. On this site $s_t$ is the robot’s state, so the signature is written as $\kappa$ here. The features extracted from a measurement $z_t$ form a set of triples: $$f(z_t) = \{f^1_t, f^2_t, \dots\}, \qquad f^i_t = (r^i_t, \phi^i_t, \kappa^i_t).$$ The number of features can change at each step. The model treats the noise in each feature as independent of the noise in every other feature. The likelihood therefore factors over the features. A filter can process them one at a time: $$p(f(z_t) \mid s_t, m) = \prod_i p(r^i_t, \phi^i_t, \kappa^i_t \mid s_t, m).$$ The map contains a list of landmarks $m = \{m_1, \dots, m_N\}$. Landmark $j$ has position $(m_{j,x}, m_{j,y})$ in the map frame and signature $\kappa_j$. Suppose feature $i$ at time $t$ comes from landmark $j$, while the robot is at $s_t = (x, y, \theta)$. The feature equals the noise-free geometry plus independent Gaussian noise: $$\begin{bmatrix} r^i_t \\ \phi^i_t \\ \kappa^i_t \end{bmatrix} = \begin{bmatrix} \sqrt{(m_{j,x} - x)^2 + (m_{j,y} - y)^2} \\ \operatorname{atan2}(m_{j,y} - y,\; m_{j,x} - x) - \theta \\ \kappa_j \end{bmatrix} + \begin{bmatrix} \varepsilon_r \\ \varepsilon_\phi \\ \varepsilon_\kappa \end{bmatrix},$$ where $\varepsilon_r$, $\varepsilon_\phi$ and $\varepsilon_\kappa$ have zero mean and variances $\sigma_r^2$, $\sigma_\phi^2$ and $\sigma_\kappa^2$.

​

Known correspondence

The model must know which landmark produced each feature. The correspondence variable $c^i_t = j$ means that feature $i$ at time $t$ came from landmark $j$. Thrun, Burgard and Fox also allow $c^i_t = N + 1$. This value represents a feature that matches no landmark in the map, such as a spurious detection or a landmark that the map does not yet contain. When the correspondence is known, the likelihood of a feature is the product of three Gaussian densities. Each density describes one error (Table 6.4 of Probabilistic Robotics): $$p(f^i_t \mid c^i_t = j, s_t, m) = \mathcal{N}(r^i_t - \hat r;\, 0, \sigma_r^2)\; \mathcal{N}(\phi^i_t - \hat\phi;\, 0, \sigma_\phi^2)\; \mathcal{N}(\kappa^i_t - \kappa_j;\, 0, \sigma_\kappa^2),$$ where $\hat r$ and $\hat\phi$ are the range and bearing that landmark $j$ would produce from pose $s_t$. The bearing error is wrapped to $(-\pi, \pi]$. The signature helps determine the correspondence. If the correspondence is unknown, the signature provides evidence about it. A feature with a signature far from $\kappa_j$ is unlikely to come from landmark $j$. A unique ID determines the correspondence directly. Without a signature, every landmark looks the same, which makes correspondence estimation harder. You can study that problem in correspondence estimation.

​

Which sensor supplies what

Sensors provide the parts of a feature in different ways: A 2D LiDAR such as the TurtleBot3’s 360° laser distance sensor returns one distance for each degree of bearing (LDS-01, LDS-02). The beam model explains this raw scan. A feature extractor groups neighboring beams that strike the same object, such as a pole or a table leg. It then reports one range and one bearing for that object. Beyond 0.5 m, the LDS-01 has an accuracy of ±5%, which is about 6 cm at 1.25 m. A LiDAR feature has no useful signature because one pole looks like every other pole. A camera gets the bearing from where the landmark appears in the image, specifically from its horizontal pixel coordinate $u$: the column of the pixel, counted from the left edge. To turn that column into a direction, you need two of the camera’s intrinsics: the focal length $f_x$ in pixels and the column $c_x$ of the image center. For a camera that faces straight ahead, the bearing is $$\phi = \arctan\!\left(\frac{c_x - u}{f_x}\right).$$ A landmark left of the image center ($u < c_x$) has a positive bearing, measured to the left as in the robot’s frame. The row $v$ gives the elevation instead, which a planar landmark model does not use. The pinhole model defines them, and calibration estimates them. An RGB-D camera such as the Intel RealSense D435i also measures depth at each pixel. This depth gives the range. The camera uses active stereo, so its range error grows with the square of the distance. Its depth field of view is 87° wide, and it works from about 0.3 to 3 m (Intel). It therefore sees fewer landmarks at once than a 360° LiDAR. Its IMU supports the motion side of a filter. It does not affect the measurement model. A camera can also provide a signature through the landmark’s appearance in the color image or the ID of a fiducial marker, such as an AprilTag attached to it. An ID names the landmark directly. The model stays the same across sensors. The noise variances can change. The set of visible landmarks also changes according to range and, for a camera, field of view. The signature may or may not determine the correspondence. EKF localization uses this model with known correspondences. Correspondence estimation handles features whose signatures do not identify the landmark.

---