API Reference

slamISAM Class

class slamISAM

Public Functions

slamISAM(std::optional<rclcpp::Logger> input_logger, std::optional<yaml_params::NoiseInputs> &yaml_noise_inputs)

Constructor for the slamISAM class.

Initializes noise models, tunable constants, and internal parameters required for iSAM2 SLAM operation. It also sets up the noise models used for different factor types (landmark, odometry, prior, and unary).

Parameters:

  • input_logger – Optional ROS 2 logger for debug output

  • yaml_noise_inputs – Optional struct containing SLAM tuning parameters

inline slamISAM()
slam_output_t step(std::optional<gtsam::Point2> gps_opt, double yaw, const std::vector<gtsam::Point2> &cone_obs_blue, const std::vector<gtsam::Point2> &cone_obs_yellow, const std::vector<gtsam::Point2> &orange_ref_cones, gtsam::Pose2 velocity, double dt)

Performs a full SLAM update step using odometry and observed cone landmarks.

This function updates the SLAM factor graph using new motion data and cone observations. It includes steps such as pose prediction, data association, graph updates with both existing and new landmarks, and optional loop closure detection.

Parameters:

  • gps_opt – Optional GPS position of the car

  • yaw – The heading of the car in radians

  • cone_obs_blue – The observed blue cones in the local frame

  • cone_obs_yellow – The observed yellow cones in the local frame

  • orange_ref_cones – The orange reference cones

  • velocity – The car’s motion since the last step as a relative Pose2

  • dt – Time elapsed since the last SLAM step

Returns:

A tuple containing:

  • A vector of the most recent estimated blue cone positions as Points

  • A vector of the most recent estimated yellow cone positions as Points

  • A Point representing the current pose of the car

SLAMEstAndMCov Class

class SLAMEstAndMCov

Maintains and updates landmark estimates and marginal covariance matrices using iSAM2.

This class provides a lgihtweight interface around GTSAM’s iSAM2 for managing SLAM estimates and covariance matrices for landmarks (cones), which are continuously updated during the first lap. It supports partial and full recalculation, cone-based proximity updates, and Mahalanobis distance computation.

Public Functions

SLAMEstAndMCov(std::shared_ptr<gtsam::ISAM2> isam2, gtsam::Symbol (*cone_key_fn)(int), std::size_t look_radius, std::size_t update_iterations_n)

Construct a new SLAMEstAndMCov::SLAMEstAndMCov object.

Construct a new SLAMEstAndMCov object with configuration parameters.

Parameters:

  • isam2 – A shared pointer to the iSAM2 model that will be used for obtaining cone estimates and marginal covariance matrices.

  • cone_key_fn – A function pointer used for getting the symbol of a cone/landmark of interest. This symbol is used for retrieving cone estimates and the marginal covariance matrix of any cone or landmark given its ID.

  • look_radius – The number of cones to recalculate estimates and marginal covariances for, surrounding some pivot cone ID.

  • update_iterations_n – The number of iterations to run the iSAM2 update for.

SLAMEstAndMCov()

Default constructor for SLAMEstAndMCov.

Initializes internal members to deault values

void update_and_recalculate_all()

Recalculates all of the slam estimates and the marginal covariance matrices.

void update_and_recalculate_by_ID(const std::vector<std::size_t> &old_cone_ids)

Recalulates the estiamtes and the marginal covariance matrices for the IDs provided in the vector.

Parameters:

old_cone_ids – The IDs of the cones to recalculate estimates and marginal covariances for

void update_and_recalculate_beginning(std::size_t num_start_cones)

Recalculates the cone estimates and the marginal covariance matrices for the first num_start_cones IDs.

Parameters:

num_start_cones – The number of cones at the beginning to recalculate estimates and marginal covariances for.

void update_and_recalculate_cone_proximity(std::size_t pivot)

Recalculates the cone estimates and the marginal covariance matrices for the cones in the look radius of the pivot cone.

For some look_radius, calculate estimates and marginal covariances for the cones with IDs in [pivot - look_radius, pivot + look_radius] and [pivot, pivot + look_radius].

For some look_radius, calculate estimates and marginal covariances for the cones with IDs in [pivot - look_radius, pivot + look_radius] and [pivot, pivot + look_radius]. Still checks if you are in bounds of the IDs.

Parameters:

  • pivot – The ID of the pivot cone

  • pivot – The ID of the pivot cone

void update_with_old_cones(const std::vector<std::size_t> &old_cone_ids)

Recalulates the estiamtes and the marginal covariance matrices for the IDs provided in the vector.

Updates SLAM state with the set of previously seen cones.

Performs an update using their IDs and cones nearby the earliest, latest, and out-of-bounds IDs for local consistency

Parameters:

  • old_cone_ids – The IDs of the cones to recalculate estimates and marginal covariances for

  • old_cone_ids – Vector of landmark IDs that were matched to current observations

void update_with_new_cones(std::size_t num_new_cones)

Adds information about the new cones to slam_est and slam_mcov.

Appends new cone estimates and their marginal covariances.

Also updates n_landmarks appropriately

Parameters:

  • num_new_cones – The number of new cones/estimates/marginal covariances to add

  • num_new_cones – The number of new cones/estimates/marginal covariances to add

std::vector<double> calculate_mdist(gtsam::Point2 global_obs_cone)

Calculates the Mahalanobis distance between the observed cone and the old cone estimates.

The distances are calculated using a SIMD method through the Eigen library.

The distances are calculated using a SIMD method through the Eigen library.

Parameters:

  • global_obs_cone – The observed cone that we are data associating in global frame

  • global_obs_cone – The observed cone that we are data associating in global frame

Returns:

std::vector<double> A vector where the ith element represents the mahalanobis distances

Returns:

A vector of Mahalanobis distances to each tracked landmark

std::size_t get_n_landmarks()

Gets the number of landmarks in the SLAM estimates.

Returns:

Number of landmarks

std::vector<gtsam::Point2> get_all_est()

Gets the SLAM cone estimates.

Returns:

A vector of 2D points representing the estimated landmark positions

bool check_lengths()

An invariant function to check that the lengths between the slam estimates and the marginal covariance matrices are the same.

Returns:

true if the lengths are the same

Returns:

false if the lengths are not the same

gtsam::Symbol get_landmark_symbol(int id)

Get the landmark symbol object.

Parameters:

  • id –

  • id – Landmark ID

Returns:

gtsam::Symbol

Returns:

Corresponding GTSAM symbol