Graph SLAM solver class. More...

#include <graph_slam_solver.hpp>

Inheritance diagram for GraphSLAMSolver:

Collaboration diagram for GraphSLAMSolver:

Public Member Functions
	GraphSLAMSolver (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 GraphSLAMSolver object.

	~GraphSLAMSolver ()=default

void	init (std::weak_ptr< rclcpp::Node > node) override
	Initialize the SLAM solver.

void	add_velocities (const common_lib::structures::Velocities &velocities) override
	Process new velocities data (prediction step)

void	add_odometry (const common_lib::structures::Pose &pose_difference) override
	Add odometry data to the solver (for prediction step)

void	add_imu_data (const common_lib::sensor_data::ImuData &imu_data) override
	Add IMU data to the solver.

void	add_observations (const std::vector< common_lib::structures::Cone > &cones, rclcpp::Time cones_timestamp) override
	Add observations to the solver (correction step)

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

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.

std::vector< common_lib::structures::Pose >	get_trajectory_estimate () override
	Get the trajectory estimate of the car (all poses)

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

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

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

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

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

Private Member Functions
void	_asynchronous_optimization_routine ()
	Asynchronous optimization routine.

bool	_add_motion_data_to_graph (const std::shared_ptr< PoseUpdater > pose_updater, const std::shared_ptr< GraphSLAMInstance > graph_slam_instance, bool force_update=false)
	Adds motion data to the graph if the pose updater is ready.

Private Attributes
std::shared_ptr< GraphSLAMInstance >	_graph_slam_instance_

std::shared_ptr< PoseUpdater >	_pose_updater_

std::shared_ptr< OdometryModel >	_odometry_model

std::queue< MotionData >	_motion_data_queue_

std::queue< ObservationData >	_observation_data_queue_

rclcpp::TimerBase::SharedPtr	_optimization_timer_

std::shared_mutex	_mutex_

bool	_optimization_under_way_ = false

Eigen::VectorXi	_associations_

Eigen::VectorXd	_observations_global_

Eigen::VectorXd	_map_coordinates_

common_lib::sensor_data::ImuData	_last_imu_data_

rclcpp::CallbackGroup::SharedPtr	_reentrant_group_

Friends
class	GraphSlamSolverTest_MotionAndObservation_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

Graph SLAM solver class.

This class implements the Graph SLAM solver using GTSAM It uses a factor graph to represent the problem and the values to store the estimates This specific class controls the access to the models used in the SLAM implementation, including measures for parallel execution

Definition at line 31 of file graph_slam_solver.hpp.

Constructor & Destructor Documentation

◆ GraphSLAMSolver()

GraphSLAMSolver::GraphSLAMSolver	(	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 GraphSLAMSolver object.

Parameters

params	Parameters for the SLAM solver
data_association	Data association module
motion_model	Motion model
execution_times	Timekeeping array

Definition at line 27 of file graph_slam_solver.cpp.

◆ ~GraphSLAMSolver()

GraphSLAMSolver::~GraphSLAMSolver ( )

default

Member Function Documentation

◆ _add_motion_data_to_graph()

bool GraphSLAMSolver::_add_motion_data_to_graph	(	const std::shared_ptr< PoseUpdater >	pose_updater,
		const std::shared_ptr< GraphSLAMInstance >	graph_slam_instance,
		bool	force_update = `false`
	)

private

Adds motion data to the graph if the pose updater is ready.

This method checks if the pose updater has a new pose to add to the graph If so, it adds the new pose to the graph and resets the pose updater's accumulated pose difference

Parameters

pose_updater	Pose updater to get the new pose from
graph_slam_instance	Graph SLAM instance to add the new pose to
force_update	If true, forces the update even if the pose updater is not ready

Returns: true if the pose was added to the graph, false otherwise

Definition at line 64 of file graph_slam_solver.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ _asynchronous_optimization_routine()

void GraphSLAMSolver::_asynchronous_optimization_routine ( )

private

Asynchronous optimization routine.

This method is used to run the optimization in a separate thread It is called by the timer and runs the optimization on the graph It also updates the pose and the graph values accordingly afterwards

Definition at line 301 of file graph_slam_solver.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ add_imu_data()

void GraphSLAMSolver::add_imu_data ( const common_lib::sensor_data::ImuData & imu_data )

overridevirtual

Add IMU data to the solver.

Parameters

imu_data IMU data

Implements ImuIntegratorTrait.

Definition at line 164 of file graph_slam_solver.cpp.

◆ add_observations()

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

overridevirtual

Add observations to the solver (correction step)

Parameters

cones	Positions of the observations
cones_timestamp	Timestamp of the observations

Implements SLAMSolver.

Definition at line 168 of file graph_slam_solver.cpp.

Here is the call graph for this function:

◆ add_odometry()

void GraphSLAMSolver::add_odometry ( const common_lib::structures::Pose & pose_difference )

overridevirtual

Add odometry data to the solver (for prediction step)

It calls the pose updater using the motion model to predict the new pose and may add the motion data to the graph

Parameters

pose_difference Pose difference in the form of [dx, dy, dtheta]

Implements OdometryIntegratorTrait.

Definition at line 83 of file graph_slam_solver.cpp.

Here is the call graph for this function:

◆ add_velocities()

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

overridevirtual

Process new velocities data (prediction step)

It calls the pose updater using the motion model to predict the new pose and may add the motion data to the graph

Parameters

velocities Velocities data

Implements SLAMSolver.

Definition at line 120 of file graph_slam_solver.cpp.

Here is the call graph for this function:

◆ get_associations()

Eigen::VectorXi GraphSLAMSolver::get_associations ( ) const

overridevirtual

Implements SLAMSolver.

Definition at line 441 of file graph_slam_solver.cpp.

◆ get_covariance()

Eigen::MatrixXd GraphSLAMSolver::get_covariance ( )

overridevirtual

Get the covariance matrix of the graph.

Returns: Eigen::MatrixXd covariance matrix

Implements SLAMSolver.

Definition at line 436 of file graph_slam_solver.cpp.

◆ get_lap_counter()

int GraphSLAMSolver::get_lap_counter ( )

inlineoverridevirtual

Get the lap counter.

Returns: int lap counter

Implements SLAMSolver.

Definition at line 183 of file graph_slam_solver.hpp.

◆ get_map_coordinates()

Eigen::VectorXd GraphSLAMSolver::get_map_coordinates ( ) const

overridevirtual

Implements SLAMSolver.

Definition at line 451 of file graph_slam_solver.cpp.

◆ get_map_estimate()

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

overridevirtual

Get the map estimate object.

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

Implements SLAMSolver.

Definition at line 381 of file graph_slam_solver.cpp.

◆ get_observations_global()

Eigen::VectorXd GraphSLAMSolver::get_observations_global ( ) const

overridevirtual

Implements SLAMSolver.

Definition at line 446 of file graph_slam_solver.cpp.

◆ get_pose_estimate()

common_lib::structures::Pose GraphSLAMSolver::get_pose_estimate ( )

overridevirtual

Get the pose estimate object.

Returns: common_lib::structures::Pose

Implements SLAMSolver.

Definition at line 410 of file graph_slam_solver.cpp.

◆ get_trajectory_estimate()

std::vector< common_lib::structures::Pose > GraphSLAMSolver::get_trajectory_estimate ( )

overridevirtual

Get the trajectory estimate of the car (all poses)

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

Implements TrajectoryCalculator.

Definition at line 421 of file graph_slam_solver.cpp.

◆ init()

void GraphSLAMSolver::init ( std::weak_ptr< rclcpp::Node > node )

overridevirtual

Initialize the SLAM solver.

This method is used to initialize the SLAM solver's aspects that require the node e.g. timer callbacks

Parameters

node ROS2 node

Implements NodeControllerTrait.

Definition at line 41 of file graph_slam_solver.cpp.

◆ load_initial_state()

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

overridevirtual

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

Parameters

map	Coordinates of the landmarks in the form of [x1, y1, x2, y2, ...] relative to the global frame
pose	Pose of the robot in the form of [x, y, theta] relative to the global frame

Implements SLAMSolver.

Definition at line 296 of file graph_slam_solver.cpp.

Friends And Related Symbol Documentation

◆ GraphSlamSolverTest_MotionAndObservation_Test

friend class GraphSlamSolverTest_MotionAndObservation_Test

friend

Definition at line 80 of file graph_slam_solver.hpp.

Member Data Documentation

◆ _associations_

Eigen::VectorXi GraphSLAMSolver::_associations_

private

Definition at line 49 of file graph_slam_solver.hpp.

◆ _graph_slam_instance_

std::shared_ptr<GraphSLAMInstance> GraphSLAMSolver::_graph_slam_instance_

private

Definition at line 37 of file graph_slam_solver.hpp.

◆ _last_imu_data_

common_lib::sensor_data::ImuData GraphSLAMSolver::_last_imu_data_

private

Definition at line 53 of file graph_slam_solver.hpp.

◆ _map_coordinates_

Eigen::VectorXd GraphSLAMSolver::_map_coordinates_

private

Definition at line 51 of file graph_slam_solver.hpp.

◆ _motion_data_queue_

std::queue<MotionData> GraphSLAMSolver::_motion_data_queue_

private

Definition at line 42 of file graph_slam_solver.hpp.

◆ _mutex_

std::shared_mutex GraphSLAMSolver::_mutex_

mutableprivate

Definition at line 47 of file graph_slam_solver.hpp.

◆ _observation_data_queue_

std::queue<ObservationData> GraphSLAMSolver::_observation_data_queue_

private

Definition at line 44 of file graph_slam_solver.hpp.

◆ _observations_global_

Eigen::VectorXd GraphSLAMSolver::_observations_global_

private

Definition at line 50 of file graph_slam_solver.hpp.

◆ _odometry_model

std::shared_ptr<OdometryModel> GraphSLAMSolver::_odometry_model

private

Definition at line 40 of file graph_slam_solver.hpp.

◆ _optimization_timer_

rclcpp::TimerBase::SharedPtr GraphSLAMSolver::_optimization_timer_

private

Definition at line 45 of file graph_slam_solver.hpp.

◆ _optimization_under_way_

bool GraphSLAMSolver::_optimization_under_way_ = false

private

Definition at line 48 of file graph_slam_solver.hpp.

◆ _pose_updater_

std::shared_ptr<PoseUpdater> GraphSLAMSolver::_pose_updater_

private

Definition at line 38 of file graph_slam_solver.hpp.

◆ _reentrant_group_

rclcpp::CallbackGroup::SharedPtr GraphSLAMSolver::_reentrant_group_

private

Definition at line 56 of file graph_slam_solver.hpp.

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

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

Public Member Functions

Private Member Functions

Private Attributes

Friends

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ GraphSLAMSolver()

◆ ~GraphSLAMSolver()

Member Function Documentation

◆ _add_motion_data_to_graph()

◆ _asynchronous_optimization_routine()

◆ add_imu_data()

◆ add_observations()

◆ add_odometry()

◆ add_velocities()

◆ get_associations()

◆ get_covariance()

◆ get_lap_counter()

◆ get_map_coordinates()

◆ get_map_estimate()

◆ get_observations_global()

◆ get_pose_estimate()

◆ get_trajectory_estimate()

◆ init()

◆ load_initial_state()

Friends And Related Symbol Documentation

◆ GraphSlamSolverTest_MotionAndObservation_Test

Member Data Documentation

◆ _associations_

◆ _graph_slam_instance_

◆ _last_imu_data_

◆ _map_coordinates_

◆ _motion_data_queue_

◆ _mutex_

◆ _observation_data_queue_

◆ _observations_global_

◆ _odometry_model

◆ _optimization_timer_

◆ _optimization_under_way_

◆ _pose_updater_

◆ _reentrant_group_