Graph SLAM instance class - class to hold the factor graph and the values. More...

#include <graph_slam_instance.hpp>

Collaboration diagram for GraphSLAMInstance:

Classes
struct	TimedPose

Public Member Functions
	GraphSLAMInstance ()=default

	GraphSLAMInstance (const SLAMParameters &params, std::shared_ptr< BaseOptimizer > optimizer)

	GraphSLAMInstance (const GraphSLAMInstance &other)

GraphSLAMInstance &	operator= (const GraphSLAMInstance &other)

	~GraphSLAMInstance ()=default

std::shared_ptr< GraphSLAMInstance >	clone () const
	Clone the graph SLAM instance.

bool	new_pose_factors () const
	Checks if new observation factors should be added.

bool	new_observation_factors () const
	Checks if it is worth running optimization.

unsigned int	get_landmark_counter () const
	Get the number of landmarks.

unsigned int	get_pose_counter () const
	Get the number of poses.

const gtsam::Pose2	get_pose () const
	Get the current pose estimate.

const gtsam::Values &	get_graph_values_reference () const
	Get the graph values reference.

Eigen::VectorXd	get_state_vector () const
	Get the state vector, EKF style.

Eigen::MatrixXd	get_covariance_matrix () const
	Get the covariance matrix of the graph.

void	process_observations (const ObservationData &observation_data)
	Add observation factors to the graph.

void	load_initial_state (const Eigen::VectorXd &map, const Eigen::Vector3d &pose, double preloaded_map_noise)
	Loads a map and an initial pose into the graph.

void	process_new_pose (const Eigen::Vector3d &pose_difference, const Eigen::Vector3d &noise_vector, const Eigen::Vector3d &new_pose, const rclcpp::Time &pose_timestamp)
	Process a new pose and respective pose difference.

void	process_pose_difference (const Eigen::Vector3d &pose_difference, const Eigen::Vector3d &noise_vector, unsigned int before_pose_id, unsigned int after_pose_id)
	Process a pose difference and add the respective factor to the graph.

void	process_pose_difference (const Eigen::Vector3d &pose_difference, const Eigen::Vector3d &noise_vector, unsigned int before_pose_id)
	Process a pose difference and add the respective factor to the graph (after pose is current pose)

void	process_pose_difference (const Eigen::Vector3d &pose_difference, const Eigen::Vector3d &noise_vector)
	Process a pose difference and add the respective factor to the graph (using current pose and last graphed pose)

void	optimize ()
	Runs optimization on the graph.

void	lock_landmarks ()
	Soft locks the positions of the landmarks, can be used after the first lap.

Protected Member Functions
unsigned int	get_landmark_id_at_time (const rclcpp::Time &timestamp) const

Protected Attributes
gtsam::NonlinearFactorGraph	_factor_graph_

gtsam::Values	_graph_values_

unsigned int	_pose_counter_ = 0

unsigned int	_landmark_counter_ = 0

bool	_new_pose_node_

bool	_new_observation_factors_

bool	_locked_landmarks_ = false

SLAMParameters	_params_

std::shared_ptr< BaseOptimizer >	_optimizer_

CircularBuffer< TimedPose >	_pose_timestamps_

Detailed Description

Graph SLAM instance class - class to hold the factor graph and the values.

This class is used to hold the factor graph and the values for the graph SLAM solver All operations to the factor graph and values are carried out by this object

Definition at line 18 of file graph_slam_instance.hpp.

Constructor & Destructor Documentation

◆ GraphSLAMInstance() [1/3]

GraphSLAMInstance::GraphSLAMInstance ( )

default

◆ GraphSLAMInstance() [2/3]

GraphSLAMInstance::GraphSLAMInstance	(	const SLAMParameters &	params,
		std::shared_ptr< BaseOptimizer >	optimizer
	)

Definition at line 78 of file graph_slam_instance.cpp.

◆ GraphSLAMInstance() [3/3]

GraphSLAMInstance::GraphSLAMInstance ( const GraphSLAMInstance & other )

Definition at line 102 of file graph_slam_instance.cpp.

◆ ~GraphSLAMInstance()

GraphSLAMInstance::~GraphSLAMInstance ( )

default

Member Function Documentation

◆ clone()

std::shared_ptr< GraphSLAMInstance > GraphSLAMInstance::clone ( ) const

inline

Clone the graph SLAM instance.

This method is used to create a copy of the graph SLAM instance It is useful for polymorphic classes that use pointers

Returns: A shared pointer to the cloned graph SLAM instance

