#include <ekf_slam_solver.hpp>

Inheritance diagram for EKFSLAMSolver:

Collaboration diagram for EKFSLAMSolver:

Public Member Functions
	EKFSLAMSolver (const SLAMParameters &params, std::shared_ptr< DataAssociationModel > data_association, std::shared_ptr< V2PMotionModel > motion_model, std::shared_ptr< LandmarkFilter > landmark_filter, std::shared_ptr< std::vector< double > > execution_times, std::shared_ptr< LoopClosure > loop_closure)

void	add_velocities (const common_lib::structures::Velocities &velocities) override
	Executed to deal with new velocity data.

void	add_observations (const std::vector< common_lib::structures::Cone > &positions, rclcpp::Time cones_timestamp) override
	process obervations of landmarks

void	load_initial_state (const Eigen::VectorXd &map, const Eigen::Vector3d &pose) override
	Initialize the EKF SLAM solver with a previously saved map and pose.

Eigen::MatrixXd	get_covariance () override
	Get the covariance matrix of the EKF.

int	get_lap_counter () override
	Get the lap counter.

Eigen::VectorXi	get_associations () const override

Eigen::VectorXd	get_observations_global () const override

Eigen::VectorXd	get_map_coordinates () const override

Public Member Functions inherited from SLAMSolver
	SLAMSolver (const SLAMParameters &params, std::shared_ptr< DataAssociationModel > data_association, std::shared_ptr< V2PMotionModel > motion_model, std::shared_ptr< LandmarkFilter > landmark_filter, std::shared_ptr< std::vector< double > > execution_times, std::shared_ptr< LoopClosure > loop_closure)
	Construct a new SLAMSolver object.

virtual	~SLAMSolver ()=default

void	set_mission (common_lib::competition_logic::Mission mission)
	Set the mission.

Public Member Functions inherited from VelocitiesIntegratorTrait
virtual	~VelocitiesIntegratorTrait ()=default

Private Member Functions
Eigen::MatrixXd	get_observation_noise_matrix (int num_landmarks) const
	Get the observation noise matrix object used in the correction step of the EKF with the right dimensions.

void	predict (Eigen::VectorXd &state, Eigen::MatrixXd &covariance, const Eigen::MatrixXd &process_noise_matrix, const rclcpp::Time last_update, const common_lib::structures::Velocities &velocities)
	executed when velocity data is received.

void	correct (Eigen::VectorXd &state, Eigen::MatrixXd &covariance, const std::vector< int > &observed_landmarks_indices, const Eigen::VectorXd &matched_observations)
	correction step of the EKF that updates the state and covariance based on the observed landmarks

void	state_augmentation (Eigen::VectorXd &state, Eigen::MatrixXd &covariance, const Eigen::VectorXd &new_landmarks)
	add new landmarks to the state vector and covariance matrix

std::vector< common_lib::structures::Cone >	get_map_estimate () override
	Get the map estimate object.

common_lib::structures::Pose	get_pose_estimate () override
	Get the pose estimate object.

Private Attributes
SLAMParameters	slam_parameters_

std::shared_ptr< SLAMObservationModel >	observation_model_

Eigen::VectorXd	state_ = Eigen::VectorXd::Zero(3)

Eigen::MatrixXd	covariance_

Eigen::MatrixXd	process_noise_matrix_

std::shared_mutex	mutex_

Eigen::Vector3d	pose_difference_ = Eigen::Vector3d::Zero()

bool	correction_step_ongoing_ = false

rclcpp::Time	last_update_

bool	velocities_received_ = false

bool	cones_receieved_ = false

Friends
class	EKFSLAMSolverTest_stateAugmentation_Test

class	EKFSLAMSolverTest_stateAugmentation2_Test

Additional Inherited Members
Protected Attributes inherited from SLAMSolver
SLAMParameters	_params_

std::shared_ptr< DataAssociationModel >	_data_association_

std::shared_ptr< V2PMotionModel >	_motion_model_

std::shared_ptr< LandmarkFilter >	_landmark_filter_

common_lib::competition_logic::Mission	_mission_ = common_lib::competition_logic::Mission::NONE

std::shared_ptr< std::vector< double > >	_execution_times_

std::shared_ptr< LoopClosure >	_loop_closure_

rclcpp::Time	_last_pose_update_ = rclcpp::Time(0)

rclcpp::Time	_last_observation_update_ = rclcpp::Time(0)

bool	_received_first_velocities_

int	lap_counter_ = 0

Detailed Description

Definition at line 12 of file ekf_slam_solver.hpp.

Constructor & Destructor Documentation

◆ EKFSLAMSolver()

EKFSLAMSolver::EKFSLAMSolver	(	const SLAMParameters &	params,
		std::shared_ptr< DataAssociationModel >	data_association,
		std::shared_ptr< V2PMotionModel >	motion_model,
		std::shared_ptr< LandmarkFilter >	landmark_filter,
		std::shared_ptr< std::vector< double > >	execution_times,
		std::shared_ptr< LoopClosure >	loop_closure
	)

