Where Am I? What’s Around Me?

Two of the four questions that organize this book are answered before the planner ever sees a lanelet. Localization asks “where am I?” Perception asks “what is around me, and what will it do next?” Both read the same input: the pointcloud map you built in Part III. Localization matches a live LiDAR scan against it to place the vehicle. Perception subtracts it from each new scan so the moving world stands out.

This chapter traces the map through both paths. It does not describe module internals. It describes what each subsystem extracts from the map, how it uses what it extracts, and why knowing that matters when you are the one building the map.

Localization: the map as ground truth

Localization answers “where am I?” The job is to place the vehicle on the map, in motion, at highway speed, with centimeter-level accuracy. The sensor that makes this possible is LiDAR, and the reference it matches against is the pointcloud map you built in Part III. That map reaches localization through the map loader, which compiles the raw files once and then hands the relevant geometry to each subsystem that asks for it: the scan matcher here, the perception pipeline next door, the planner downstream.

Autoware’s localization pipeline has four stages: a pose estimator, a twist-acceleration estimator, a kinematics fusion filter, and a diagnostics monitor. Only the first stage touches the pointcloud map directly. The rest solve a problem that the first stage cannot avoid.

The pose estimator and why it is not fast enough

The pose estimator compares the vehicle’s current LiDAR scan against the pre-built pointcloud map. The scan does not arrive raw: a sensing step first strips the points reflected from the vehicle’s own body and corrects for the small distortion of the scanner sweeping while the vehicle moves, leaving a clean record of the world rather than of the robot. It searches for the transform (translation plus rotation) that best aligns the live points with the map points. The algorithm is called NDT scan matching,¹ and it produces a pose: an (X, Y, Z) position and a roll, pitch, yaw orientation, each with a covariance that quantifies the uncertainty.

A LiDAR scan arrives every 100 milliseconds. The scan matching computation itself takes additional time, on the order of tens of milliseconds depending on the scan density, the map resolution, and the search radius. By the time the pose estimator finishes computing a pose, the vehicle has moved. At 30 mph, 100 milliseconds is 1.3 meters. At highway speed, it is more. A pose that was accurate when the scan was captured is already stale by the time it is published.

The fusion filter: filling the gap

Two sensors run much faster than LiDAR. The IMU measures linear acceleration and angular velocity at hundreds of hertz. Wheel odometry reports rotation counts on each driven wheel, also at high frequency. Neither one knows where the vehicle is in the world. The IMU drifts. Odometry slips. But together they can track how the vehicle has moved since the last known-good pose.

The twist-acceleration estimator takes the raw IMU and wheel odometry signals and produces velocity, angular velocity, and acceleration estimates with covariances. The kinematics fusion filter takes both inputs, the pose from the LiDAR scan matcher and the twist-acceleration from the internal sensors, and fuses them into a single estimate: the likeliest current pose, velocity, and acceleration, each with a covariance, published at 50 Hz.

Fifty hertz is once every 20 milliseconds. At 30 mph, that is about 27 centimeters of travel between updates. The fusion filter is not more accurate than the scan matcher. It is faster. It keeps the pose alive between LiDAR fixes, dead-reckoning through the 100-millisecond gaps with the IMU and wheel odometry holding the estimate on track.

The result is a pose stream that is both globally accurate (from the LiDAR map match, running slowly) and locally smooth (from the IMU and odometry, running fast). Each corrects the other: the scan matcher eliminates the drift the IMU accumulates, and the IMU bridges the gap the scan matcher leaves open.

What the map must provide

The pose estimator matches a live LiDAR scan against the pointcloud map. For the match to succeed, the pointcloud map must contain enough structure to distinguish one place from another. An open field, a long featureless hallway, a uniform tunnel wall: these are structure-poor environments where scan matching fails because many candidate poses produce equally good alignments. A site like Harbor Yard, dense with containers, loading docks, and parked vehicles, is structure-rich by comparison, but the map must still be current. A container that was moved last week and a container that is still in the map from last month’s scan will pull the scan matcher toward the old pose.

