isam2.cpp
-
class slamISAM
A class that uses data association to discern new cones from previously seen cones and utilizes the iSAM2 model to perform SLAM.
-
inline void slamISAM::step(auto logger, gtsam::Pose2 global_odom, std::vector<Point2> &cone_obs, vector<Point2> &orange_ref_cones, gtsam::Point2 velocity, long time_ns, bool loopClosure)
Updates iSAM2 model by first performing data association on the cones observed from current time stamp. After determining which of the observed cones are new cones, the new cones are added to the factor graph which will be used to update the iSAM2 model.
- Parameters:
logger – RCLCPP logger used to print information to the terminal
global_odom – Pose2 type representing the global pose of the car as predicted by the motion model. This will be updated by step to hold the global pose of the car after the current time step.
cone_obs – The memory address of the vector containing the relative positions (with respect to the car) of the cones observed at the current time step.
orange_ref_cones – The memory address of the vector containing the positions of the orange cones observed at the start of the track. Will be used for loop closure
velocity – The velocity of the car at the current time step. Used as a part of the motion model to calculate the current pose of the car.
time_ns – The time elapsed from the previous time step. Used as a part of the motion model to calculate the current pose of the car.
loopClosure – Boolean that will be updated if loop closure has been detected.
-
inline void slamISAM::t_associate(vector<Point2> *cone_obs, vector<Pose2> *global_obs_cones, vector<Pose2> *all_cone_est, Pose2 global_odom, vector<float> *m_dist, int lo, int hi)
A function called by threads that will perform data association on the observed Point2 cones stored inside vector cone_obs. All calculated mahalanobis distances will be stored in vector m_dist. For a given time stamp, multiple threads will be spawned to calculate a subsection of the the mahalanobis distances from indices [lo, hi)
- Parameters:
global_obs_cones – A vector containing the global position for the Point2 cones observed in the current time stamp
all_cone_est – A vector containing the estimates for the poses of previously seen cones, calculated by the iSAM2 model
global_odom – A Pose2 variable representing the global pose of the car.
m_dist – A vector containing all the mahalanobis distance calculations that will need to be calculated for the current time stamp. Let n_landmarks represent the number of previously seen landmarks, m_dist is organized such that each set/multiple of n_landmarks + 1 elements corresponds to the mahalanobis distance between an observed cone and the global positions of all previously seen cones. The +1 represents the mahalanobis distance threshold (M_DIST_TH) These mahalanobis distances will be used to perform data association on the observed landmarks, by observing which previously seen cone had the smallest mahalanobis distance from the current observed cone. If the smallest distance is M_DIST_TH, then the current observed cone is a new cone
lo – An integer representing the index in m_dist the current thread should start populating m_dist
hi – An integer representing the index in m_dist the current thread should stop populating m_dist. Does not calculate the element at index hi