Explainers

How is Midline Generated?

Our midline calculation relies on Support Vector Machines (SVMs). Using the cones received from our lidar, the SVM generates a decision boundary using a cubic polynomial kernel. This decision boundary is converted into a series of points that represents the path the car should take.

Why iSAM2?

We chose this algorithm after considering 1. iSAM2 is capable of incrementally optimizing its estimates for previous cone position estimates and pose estimates, as opposed to optimizing only at loop closure, 2. iSAM2’s performance made it a clear choice for SLAM, and 3. we are able to work closely with the author of iSAM2, Professor Michael Kaess. Thus, we would like to take this opportunity to thank Professor Michael Kaess for dedicating his time and efforts to assisting our implementation of SLAM.

What is iSAM2 and Factor Graph SLAM?

iSAM2 (Incremental Smoothing and Mapping) is a SLAM (Simultaneous Localization and Mapping) algorithm used to construct a map of the track from the car’s position and the cones observed around the track. iSAM2 does this by constructing and optimizing a Factor Graph containing Variable Node s (which represent either landmark poses or car poses) and Factor Node s.

The Factor Graph

As stated previously, iSAM2 relies on a Factor Graph containing Variable Nodes and Factor Nodes. Factor Nodes, akin to labeled edges between the Variable Nodes, represent a joint probability distribution on the Variable Nodes connected to it. This joint probability distribution represents how certain iSAM2 is of the corresponding variables’ positions.

Note

A Variable Node cannot be adjacent to another Variable Node and a Factor Node cannot be adjacent to another Factor Node. Blue nodes represent Variable Nodes (X variable nodes represent car poses, L variable nodes represent landmark positions). Gray nodes represent Factor Nodes. Factor Node \(f_{0}\) is called a Prior Factor node; prior factor nodes are added to the first pose and sometimes the first landmark for iSAM2 to use as reference when localizing and mapping future poses and landmarks.

For example, observe how in Figure 1, Factor Node \(f_{1}\) is connected to Variable Node \(x_{0}\), representing the first car pose, and \(l_{0}\), representing the first landmark. Factor Node \(f_{1}\) represents a joint probabilistic distribution function over \(x_{0}\) and \(l_{0}\), which indicates how certain iSAM2 is of the positions for \(x_{0}\) and \(l_{0}\). Altogether, the entire Factor Graph represents a joint probabilistic distribution function F on all landmark positions and car poses. This function F is equal to the product of all factors \(f_{n}\), the joint probabilistic distribution function represented by each Factor Node in the graph.

Goal with respect to the Factor Graph The goal of iSAM2 is to maximize the joint probabilistic distribution function F by maximizing its factors. Intuitively, iSAM2 is seeking to maximize its certainty of landmark positions and car poses by updating its estimates for car poses and landmark positions over time (with the help of incoming observations). Considering the previous example, iSAM2 can maximize this function F by maximizing the joint probability distribution function represented by Factor Node \(f_{1}\).

Implementation

The iSAM2 node first parses the cones received by perceptions into separate vectors by color. This vector of observed cones and other Odometry information is used to update the iSAM2 model as well as the car’s current pose. Variable Node \(x_{n}\), representing the car pose at the current time stamp, is added alongside a Factor Node connecting \(x_{n}\) to \(x_{n-1}\), the Variable Node representing the previous car pose.

After determining the car pose, Data Association is performed on the cones observed at the current timestamp to determine which of the observed cones are new. To perform this Data Association, the Mahalanobis Distance is calculated between one observed cone, and all iSAM2 estimates for the previously seen cones. Intuitively, the Mahalanobis Distance represents how much the observed cone resembles a previously seen cone (the smaller the distance, the more the observed cone resembles the previously seen cone). If the smallest distance is greater than the Mahalanobis Distance Threshold, then the observed cone is a new cone.

Note

The Mahalanobis Distance threshold is generally found through tuning and trial and error.

Note

Mahalanobis Distance is used instead of Euclidean distance because where Euclidean distance can calculate the distance between two points, Mahalanobis Distance can calculate the distance between a point and a distribution. This is important because the cone positions come with uncertainty which is represented by a distribution (See more)

This process is repeated for all observed cones. Each detected new cone must be added to the Factor Graph as a Variable Node with a Factor Node connected to \(x_{n}\), the Variable Node representing the current car pose.