The Lanelet2 map is not directly involved in the scan matching computation. The pointcloud map is the reference. But the Lanelet2 map defines the coordinate frame the planner uses, so the two maps must agree on where the road is. When both are built from the same survey data and projected with the same parameters, they align. This is one more reason the projection step in the loader matters: every point in the Lanelet2 map passes through the same projection as every point in the pointcloud map. If the projection is the same, the maps agree. If it is not, they describe two different versions of the same place, and the planner places the vehicle on a lanelet that does not match the pointcloud the scan matcher used.

The localization diagnostics monitor watches the match score, the covariance, and the convergence time. When the scan matcher reports a low match score or a high covariance, the diagnostics flag it. The vehicle has not lost its pose yet, but the estimate is softening. The planner may reduce speed or request a fallback behavior before the error grows large enough to matter.

Perception: detection, tracking, prediction

Perception answers “what is around me?” Once the vehicle knows where it is, it needs to know what else is out there and what those things are going to do next. The primary sensor for this job is the same one that drives localization: LiDAR. The pointcloud map you built in Part III appears again, this time as the static background that perception subtracts so the moving world stands out.

Autoware’s perception pipeline runs in three stages. Detection finds objects in the current frame. Tracking follows those objects across frames, assigning each one a persistent ID and estimating its velocity. Prediction forecasts where each tracked object is likely to go next, using the vector map to constrain the forecast to the road geometry.

Detection: finding objects in the point cloud

Detection answers a single question: what is in this frame that was not here when the map was made? The primary detector is a LiDAR-based deep neural network (architectures like CenterPoint or TransFusion-L) that takes the current point cloud as input and outputs 3D bounding boxes around vehicles, trucks, pedestrians, bicycles, and other dynamic objects. Typical detection range is 90 to 120 meters. At highway speed, that is about four seconds of look-ahead.

A single detector is not enough for every situation. Near-range detection uses higher-resolution voxel grids to catch small objects within 30 to 50 meters. Radar extends detection range for faraway objects. A clustering-based detector catches unlabeled obstacles that do not fit any trained category: a fallen tree branch, a piece of road debris, a spilled pallet. Each detector publishes its own list of objects. An object merger combines them, resolving duplicates by priority so the primary LiDAR detector’s output takes precedence when it conflicts with a secondary source.

Before detection runs, the point cloud goes through obstacle segmentation: ground removal strips out the road surface, and the pointcloud map is used to subtract known static geometry. What remains is a filtered point cloud containing only the things that are not in the map. This is the input the detectors receive, and it is why perception is sometimes described as a background subtraction problem. The map is the background. Perception finds the foreground.

The same processing produces an occupancy grid: a rolling grid of cells around the vehicle, each marked free, occupied, or unknown. The unknown cells are blind spots, the regions behind a truck or a wall that the sensors cannot reach. The planner reads this grid alongside the Lanelet2 map, and together they answer “where can I go?”: the lanelets say where driving is permitted, the occupancy grid says where it is physically clear.

Tracking: following objects across frames

A LiDAR scan arrives every 100 milliseconds. A vehicle detected in one scan and a vehicle detected in the next scan are the same vehicle. Tracking connects the two.

The multi-object tracker assigns each detection a persistent ID and estimates its velocity by comparing its position across consecutive frames. When a detection is missed in one frame (because the object was occluded, or the detector’s confidence dipped below threshold), the tracker interpolates the gap, maintaining the object’s existence and updating its estimated position from the last known velocity. This interpolation step keeps a pedestrian who briefly disappears behind a parked truck from vanishing from the planner’s awareness and then reappearing as a new, unknown object a half-second later.

The output of tracking is a list of tracked objects, each with an ID, a 3D position, a velocity vector, and a classification. At Harbor Yard, a cargo tug moving down the loop road is not a new detection in every frame. It is tracked object #7, moving at 4 mph, heading toward the loading dock, and the planner knows it has been there for the last three seconds and is still there now.

Prediction: forecasting what happens next

Tracking tells the vehicle where each object is and how fast it is moving. Prediction tells the vehicle where each object is likely to be two seconds from now, four seconds from now, at the point where its path intersects the ego vehicle’s planned route.