Definition at line 3 of file ekf_slam_solver.cpp.

Member Function Documentation

◆ add_observations()

void EKFSLAMSolver::add_observations	(	const std::vector< common_lib::structures::Cone > &	positions,
		rclcpp::Time	cones_timestamp
	)

overridevirtual

process obervations of landmarks

Parameters

position

Implements SLAMSolver.

Definition at line 47 of file ekf_slam_solver.cpp.

Here is the call graph for this function:

◆ add_velocities()

void EKFSLAMSolver::add_velocities ( const common_lib::structures::Velocities & velocities )

overridevirtual

Executed to deal with new velocity data.

Parameters

velocities

Implements SLAMSolver.

Definition at line 33 of file ekf_slam_solver.cpp.

Here is the call graph for this function:

◆ correct()

void EKFSLAMSolver::correct	(	Eigen::VectorXd &	state,
		Eigen::MatrixXd &	covariance,
		const std::vector< int > &	observed_landmarks_indices,
		const Eigen::VectorXd &	matched_observations
	)

private

correction step of the EKF that updates the state and covariance based on the observed landmarks

Parameters

state	state vector previouslt to the correction step
covariance	covariance matrix previously to the correction step
matched_landmarks_indices	indexes of the x coordinate of landmarks in the state vector that were observed
matched_observations	landmarks in the state vector that were observed, in the form {x_cone_1, y_cone_1, x_cone_2, y_cone_2, ...}

Definition at line 138 of file ekf_slam_solver.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_associations()

Eigen::VectorXi EKFSLAMSolver::get_associations ( ) const

inlineoverridevirtual

Implements SLAMSolver.

Definition at line 143 of file ekf_slam_solver.hpp.

◆ get_covariance()

Eigen::MatrixXd EKFSLAMSolver::get_covariance ( )

inlineoverridevirtual

Get the covariance matrix of the EKF.

Returns: Eigen::MatrixXd covariance matrix

Implements SLAMSolver.

Definition at line 134 of file ekf_slam_solver.hpp.

◆ get_lap_counter()

int EKFSLAMSolver::get_lap_counter ( )

inlineoverridevirtual

Get the lap counter.

Returns: int lap counter

Implements SLAMSolver.

Definition at line 141 of file ekf_slam_solver.hpp.

◆ get_map_coordinates()

Eigen::VectorXd EKFSLAMSolver::get_map_coordinates ( ) const

inlineoverridevirtual

Implements SLAMSolver.

Definition at line 145 of file ekf_slam_solver.hpp.

◆ get_map_estimate()

std::vector< common_lib::structures::Cone > EKFSLAMSolver::get_map_estimate ( )

overrideprivatevirtual

Get the map estimate object.

Returns: std::vector<common_lib::structures::Cone>

Implements SLAMSolver.

Definition at line 191 of file ekf_slam_solver.cpp.

◆ get_observation_noise_matrix()

Eigen::MatrixXd EKFSLAMSolver::get_observation_noise_matrix ( int num_landmarks ) const

private

Get the observation noise matrix object used in the correction step of the EKF with the right dimensions.

Parameters

num_landmarks number of observed landmarks in a given EKF correction step

Returns: Eigen::MatrixXd

Definition at line 23 of file ekf_slam_solver.cpp.

Here is the caller graph for this function:

◆ get_observations_global()

Eigen::VectorXd EKFSLAMSolver::get_observations_global ( ) const

inlineoverridevirtual

Implements SLAMSolver.

Definition at line 144 of file ekf_slam_solver.hpp.

◆ get_pose_estimate()

common_lib::structures::Pose EKFSLAMSolver::get_pose_estimate ( )

overrideprivatevirtual

Get the pose estimate object.

Returns: common_lib::structures::Pose

Implements SLAMSolver.

Definition at line 207 of file ekf_slam_solver.cpp.

◆ load_initial_state()

void EKFSLAMSolver::load_initial_state	(	const Eigen::VectorXd &	map,
		const Eigen::Vector3d &	pose
	)

overridevirtual

Initialize the EKF SLAM solver with a previously saved map and pose.

Simply copies the map and pose into the state vector and covariance matrix.

Returns: Eigen::VectorXd state vector

Implements SLAMSolver.

Definition at line 175 of file ekf_slam_solver.cpp.

◆ predict()

void EKFSLAMSolver::predict	(	Eigen::VectorXd &	state,
		Eigen::MatrixXd &	covariance,
		const Eigen::MatrixXd &	process_noise_matrix,
		const rclcpp::Time	last_update,
		const common_lib::structures::Velocities &	velocities
	)

private

executed when velocity data is received.

Prediction step of the EKF which is meant to capture changes in the state of the system

Parameters

state	vector with position and orientation, followed by the landmark positions {car_x, car_y, car_theta, x_cone_1, y_cone_1, x_cone_2, y_cone_2, ...}
covariance
process_noise_matrix	estimated process noise
last_update	last time velocity data was received
velocities	new velocity data

