SuMa++: Efficient LiDAR-based Semantic SLAM

In plain words

A car with a spinning laser scanner builds a map of the road while it drives, by lining each new scan up with the map built so far. On a busy highway, the cars next to the car move, and the scanner sees them as part of the scenery. The map then smears, and the car's estimate of its own position drifts.

SuMa++ adds a neural network that labels every laser point as car, road, person or vegetation, among other classes. Points on things that move are treated with suspicion: when a new scan disagrees with the map about what a surface is, the map part loses trust and is eventually dropped. The same labels also make the alignment step less willing to be pulled by moving objects. The idea is simple: know what things are, and do not let moving things vote on where you are.

TL;DR

• SuMa++ extends SuMa, a surfel-based LiDAR mapping pipeline, with semantic labels from RangeNet++ §I §III-C.
• A flood-fill refines the label image; surfels whose label conflicts with a movable class are penalised in a stability log-odds update §III-D §III-E eq. 1.
• The labels also weight the ICP residuals, so moving objects count less in pose estimation §III-F eq. 6.
• Author-reported gains are on KITTI only; the largest road-sequence gain is on sequence 35 §IV-A tab. I. On odometry training sequences, average error is between LOAM and IMLS-SLAM §IV-B tab. II.

Problem

Most SLAM systems assume the environment is mostly static. Moving objects can produce wrong associations between the new observation and the map §III-E. Geometric outlier rejection, such as the Huber norm, cannot tell moving from static structure and often down-weights useful parts §IV-C. On KITTI highway sequences with few static structures and many moving cars, a purely geometric surfel approach shows large map inconsistencies and drift §IV-A fig. 6.

Naively deleting every surfel of a movable class is not a fix. Parked cars are useful features for scan registration, and wrong labels delete static surfels. On urban sequences this variant (SuMa_nomovable) diverges §IV-B tab. II.

Assumptions: LiDAR only, no camera §V; a semantic segmenter trained on labelled KITTI data is available §IV. The method is evaluated on KITTI only §IV.

Method

SuMa++ keeps the SuMa pipeline and inserts semantics at three places fig. 2: (1) the raw label image comes from RangeNet++ on the scan's spherical projection; (2) the label image is refined with an erosion and a depth-based flood-fill; (3) during map update, a semantic consistency check against the map is used to detect and remove dynamic surfels; (4) during ICP, semantic compatibility is folded into the residual weights §III fig. 2.

Notation and map. Transformations are written \mathbf{T}_{BA} and each scan frame is C_t with pose \mathbf{T}_{WC_t} §III-A. The map consists of surfels, each with position, normal and radius, a creation and an update timestamp, and a stability log-odds l_s maintained with a binary Bayes filter §III-B. SuMa also performs loop closure and pose-graph optimisation; this paper does not change that part §III-B.

Semantic labels. RangeNet++ predicts a label for each point of the spherical range image. Each surfel stores its inferred label y and that label's probability §III-C.

Refined label image (Algorithm 1). Labels at object boundaries are unreliable, so the raw mask is first eroded: a pixel is removed if a neighbour in its kernel carries a different label §III-D alg. 1. Empty pixels are then filled from a labelled neighbour whose 3-D distance is consistent with the pixel's own §III-D alg. 1 fig. 3.

Dynamic-object filtering. When a scan is integrated, the new label is compared with the surfel's label. If they disagree and the surfel's class is movable (car, bicycle, bus, motorcycle, truck, other vehicle, person, bicyclist, motorcyclist), the surfel receives a penalty in its stability update. The penalty is applied as a term in the log-odds update §III-E eq. 1 §IV-C. Surfels of static classes such as vegetation are not penalised §IV-C.

Semantic ICP. Frame-to-model ICP uses Gauss-Newton on a point-to-plane objective. Each residual is weighted by a Huber weight and by a semantic compatibility term; the paper also writes an indicator for surfel stability §III-F eq. 2 eq. 5 eq. 6 eq. 7 eq. 8.

Pseudocode for one scan (the paper gives the steps in prose and in Algorithm 1; the structure below is ours):

# One scan of semantic surfel mapping  [§III] [fig. 2]
# Input:  scan points, raw label image (H, W), surfel map, previous pose
# Output: pose of the scan, updated surfel map

def process_scan(scan, raw_labels, surfel_map, pose_prev):
    # 1. Spherical projection to vertex map and normal map  [§III-B]
    vertices, normals = project_and_normals(scan)
    # 2. Erode label boundaries in the raw label image  [§III-D] [alg. 1]
    eroded = erode_labels(raw_labels, kernel_size)
    # 3. Fill eroded pixels from neighbours with consistent distance  [alg. 1]
    data_labels = flood_fill(eroded, vertices, theta)
    # 4. Frame-to-model ICP with semantic residual weights  [§III-F] [eq. 5]
    pose = semantic_icp(vertices, data_labels, surfel_map, pose_prev)
    # 5. Integrate the scan and update the stability of each surfel  [§III-E]
    surfel_map.integrate(scan, data_labels, pose)
    return pose