Prediction takes the tracked objects and the vector map (the Lanelet2 map) and produces a set of probable future paths for each object, each with a confidence score. The map constrains the forecast. A vehicle in a lanelet is likely to follow the lanelet’s centerline. A pedestrian near a crosswalk is likely to cross. A forklift in a loading zone can move in any direction, and the prediction reflects that uncertainty by producing a wider fan of candidate paths.

Mechanically, the map anchors prediction by supplying the candidate paths themselves. For each tracked object, prediction asks the vector map which lanelets the object could legally occupy next and reads their centerlines as the skeleton of the forecast; the object’s measured velocity and heading then decide which candidates carry the weight. The map does not predict. It supplies the menu of futures, and the tracker’s numbers order from it. That is also its failure mode: a site whose map omits the pedestrian path behind the warehouse produces a pedestrian forecast that walks straight through the container zone, because the correct future was never on the menu.

The map’s role in prediction is to provide the lanes, the crosswalks, the zones that constrain movement. A tracked vehicle on the Harbor Yard loop road does not get a prediction that veers into the container zone, because the vector map says that region is not a driving lane. The prediction is anchored to the same lanelet geometry the planner reads. The planner then uses the predicted paths to decide whether to slow down, stop, or swerve.

Traffic lights: the map points the camera

Everything above runs on LiDAR. Traffic lights run on cameras, and here the map does a job that may surprise you: it tells the detector where to look. Because the map records each signal’s position, the detector narrows its search to the right patch of the image, reads the color, and holds that reading through brief occlusions by trucks or trees. For any site with traffic lights, the signal positions must be encoded in the map; without them, the detector is left searching a whole image for a small circle of red or green.

What the map must provide

For obstacle segmentation to subtract the static world, the pointcloud map must be current. A container that was moved last week but is still in the map becomes a blind spot: the segmentation step erases the real container’s points from the live scan, and the occupancy grid reports no obstacle where one physically exists. The reverse case, a new barrier not yet in the map, is handled correctly: the segmentation step does not subtract it, and the clustering detector flags it as an unknown obstacle. The map ages with the place it describes, and perception is the subsystem that feels the aging first.

The pointcloud map and the Lanelet2 map must agree on where the static world is. Obstacle segmentation subtracts the pointcloud map. Prediction constrains forecasts to the Lanelet2 map. When both maps were built from the same survey and projected with the same parameters, they describe the same dock edge in the same position. The segmentation step subtracts it from the point cloud, and the prediction step uses it to constrain the forklift’s forecast. The two map consumers never communicate directly. They share a coordinate frame, and the consistency of that frame is the consistency of the entire perception pipeline.

Try this

[10 min] Follow a single LiDAR scan through the three-stage pipeline at Harbor Yard.

1. A cargo tug is parked at the dock. A forklift is crossing the loop road toward the container zone. The LiDAR captures a point cloud. Obstacle segmentation subtracts the dock, the containers, the pavement. A cluster of points remains where the forklift is.
2. In detection, the LiDAR DNN classifies the cluster as a vehicle, places a 3D bounding box around it, and publishes it as a detected object. The clustering detector also flags a pile of pallets near the warehouse that the DNN did not recognize and publishes it as an unknown obstacle.
3. In tracking, the forklift is assigned ID #3 and its velocity is estimated from the previous two frames: 6 mph, heading south across the loop. The pallets are assigned ID #12, stationary.
4. In prediction, the vector map constrains the forklift’s forecast to the container zone. The pallets, stationary and unclassified, get a simple prediction: they are not moving. The planner receives tracked object #3 with a predicted path crossing the loop road and object #12 as a stationary obstacle near the warehouse wall.

Pick any object in your Harbor Yard map and ask: would perception detect it, track it, and predict it the way you expect? If the map is missing the pallets, perception flags them as unknown. If the map has an old container that no longer exists, perception subtracts it and creates a blind spot. The map is the background model. You control what the model says is there.

Footnotes

1. Scan matching needs a starting guess, a rough idea of where the vehicle is, so the search has somewhere to begin. At startup that guess comes from GNSS or from a manual pose an operator sets; thereafter the previous frame’s pose is the guess, and GNSS is no longer needed. This is why a site can be hostile to GNSS and still localize precisely: GNSS only ever buys the first fix. ↩