Definition at line 63 of file graph_slam_instance.hpp.

◆ get_covariance_matrix()

Eigen::MatrixXd GraphSLAMInstance::get_covariance_matrix ( ) const

Get the covariance matrix of the graph.

Returns: Eigen::MatrixXd covariance matrix

Definition at line 57 of file graph_slam_instance.cpp.

◆ get_graph_values_reference()

const gtsam::Values & GraphSLAMInstance::get_graph_values_reference ( ) const

Get the graph values reference.

Returns: const gtsam::gtsam::Values& reference to the factor graph

Definition at line 26 of file graph_slam_instance.cpp.

◆ get_landmark_counter()

unsigned int GraphSLAMInstance::get_landmark_counter ( ) const

Get the number of landmarks.

Returns: unsigned int number of landmarks

Definition at line 53 of file graph_slam_instance.cpp.

◆ get_landmark_id_at_time()

unsigned int GraphSLAMInstance::get_landmark_id_at_time ( const rclcpp::Time & timestamp ) const

protected

Definition at line 316 of file graph_slam_instance.cpp.

Here is the caller graph for this function:

◆ get_pose()

const gtsam::Pose2 GraphSLAMInstance::get_pose ( ) const

Get the current pose estimate.

Returns: const gtsam::Pose2 current pose

Definition at line 30 of file graph_slam_instance.cpp.

◆ get_pose_counter()

unsigned int GraphSLAMInstance::get_pose_counter ( ) const

Get the number of poses.

Returns: unsigned int number of poses

Definition at line 55 of file graph_slam_instance.cpp.

◆ get_state_vector()

Eigen::VectorXd GraphSLAMInstance::get_state_vector ( ) const

Get the state vector, EKF style.

Returns: const Eigen::VectorXd state vector

Definition at line 34 of file graph_slam_instance.cpp.

◆ load_initial_state()

void GraphSLAMInstance::load_initial_state	(	const Eigen::VectorXd &	map,
		const Eigen::Vector3d &	pose,
		double	preloaded_map_noise
	)

Loads a map and an initial pose into the graph.

Parameters

map	Coordinates of the map landmarks in the form of an Eigen vector [x1, y1, x2, y2, ...]
pose	Initial pose of the robot in the form of an Eigen vector [x, y, theta]
preloaded_map_noise	Noise to apply to the preloaded map landmarks

Definition at line 257 of file graph_slam_instance.cpp.

◆ lock_landmarks()

void GraphSLAMInstance::lock_landmarks ( )

Soft locks the positions of the landmarks, can be used after the first lap.

Definition at line 298 of file graph_slam_instance.cpp.

◆ new_observation_factors()

bool GraphSLAMInstance::new_observation_factors ( ) const

Checks if it is worth running optimization.

Returns: true if new observation factors exist

Definition at line 24 of file graph_slam_instance.cpp.

Here is the caller graph for this function:

◆ new_pose_factors()

bool GraphSLAMInstance::new_pose_factors ( ) const

Checks if new observation factors should be added.

Returns: true if new pose factors exist

Definition at line 22 of file graph_slam_instance.cpp.

Here is the caller graph for this function:

◆ operator=()

GraphSLAMInstance & GraphSLAMInstance::operator= ( const GraphSLAMInstance & other )

Definition at line 116 of file graph_slam_instance.cpp.

◆ optimize()

void GraphSLAMInstance::optimize ( )

Runs optimization on the graph.

This method calls the optimizer to run optimization on the current factor graph

Definition at line 287 of file graph_slam_instance.cpp.

◆ process_new_pose()

void GraphSLAMInstance::process_new_pose	(	const Eigen::Vector3d &	pose_difference,
		const Eigen::Vector3d &	noise_vector,
		const Eigen::Vector3d &	new_pose,
		const rclcpp::Time &	pose_timestamp
	)

Process a new pose and respective pose difference.

This method adds a new pose node to the graph values and calls the process_pose_difference method to add the respective pose difference factor to the graph

Parameters

pose_difference	Pose difference from the last pose
noise_vector	Noise associated with the pose difference
new_pose	The new pose to add to the graph

Definition at line 134 of file graph_slam_instance.cpp.

Here is the call graph for this function:

◆ process_observations()

void GraphSLAMInstance::process_observations ( const ObservationData & observation_data )

Add observation factors to the graph.

Parameters

observation_data Observation data to add to the graph

Definition at line 196 of file graph_slam_instance.cpp.

