#include <decentralized_pgo.h>

Public Member Functions
	DecentralizedPGO (std::shared_ptr< rclcpp::Node > &node)
	Initialization of parameters and ROS 2 objects. More...

	~DecentralizedPGO ()

void	odometry_callback (const cslam_common_interfaces::msg::KeyframeOdom::ConstSharedPtr msg)
	Receives odometry msg + keyframe id. More...

void	inter_robot_loop_closure_callback (const cslam_common_interfaces::msg::InterRobotLoopClosure::ConstSharedPtr msg)
	Receives inter-robot loop closures. More...

void	intra_robot_loop_closure_callback (const cslam_common_interfaces::msg::IntraRobotLoopClosure::ConstSharedPtr msg)
	Receives intra-robot loop closures. More...

void	current_neighbors_callback (const cslam_common_interfaces::msg::RobotIdsAndOrigin::ConstSharedPtr msg)
	Receives current neighbors. More...

cslam_common_interfaces::msg::PoseGraph	fill_pose_graph_msg ()
	Fill a PoseGraph message with the local data. More...

cslam_common_interfaces::msg::PoseGraph	fill_pose_graph_msg (const cslam_common_interfaces::msg::RobotIds &msg)
	Fill a PoseGraph message with the local data. More...

void	get_pose_graph_callback (const cslam_common_interfaces::msg::RobotIds::ConstSharedPtr msg)
	Receives request for pose graph. More...

void	pose_graph_callback (const cslam_common_interfaces::msg::PoseGraph::ConstSharedPtr msg)
	Receives pose graph. More...

void	optimized_estimates_callback (const cslam_common_interfaces::msg::OptimizationResult::ConstSharedPtr msg)
	Receives estimates from the optimizer. More...

void	write_current_estimates_callback (const std_msgs::msg::String::ConstSharedPtr msg)
	Prints current estimates. More...

void	optimization_callback ()
	Starts pose graph optimization process every X ms (defined in config) More...

void	optimization_loop_callback ()
	Step execution of the optimization State Machine. More...

void	update_transform_to_origin (const gtsam::Pose3 &pose)
	Update transform to origin. More...

void	broadcast_tf_callback ()
	Broadcast tf to current origin point. More...

void	start_optimization ()
	Performs pose graph optimization. More...

void	resquest_current_neighbors ()
	Request to check the current neighbors. More...

void	reinitialize_received_pose_graphs ()
	Sets received pose graphs to false. More...

bool	check_received_pose_graphs ()
	Check if received all pose graphs. More...

void	start_waiting ()
	Start waiting. More...

bool	check_waiting_timeout ()
	Check waiting timeout. More...

void	end_waiting ()
	End waiting. More...

bool	is_waiting ()
	Check if waiting. More...

std::map< unsigned int, bool >	connected_robot_pose_graph ()
	Breadth First Seach to check connectivity. More...

std::pair< gtsam::NonlinearFactorGraph::shared_ptr, gtsam::Values::shared_ptr >	aggregate_pose_graphs ()
	Put all connected pose graphs into one. More...

void	share_optimized_estimates (const gtsam::Values &estimates)
	Shares optimizes estimates with the robots concerned. More...

void	heartbeat_timer_callback ()
	Publish heartbeat message periodically. More...

void	visualization_callback ()
	Publish heartbeat message periodically. More...

bool	is_optimizer ()
	Check if the local robot is the optimizer according to the specified priority rule Default priority rule: lowest ID. More...

gtsam::Values	optimize (const gtsam::NonlinearFactorGraph::shared_ptr &graph, const gtsam::Values::shared_ptr &initial)
	Pose graph optimization function. More...

void	check_result_and_finish_optimization ()
	Check if the optimization is finished. More...

Detailed Description

Definition at line 64 of file decentralized_pgo.h.

Constructor & Destructor Documentation

◆ DecentralizedPGO()

DecentralizedPGO::DecentralizedPGO ( std::shared_ptr< rclcpp::Node > & node )

Initialization of parameters and ROS 2 objects.

Parameters

node	ROS 2 node handle

Definition at line 9 of file decentralized_pgo.cpp.

Here is the call graph for this function:

◆ ~DecentralizedPGO()

cslam::DecentralizedPGO::~DecentralizedPGO ( )

inline