# Semantic ICP, one Gauss-Newton step  [§III-F] [eq. 2] [eq. 5] [eq. 6]
# Shapes: points (N, 3); residuals (N,); Jacobian (N, 6)

def gauss_newton_step(pose, points, model, data_labels):
    # Projective association of each point to the model  [eq. 3] [eq. 4]
    model_vertex, model_normal = associate(pose, points, model)
    # Point-to-plane residual, linear in the offset  [eq. 2]
    residual = dot(model_normal, pose * points - model_vertex)
    # Huber weight times semantic compatibility times stability  [eq. 6]
    weight = huber(residual) * semantic_weight(data_labels, model)
    # Solve the weighted normal equations for the increment  [eq. 5]
    jacobian = point_to_plane_jacobian(pose, points, model_normal)
    delta = solve(jacobian.T @ W @ jacobian, jacobian.T @ W @ residual)
    return apply_increment(pose, delta)

# Stability log-odds update of one surfel  [§III-E] [eq. 1]
# odds(p) = log(p / (1 - p))  [§III-E]

def update_stability(log_odds, angle, distance, labels_conflict):
    # Compatibility of the new measurement with the surfel  [§III-E]
    p_compatible = (
        p_stable
        * exp(-angle**2 / sigma_angle**2)
        * exp(-distance**2 / sigma_distance**2)
    )
    log_odds = log_odds + odds(p_compatible) - odds(p_prior)
    # Penalty only for a movable surfel whose label conflicts  [§III-E] [§IV-C]
    if labels_conflict:
        # p_penalty is not stated in the paper; see Paper vs code
        log_odds = log_odds - odds(p_penalty)
    return log_odds

Key equations, as written in the paper:

l^{(t)}_s = l^{(t-1)}_s + \mathrm{odds}\left(p_{\mathrm{stable}} \exp\left(-\frac{\alpha^2}{\sigma_{\alpha}^2}\right)\exp\left(-\frac{d^2}{\sigma_{d}^2}\right)\right) - \mathrm{odds}(p_{\mathrm{prior}}) - \mathrm{odds}(p_{\mathrm{penalty}}) eq. 1

E(\mathcal{V}_D, \mathcal{V}_M, \mathcal{N}_M) = \sum_{u \in \mathcal{V}_D} w_u \, r_u, \qquad r_u = \mathbf{n}_u^\top \left(\mathbf{T}^{(k)}_{C_{t-1}C_t}\, u - \mathbf{v}_u\right)^2 eq. 2

\mathbf{v}_u = \mathcal{V}_M\!\left(\Pi\left(\mathbf{T}^{(k)}_{C_{t-1}C_t}\, u\right)\right), \qquad \mathbf{n}_u = \mathcal{N}_M\!\left(\Pi\left(\mathbf{T}^{(k)}_{C_{t-1}C_t}\, u\right)\right) eq. 3 eq. 4

\delta = \left(\mathbf{J}^\top \mathbf{W} \mathbf{J}\right)^{-1} \mathbf{J}^\top \mathbf{W} \mathbf{r} eq. 5

w_u^{(k)} = \rho_{\text{Huber}}\left(r_u^{(k)}\right) \, C_{\text{semantic}}\left(\mathcal{S}_D(u), \mathcal{S}_M(u)\right) \, \mathbb{I}\left\{ l_s^{(k)} \ge l_{\text{stable}} \right\} eq. 6

\rho_{\text{Huber}}(r) = \begin{cases} 1 & \text{if } |r| < \delta \\ \delta\,|r|^{-1} & \text{otherwise} \end{cases} eq. 7

C_{\text{semantic}} = \begin{cases} P(y_u \mid u) & \text{if } y_u = y_{v_u} \\ 1 - P(y_u \mid u) & \text{otherwise} \end{cases} eq. 8

Hyperparameters

The paper states few numeric values; most of the table comes from the official configuration and the shaders. These are code defaults for the KITTI setting, not values the paper reports as tuned.

Results

Author-reported. Setup: KITTI raw "road" sequences 30 to 41 (renamed from raw drives; not part of the odometry benchmark, and the paper says the network was not trained on labels for them) §IV-A tab. I; KITTI odometry training sequences 00 to 10 §IV-B tab. II; and the KITTI test set via the server §IV-B. Hardware: Intel Xeon W-2123 (8 cores, 3.6 GHz, 16 GB RAM) and Nvidia Quadro P4000 (8 GB) §IV.

Runtime, author-reported: RangeNet++ about 75 ms per scan, surfel mapping about 48 ms per scan, and up to 190 ms when loop closures are integrated §IV.

Paper vs code

Repository: https://github.com/PRBonn/semantic_suma (MIT), linked from the paper §I, cloned at commit 531954dd602ef1803707a29b9f4a5eb50cdb07de. Shader citations are to GLSL sources in src/shader/; the code is an OpenGL pipeline, so the pseudocode above describes its logic, not its implementation.