Definition at line 111 of file ekf_slam_solver.cpp.

Here is the caller graph for this function:

◆ state_augmentation()

void EKFSLAMSolver::state_augmentation	(	Eigen::VectorXd &	state,
		Eigen::MatrixXd &	covariance,
		const Eigen::VectorXd &	new_landmarks
	)

private

add new landmarks to the state vector and covariance matrix

Expands the covariance matrix which originally has the structure: P = [ Pcc, Pcl Plc, Pll] To include the new landmarks, the covariance matrix is expanded to: P = [ Pcc, Pcl, Pcn Plc, Pll, Pln Pnc, Pnl, Pnn] consider C to be the car's position and orientation, L to be the landmarks that were already in the state vector and N to be the new landmarks to be added to the state vector: Pcc is the covariance of C with C, Pcl is the covariance of C with L, Plc is the covariance of L with C, ...

The new landmarks are added to the state vector after being converted to the global frame.

Parameters

state	state vector with the car's position and orientation, followed by the landmarks in the form {car_x, car_y, car_theta, x_cone_1, y_cone_1, x_cone_2, y_cone_2, ...}
covariance	covariance matrix of the state vector
new_landmarks	new landmarks in the form {x_cone_1, y_cone_1, x_cone_2, y_cone_2, ...} in the car's frame

Definition at line 155 of file ekf_slam_solver.cpp.

Here is the caller graph for this function:

Friends And Related Symbol Documentation

◆ EKFSLAMSolverTest_stateAugmentation2_Test

friend class EKFSLAMSolverTest_stateAugmentation2_Test

friend

Definition at line 96 of file ekf_slam_solver.hpp.

◆ EKFSLAMSolverTest_stateAugmentation_Test

friend class EKFSLAMSolverTest_stateAugmentation_Test

friend

Definition at line 95 of file ekf_slam_solver.hpp.

Member Data Documentation

◆ cones_receieved_

bool EKFSLAMSolver::cones_receieved_ = false

private

Definition at line 25 of file ekf_slam_solver.hpp.

◆ correction_step_ongoing_

bool EKFSLAMSolver::correction_step_ongoing_ = false

private

Definition at line 20 of file ekf_slam_solver.hpp.

◆ covariance_

Eigen::MatrixXd EKFSLAMSolver::covariance_

private

Definition at line 16 of file ekf_slam_solver.hpp.

◆ last_update_

rclcpp::Time EKFSLAMSolver::last_update_

private

Definition at line 22 of file ekf_slam_solver.hpp.

◆ mutex_

std::shared_mutex EKFSLAMSolver::mutex_

private

Definition at line 18 of file ekf_slam_solver.hpp.

◆ observation_model_

std::shared_ptr<SLAMObservationModel> EKFSLAMSolver::observation_model_

private

Definition at line 14 of file ekf_slam_solver.hpp.

◆ pose_difference_

Eigen::Vector3d EKFSLAMSolver::pose_difference_ = Eigen::Vector3d::Zero()

private

Definition at line 19 of file ekf_slam_solver.hpp.

◆ process_noise_matrix_

Eigen::MatrixXd EKFSLAMSolver::process_noise_matrix_

private

Definition at line 17 of file ekf_slam_solver.hpp.

◆ slam_parameters_

SLAMParameters EKFSLAMSolver::slam_parameters_

private

Definition at line 13 of file ekf_slam_solver.hpp.

◆ state_

Eigen::VectorXd EKFSLAMSolver::state_ = Eigen::VectorXd::Zero(3)

private

Definition at line 15 of file ekf_slam_solver.hpp.

◆ velocities_received_

bool EKFSLAMSolver::velocities_received_ = false

private

Definition at line 24 of file ekf_slam_solver.hpp.

The documentation for this class was generated from the following files:

src/slam/include/slam_solver/ekf_slam_solver.hpp
src/slam/src/slam_solver/ekf_slam_solver.cpp

Public Member Functions

Private Member Functions

Private Attributes

Friends

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ EKFSLAMSolver()

Member Function Documentation

◆ add_observations()

◆ add_velocities()

◆ correct()

◆ get_associations()

◆ get_covariance()

◆ get_lap_counter()

◆ get_map_coordinates()

◆ get_map_estimate()

◆ get_observation_noise_matrix()

◆ get_observations_global()

◆ get_pose_estimate()

◆ load_initial_state()

◆ predict()

◆ state_augmentation()

Friends And Related Symbol Documentation

◆ EKFSLAMSolverTest_stateAugmentation2_Test

◆ EKFSLAMSolverTest_stateAugmentation_Test

Member Data Documentation

◆ cones_receieved_

◆ correction_step_ongoing_

◆ covariance_

◆ last_update_

◆ mutex_

◆ observation_model_

◆ pose_difference_

◆ process_noise_matrix_

◆ slam_parameters_

◆ state_

◆ velocities_received_