Definition at line 73 of file decentralized_pgo.h.

Member Function Documentation

◆ aggregate_pose_graphs()

std::pair< gtsam::NonlinearFactorGraph::shared_ptr, gtsam::Values::shared_ptr > DecentralizedPGO::aggregate_pose_graphs ( )

Put all connected pose graphs into one.

Definition at line 556 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ broadcast_tf_callback()

void DecentralizedPGO::broadcast_tf_callback ( )

Broadcast tf to current origin point.

Definition at line 755 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ check_received_pose_graphs()

bool DecentralizedPGO::check_received_pose_graphs ( )

Check if received all pose graphs.

Definition at line 223 of file decentralized_pgo.cpp.

Here is the caller graph for this function:

◆ check_result_and_finish_optimization()

void DecentralizedPGO::check_result_and_finish_optimization ( )

Check if the optimization is finished.

Definition at line 858 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ check_waiting_timeout()

bool DecentralizedPGO::check_waiting_timeout ( )

Check waiting timeout.

Definition at line 533 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ connected_robot_pose_graph()

std::map< unsigned int, bool > DecentralizedPGO::connected_robot_pose_graph ( )

Breadth First Seach to check connectivity.

Definition at line 464 of file decentralized_pgo.cpp.

Here is the caller graph for this function:

◆ current_neighbors_callback()

void DecentralizedPGO::current_neighbors_callback ( const cslam_common_interfaces::msg::RobotIdsAndOrigin::ConstSharedPtr msg )

Receives current neighbors.

Parameters

msg

Definition at line 332 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ end_waiting()

void DecentralizedPGO::end_waiting ( )

End waiting.

Definition at line 529 of file decentralized_pgo.cpp.

Here is the caller graph for this function:

◆ fill_pose_graph_msg() [1/2]

cslam_common_interfaces::msg::PoseGraph DecentralizedPGO::fill_pose_graph_msg ( )

Fill a PoseGraph message with the local data.

Returns: cslam_common_interfaces::msg::PoseGraph

Definition at line 370 of file decentralized_pgo.cpp.

Here is the caller graph for this function:

◆ fill_pose_graph_msg() [2/2]

cslam_common_interfaces::msg::PoseGraph DecentralizedPGO::fill_pose_graph_msg ( const cslam_common_interfaces::msg::RobotIds & msg )

Fill a PoseGraph message with the local data.

Returns: cslam_common_interfaces::msg::PoseGraph

Definition at line 376 of file decentralized_pgo.cpp.

Here is the call graph for this function:

◆ get_pose_graph_callback()

void DecentralizedPGO::get_pose_graph_callback ( const cslam_common_interfaces::msg::RobotIds::ConstSharedPtr msg )

Receives request for pose graph.

Parameters

msg

Definition at line 430 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ heartbeat_timer_callback()

void DecentralizedPGO::heartbeat_timer_callback ( )

Publish heartbeat message periodically.

Definition at line 683 of file decentralized_pgo.cpp.

Here is the caller graph for this function:

◆ inter_robot_loop_closure_callback()

void DecentralizedPGO::inter_robot_loop_closure_callback ( const cslam_common_interfaces::msg::InterRobotLoopClosure::ConstSharedPtr msg )

Receives inter-robot loop closures.

Parameters

msg

Definition at line 290 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ intra_robot_loop_closure_callback()

void DecentralizedPGO::intra_robot_loop_closure_callback ( const cslam_common_interfaces::msg::IntraRobotLoopClosure::ConstSharedPtr msg )

Receives intra-robot loop closures.

Parameters

msg

Definition at line 268 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ is_optimizer()

bool DecentralizedPGO::is_optimizer ( )

Check if the local robot is the optimizer according to the specified priority rule Default priority rule: lowest ID.

Returns: true; false

Definition at line 347 of file decentralized_pgo.cpp.

Here is the caller graph for this function:

◆ is_waiting()

bool DecentralizedPGO::is_waiting ( )

Check if waiting.

Definition at line 531 of file decentralized_pgo.cpp.

Here is the caller graph for this function:

◆ odometry_callback()

void DecentralizedPGO::odometry_callback ( const cslam_common_interfaces::msg::KeyframeOdom::ConstSharedPtr msg )

Receives odometry msg + keyframe id.

Parameters

msg

Definition at line 233 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ optimization_callback()