Here is the call graph for this function:

◆ process_pose_difference() [1/3]

void GraphSLAMInstance::process_pose_difference	(	const Eigen::Vector3d &	pose_difference,
		const Eigen::Vector3d &	noise_vector
	)

Process a pose difference and add the respective factor to the graph (using current pose and last graphed pose)

Parameters

pose_difference	Pose difference from the last pose
noise_vector	Noise associated with the pose difference

Definition at line 190 of file graph_slam_instance.cpp.

Here is the call graph for this function:

◆ process_pose_difference() [2/3]

void GraphSLAMInstance::process_pose_difference	(	const Eigen::Vector3d &	pose_difference,
		const Eigen::Vector3d &	noise_vector,
		unsigned int	before_pose_id
	)

Process a pose difference and add the respective factor to the graph (after pose is current pose)

Parameters

pose_difference	Pose difference from the last pose
noise_vector	Noise associated with the pose difference
before_pose_id	ID of the pose before the difference (optional)

Definition at line 183 of file graph_slam_instance.cpp.

Here is the call graph for this function:

◆ process_pose_difference() [3/3]

void GraphSLAMInstance::process_pose_difference	(	const Eigen::Vector3d &	pose_difference,
		const Eigen::Vector3d &	noise_vector,
		unsigned int	before_pose_id,
		unsigned int	after_pose_id
	)

Process a pose difference and add the respective factor to the graph.

Parameters

pose_difference	Pose difference from the last pose
noise_vector	Noise associated with the pose difference
before_pose_id	ID of the pose before the difference (optional)
after_pose_id	ID of the pose after the difference (optional)

Definition at line 166 of file graph_slam_instance.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ _factor_graph_

gtsam::NonlinearFactorGraph GraphSLAMInstance::_factor_graph_

protected

Definition at line 26 of file graph_slam_instance.hpp.

◆ _graph_values_

gtsam::Values GraphSLAMInstance::_graph_values_

protected

Definition at line 28 of file graph_slam_instance.hpp.

◆ _landmark_counter_

unsigned int GraphSLAMInstance::_landmark_counter_ = 0

protected

Definition at line 30 of file graph_slam_instance.hpp.

◆ _locked_landmarks_

bool GraphSLAMInstance::_locked_landmarks_ = false

protected

Definition at line 35 of file graph_slam_instance.hpp.

◆ _new_observation_factors_

bool GraphSLAMInstance::_new_observation_factors_

protected

Initial value:

=

false

Definition at line 33 of file graph_slam_instance.hpp.

◆ _new_pose_node_

bool GraphSLAMInstance::_new_pose_node_

protected

Initial value:

=

false

Definition at line 31 of file graph_slam_instance.hpp.

◆ _optimizer_

std::shared_ptr<BaseOptimizer> GraphSLAMInstance::_optimizer_

protected

Definition at line 38 of file graph_slam_instance.hpp.

◆ _params_

SLAMParameters GraphSLAMInstance::_params_

protected

Definition at line 37 of file graph_slam_instance.hpp.

◆ _pose_counter_

unsigned int GraphSLAMInstance::_pose_counter_ = 0

protected

Definition at line 29 of file graph_slam_instance.hpp.

◆ _pose_timestamps_

CircularBuffer<TimedPose> GraphSLAMInstance::_pose_timestamps_

protected

Definition at line 41 of file graph_slam_instance.hpp.

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

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

Classes

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ GraphSLAMInstance() [1/3]

◆ GraphSLAMInstance() [2/3]

◆ GraphSLAMInstance() [3/3]

◆ ~GraphSLAMInstance()

Member Function Documentation

◆ clone()

◆ get_covariance_matrix()

◆ get_graph_values_reference()

◆ get_landmark_counter()

◆ get_landmark_id_at_time()

◆ get_pose()

◆ get_pose_counter()

◆ get_state_vector()

◆ load_initial_state()

◆ lock_landmarks()

◆ new_observation_factors()

◆ new_pose_factors()

◆ operator=()

◆ optimize()

◆ process_new_pose()

◆ process_observations()

◆ process_pose_difference() [1/3]

◆ process_pose_difference() [2/3]

◆ process_pose_difference() [3/3]

Member Data Documentation

◆ _factor_graph_

◆ _graph_values_

◆ _landmark_counter_

◆ _locked_landmarks_

◆ _new_observation_factors_

◆ _new_pose_node_

◆ _optimizer_

◆ _params_

◆ _pose_counter_

◆ _pose_timestamps_