Matches: the movable-class list §IV matches the code's list; the Huber form eq. 7 matches with δ = 0.5; the stability odds form eq. 1 is used with both sigma terms, which is confidence_mode 3 default.xml.

Differences:

• Penalty term. Paper subtracts odds(p_penalty), with p_penalty not given eq. 1. Code subtracts a constant 1.0 in log-odds, and only when the model label is a movable class and differs from the data label [code: src/shader/update_surfels.vert#L187-L195] update_surfels.vert.
• Angle term. Paper's α is the angle between normals §III-E. Code uses the length of the cross product of the two normals, which is sin α update_surfels.vert, and the exponential is taken of that value update_surfels.vert.
• Clamp. Code clamps the compatibility probability to [p_unstable, 1] update_surfels.vert. The paper does not mention a clamp.
• Measurement absent or mismatched. When no compatible measurement exists, code adds log_prior to the update, which leaves the log-odds unchanged. When the pixel's closest visible surfel does not match, it adds the log-odds of p_unstable update_surfels.vert update_surfels.vert. Neither case is described in the paper.
• Stability validity. Code caps log-odds at 20.0 update_surfels.vert and invalidates a surfel once its log-odds fall below log(p_unstable/(1 − p_unstable)) update_surfels.vert; a surfel below confidence threshold is also invalid once older than unstable_age update_surfels.vert. The paper's removal rule is "after several observations" §III-E and its indicator uses l_stable eq. 6, which is not defined in code or paper.
• Semantic ICP scope. Code applies the semantic term only when the model label is a movable class Frame2Model_jacobians.geom. The paper's C_semantic eq. 8 is written for all residuals.
• Residual form. Paper's eq. 2 squares the residual inside r_u. Code uses the linear point-to-plane residual in the normal equations Frame2Model_jacobians.geom Frame2Model_jacobians.geom.
• Erosion neighbours. Paper removes a pixel if any neighbour in the kernel has a different label. Code ignores neighbours with label 0 gen_normalmap.frag, and uses only offset 1 gen_normalmap.frag.
• Flood-fill criterion. Paper's text uses 3-D point distances; Algorithm 1 writes a pixel-distance test ‖u − u_n‖ < θ‖u‖ alg. 1. Code uses a relative range test, |‖p‖ − ‖u‖| < 0.007‖p‖ floodfill.frag, and weights the filled probability by 1/(offset + 1) floodfill.frag.
• cutoff_threshold. The config sets it to 10.0 default.xml. It is declared in the ICP geometry shader Frame2Model_jacobians.geom but not referenced there; I did not find where it is used.

Pitfalls

• Unstated values. The paper gives none of p_stable, p_prior, p_penalty, σ_α, σ_d, θ, the kernel size d, δ, l_stable, the projection size or the ICP iteration count. The table above lists the code defaults. Treat them as defaults for KITTI, not as the paper's tuning.
• Eq. 2 squares the residual. Use the linear point-to-plane residual in the Gauss-Newton system eq. 5, as the code does. Squaring it in the solve gives a different objective.
• α in eq. 1 is an angle, but code uses sin α. Using the angle directly changes the exponential's shape near 45°, where the map gate is set.
• Flood-fill text and algorithm disagree on the consistency test (3-D distance vs pixel distance). The code uses a relative range test. Pick one and test it on your projection geometry.
• Odometry Table II is in-sample. RangeNet++ was trained on KITTI sequences 00–10 except 08, which the paper uses for validation §IV. Table II reports the training sequences, so only the server-side test result is held out §IV-B.
• Test-set rotation error. The paper gives 0.0032 deg/m for both SuMa and SuMa++ §IV-B. The tables use deg per 100 m, so the unit differs from them. Check the unit before comparing.
• Costs the abstract hides. Timings were measured with the segmenter on the paper's Quadro P4000, about 75 ms per scan §IV. The approach also needs a labelled training set and a trained segmenter.
• Dynamics in the first scan. The method cannot filter moving objects in the first observation, and the paper's fix is to remove all potentially movable classes during initialisation §IV-C. A larger number of moving objects in the first scan breaks the initial velocity estimate §IV-C.
• Static classes stay in the map. Vegetation is not penalised, which can leave changing foliage in the map §IV-C.
• Table numbering. The class is IEEE conference style, so tables are cited I and II in order of appearance. This is inferred from the source, not from a rendered PDF.

Lineage

• Builds on: SuMa, surfel-based LiDAR mapping with loop closure, by Behley and Stachniss (RSS 2018) §III-B. No arXiv id is given in the paper.
• Builds on: RangeNet++, a fully convolutional segmenter for range images, by Milioto et al. (IROS 2019), which uses a SqueezeSeg-style architecture with a DarkNet53 backbone §III-C. No arXiv id is given in the paper.
• Labels: SemanticKITTI, by Behley et al. (ICCV 2019) §IV.
• Compared against: SuMa and SuMa_nomovable §IV, IMLS-SLAM tab. II and LOAM tab. II.
• Superseded by: not stated in the paper or in the content of this wiki.