void DecentralizedPGO::optimization_callback ( )

Starts pose graph optimization process every X ms (defined in config)

Definition at line 544 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ optimization_loop_callback()

void DecentralizedPGO::optimization_loop_callback ( )

Step execution of the optimization State Machine.

Definition at line 884 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ optimize()

gtsam::Values DecentralizedPGO::optimize	(	const gtsam::NonlinearFactorGraph::shared_ptr &	graph,
		const gtsam::Values::shared_ptr &	initial
	)

Pose graph optimization function.

Parameters

graph	pose graph
initial	initial values

Returns: gtsam::Values optimized values

Definition at line 797 of file decentralized_pgo.cpp.

Here is the caller graph for this function:

◆ optimized_estimates_callback()

void DecentralizedPGO::optimized_estimates_callback ( const cslam_common_interfaces::msg::OptimizationResult::ConstSharedPtr msg )

Receives estimates from the optimizer.

Parameters

msg

Definition at line 636 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ pose_graph_callback()

void DecentralizedPGO::pose_graph_callback ( const cslam_common_interfaces::msg::PoseGraph::ConstSharedPtr msg )

Receives pose graph.

Parameters

msg

Definition at line 438 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reinitialize_received_pose_graphs()

void DecentralizedPGO::reinitialize_received_pose_graphs ( )

Sets received pose graphs to false.

Definition at line 213 of file decentralized_pgo.cpp.

Here is the caller graph for this function:

◆ resquest_current_neighbors()

void DecentralizedPGO::resquest_current_neighbors ( )

Request to check the current neighbors.

Definition at line 510 of file decentralized_pgo.cpp.

Here is the caller graph for this function:

◆ share_optimized_estimates()

void DecentralizedPGO::share_optimized_estimates ( const gtsam::Values & estimates )

Shares optimizes estimates with the robots concerned.

Definition at line 665 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ start_optimization()

void DecentralizedPGO::start_optimization ( )

Performs pose graph optimization.

Definition at line 829 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ start_waiting()

void DecentralizedPGO::start_waiting ( )

Start waiting.

Definition at line 515 of file decentralized_pgo.cpp.

Here is the caller graph for this function:

◆ update_transform_to_origin()

void DecentralizedPGO::update_transform_to_origin ( const gtsam::Pose3 & pose )

Update transform to origin.

Definition at line 731 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ visualization_callback()

void DecentralizedPGO::visualization_callback ( )

Publish heartbeat message periodically.

Definition at line 696 of file decentralized_pgo.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ write_current_estimates_callback()

void DecentralizedPGO::write_current_estimates_callback ( const std_msgs::msg::String::ConstSharedPtr msg )

Prints current estimates.

Parameters

msg

Definition at line 322 of file decentralized_pgo.cpp.

Here is the caller graph for this function:

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

/home/lajoiepy/Documents/phd/projects/C-SLAM/Swarm-SLAM/src/cslam/include/cslam/back_end/decentralized_pgo.h
/home/lajoiepy/Documents/phd/projects/C-SLAM/Swarm-SLAM/src/cslam/src/back_end/decentralized_pgo.cpp

Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ DecentralizedPGO()

◆ ~DecentralizedPGO()

Member Function Documentation

◆ aggregate_pose_graphs()

◆ broadcast_tf_callback()

◆ check_received_pose_graphs()

◆ check_result_and_finish_optimization()

◆ check_waiting_timeout()

◆ connected_robot_pose_graph()

◆ current_neighbors_callback()

◆ end_waiting()

◆ fill_pose_graph_msg() [1/2]

◆ fill_pose_graph_msg() [2/2]

◆ get_pose_graph_callback()

◆ heartbeat_timer_callback()

◆ inter_robot_loop_closure_callback()

◆ intra_robot_loop_closure_callback()

◆ is_optimizer()

◆ is_waiting()

◆ odometry_callback()

◆ optimization_callback()

◆ optimization_loop_callback()

◆ optimize()

◆ optimized_estimates_callback()

◆ pose_graph_callback()

◆ reinitialize_received_pose_graphs()

◆ resquest_current_neighbors()

◆ share_optimized_estimates()

◆ start_optimization()

◆ start_waiting()

◆ update_transform_to_origin()

◆ visualization_callback()

◆ write_current_estimates_callback()