mrpt::graphslam::CGraphSlamEngine derived class for executing multi-robot graphSLAM More...

#include <CGraphSlamEngine_MR.h>

Inheritance diagram for mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >:

Classes
struct	TNeighborAgentProps
	Struct responsible for holding properties (nodeIDs, node positions, LaserScans) that have been registered by a nearby GraphSlamAgent. More...

struct	TOptions

Public Types
typedef GRAPH_T::constraint_t	constraint_t

typedef mrpt::graphslam::deciders::CEdgeRegistrationDecider_MR< GRAPH_T >	edge_reg_mr_t

typedef GRAPH_T::global_pose_t	global_pose_t

typedef mrpt::graphs::detail::THypothesis< GRAPH_T >	hypot_t

typedef std::vector< hypot_t >	hypots_t

typedef std::vector< hypot_t * >	hypotsp_t

typedef std::pair< TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr >	MRPT_NodeIDWithLaserScan

typedef std::vector< TNeighborAgentProps * >	neighbors_t

typedef mrpt::graphslam::detail::TNodeProps< GRAPH_T >	node_props_t

typedef std::map< TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr >	nodes_to_scans2D_t

typedef CGraphSlamEngine_ROS< GRAPH_T >	parent_t

typedef std::vector< std::vector< TNodeID > >	partitions_t

typedef mrpt::graphslam::TUncertaintyPath< GRAPH_T >	path_t

typedef std::vector< path_t >	paths_t

typedef constraint_t::type_value	pose_t

typedef CGraphSlamEngine_MR< GRAPH_T >	self_t

Public Types inherited from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >
typedef CGraphSlamEngine< GRAPH_T >	parent

Public Types inherited from mrpt::graphslam::CGraphSlamEngine< GRAPH_T >
typedef GRAPH_T::constraint_t	constraint_t

typedef GRAPH_T::constraint_t	constraint_t

typedef std::map< std::string, mrpt::utils::CFileOutputStream * >	fstreams_out

typedef std::map< std::string, mrpt::utils::CFileOutputStream * >	fstreams_out

typedef std::map< std::string, mrpt::utils::CFileOutputStream * >::iterator	fstreams_out_it

typedef std::map< std::string, mrpt::utils::CFileOutputStream * >::iterator	fstreams_out_it

typedef GRAPH_T::global_pose_t	global_pose_t

typedef GRAPH_T::global_pose_t	global_pose_t

typedef std::map< mrpt::utils::TNodeID, mrpt::obs::CObservation2DRangeScanPtr >	nodes_to_scans2D_t

typedef std::map< mrpt::utils::TNodeID, mrpt::obs::CObservation2DRangeScanPtr >	nodes_to_scans2D_t

typedef GRAPH_T::constraint_t::type_value	pose_t

typedef GRAPH_T::constraint_t::type_value	pose_t

Public Member Functions
bool	_execGraphSlamStep (mrpt::obs::CActionCollection::Ptr &action, mrpt::obs::CSensoryFrame::Ptr &observations, mrpt::obs::CObservation::Ptr &observation, size_t &rawlog_entry)

	CGraphSlamEngine_MR (ros::NodeHandle nh, const std::string &config_file, const std::string &rawlog_fname="", const std::string &fname_GT="", mrpt::graphslam::CWindowManager win_manager=NULL, mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > node_reg=NULL, mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > edge_reg=NULL, mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > *optimizer=NULL)

const neighbors_t &	getVecOfNeighborAgentProps () const

void	initClass ()

bool	isOwnNodeID (const TNodeID nodeID, const global_pose_t *pose_out=NULL) const
	Return true if current CGraphSlamEngine_MR object initially registered this nodeID, false otherwise. More...

	~CGraphSlamEngine_MR ()

Public Member Functions inherited from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >
	CGraphSlamEngine_ROS (ros::NodeHandle nh, const std::string &config_file, const std::string &rawlog_fname="", const std::string &fname_GT="", mrpt::graphslam::CWindowManager win_manager=NULL, mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > node_reg=NULL, mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > edge_reg=NULL, mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > *optimizer=NULL)

void	initClass ()
	Initialize object instance. More...

void	setupComm ()
	Wrapper method around the protected setup* class methods. More...

virtual	~CGraphSlamEngine_ROS ()

Public Member Functions inherited from mrpt::graphslam::CGraphSlamEngine< GRAPH_T >
virtual bool	_execGraphSlamStep (mrpt::obs::CActionCollectionPtr &action, mrpt::obs::CSensoryFramePtr &observations, mrpt::obs::CObservationPtr &observation, size_t &rawlog_entry)

	CGraphSlamEngine (const std::string &config_file, const std::string &rawlog_fname="", const std::string &fname_GT="", mrpt::graphslam::CWindowManager win_manager=NULL, mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > node_reg=NULL, mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > edge_reg=NULL, mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > optimizer=NULL)

void	computeMap () const

void	computeMap () const

bool	execGraphSlamStep (mrpt::obs::CObservationPtr &observation, size_t &rawlog_entry)

void	generateReportFiles (const std::string &output_dir_fname_in)

global_pose_t	getCurrentRobotPosEstimation () const

void	getDeformationEnergyVector (std::vector< double > *vec_out) const

const GRAPH_T &	getGraph () const

bool	getGraphSlamStats (std::map< std::string, int > node_stats, std::map< std::string, int > edge_stats, mrpt::system::TTimeStamp *timestamp=NULL)

void	getMap (mrpt::maps::COccupancyGridMap2DPtr map, mrpt::system::TTimeStamp *acquisition_time=NULL) const

void	getMap (mrpt::maps::COctoMapPtr map, mrpt::system::TTimeStamp *acquisition_time=NULL) const

void	getMap (mrpt::maps::COccupancyGridMap2DPtr map, mrpt::system::TTimeStamp *acquisition_time=NULL) const

void	getMap (mrpt::maps::COctoMapPtr map, mrpt::system::TTimeStamp *acquisition_time=NULL) const

virtual void	getNodeIDsOfEstimatedTrajectory (std::set< mrpt::utils::TNodeID > *nodes_set) const

void	getParamsAsString (std::string *params_out) const

std::string	getParamsAsString () const

std::string	getRawlogFname ()

bool	isPaused () const

bool	isPaused () const

void	loadParams (const std::string &fname)

void	pauseExec ()

void	pauseExec ()

void	printParams () const

void	resumeExec () const

void	resumeExec () const

void	save3DScene (const std::string *fname_in=NULL) const

void	saveGraph (const std::string *fname_in=NULL) const

void	togglePause ()

void	togglePause ()

virtual	~CGraphSlamEngine ()

Private Member Functions
bool	addNodeBatchesFromAllNeighbors ()
	Traverse all neighbors for which I know the inter-graph transformation, run addNodeBatchFromNeighbor. More...

bool	addNodeBatchFromNeighbor (TNeighborAgentProps *neighbor)
	neighbors for which I know the inter-graph transformation, add new batches of fetches nodeIDs and scans More...

bool	findTFsWithAllNeighbors ()
	Using map-merging find a potentuial transofrmation between own graph map to another agent's map and using that transformation add other agent's nodes to own graph. More...

bool	findTFWithNeighbor (TNeighborAgentProps *neighbor)
	Method is used only when I haven't found any transformation between mine and the current neighbor's graph. When I do find one, I just add every new nodeID + LS directly in own graph since I can now connect the new ones with the already integrated nodes of the neighbor. More...

void	getAllOwnNodes (std::set< TNodeID > *nodes_set) const
	Fill given set with the nodeIDs that were initially registered by the current graphSLAM engine (and not by any neighboring agent. More...

bool	getCMGraph (mrpt_msgs::GetCMGraph::Request &req, mrpt_msgs::GetCMGraph::Response &res)
	Compute and fill the Condensed Measurements Graph. More...

mrpt::poses::CPose3D	getLSPoseForGridMapVisualization (const TNodeID nodeID) const

bool	getNeighborByAgentID (const std::string &agent_ID_str, TNeighborAgentProps *&neighbor) const
	Fill the TNeighborAgentProps instance based on the given agent_ID_str string that is provided. More...

void	getNodeIDsOfEstimatedTrajectory (std::set< TNodeID > *nodes_set) const

void	getRobotEstimatedTrajectory (typename GRAPH_T::global_poses_t *graph_poses) const

void	monitorNodeRegistration (bool registered=false, std::string class_name="Class")
	Overriden method that takes in account registration of multiple nodes of other running graphSLAM agents. More...

void	printParams () const

bool	pubLastRegdIDScan ()
	Update the last registered NodeID and LaserScan of the current agent. More...

bool	pubUpdatedNodesList ()
	Update the last registered NodeIDs and corresponding positions of the current agent. More...

void	readParams ()

void	readROSParameters ()

void	setObjectPropsFromNodeID (const TNodeID nodeID, mrpt::opengl::CSetOfObjects::Ptr &viz_object)

void	setupPubs ()

void	setupSrvs ()

void	setupSubs ()

void	usePublishersBroadcasters ()
	Provide feedback about the SLAM operation. More...

Private Attributes
ros::AsyncSpinner	cm_graph_async_spinner
	AsyncSpinner that is used to query the CM-Graph service in case a new request arrives. More...

mrpt::slam::CGridMapAligner::TConfigParams	m_alignment_options
	Parameters used during the alignment operation. More...

mrpt::graphslam::detail::CConnectionManager	m_conn_manager
	CConnectionManager instance. More...

size_t	m_graph_nodes_last_size
	Last known size of the m_nodes map. More...

TColorManager	m_neighbor_colors_manager

std::map< TNeighborAgentProps *, bool >	m_neighbor_to_found_initial_tf
	Map from neighbor instance to transformation from own to neighbor's graph. More...

neighbors_t	m_neighbors
	GraphSlamAgent instance pointer to TNeighborAgentProps. More...

ros::NodeHandle *	m_nh
	NodeHandle pointer - inherited by the parent. Redefined here for convenience. More...

size_t	m_nodes_to_laser_scans2D_last_size
	Last known size of the m_nodes_to_laser_scans2D map. More...

mrpt::graphslam::CGraphSlamEngine_MR::TOptions	m_opts

bool	m_registered_multiple_nodes
	Indicates whether multiple nodes were just registered. Used for checking correct node registration in the monitorNodeRgistration method. More...

std::string	m_sec_alignment_params

std::string	m_sec_mr_slam_params

Subscribers - Publishers
ROS Topic Subscriber/Publisher/Service instances
ros::Publisher	m_list_neighbors_pub

ros::Publisher	m_last_regd_id_scan_pub
	Publisher of the laserScan + the corresponding (last) registered node. More...

ros::Publisher	m_last_regd_nodes_pub
	Publisher of the last registered nodeIDs and positions. More...

ros::ServiceServer	m_cm_graph_srvserver

Topic Names
Names of the topics that the class instance subscribes or publishes to
std::string	m_mr_ns
	Condensed Measurements topic namespace. More...

std::string	m_list_neighbors_topic
	Name of topic at which we publish information about the agents that we can currently communicate with. More...

std::string	m_last_regd_id_scan_topic
	Name of the topic that the last registered laser scan (+ corresponding nodeID) is published at. More...

std::string	m_last_regd_nodes_topic
	Name of the topic that the last X registered nodes + positions are going to be published at. More...

std::string	m_cm_graph_service
	Name of the server which is to be called when other agent wants to have a subgraph of certain nodes returned. More...


double	m_offset_y_nrd
	Display the Deciders/Optimizers with which we are running as well as the namespace of the current agent. More...

double	m_offset_y_erd

double	m_offset_y_gso

double	m_offset_y_namespace

int	m_text_index_nrd

int	m_text_index_erd

int	m_text_index_gso

int	m_text_index_namespace

Additional Inherited Members
Static Public Member Functions inherited from mrpt::graphslam::CGraphSlamEngine< GRAPH_T >
static void	readGTFile (const std::string &fname_GT, std::vector< mrpt::poses::CPose2D > gt_poses, std::vector< mrpt::system::TTimeStamp > gt_timestamps=NULL)

static void	readGTFile (const std::string &fname_GT, std::vector< mrpt::poses::CPose3D > gt_poses, std::vector< mrpt::system::TTimeStamp > gt_timestamps=NULL)

static void	readGTFile (const std::string &fname_GT, std::vector< mrpt::poses::CPose3D > gt_poses, std::vector< mrpt::system::TTimeStamp > gt_timestamps=NULL)

static void	readGTFile (const std::string &fname_GT, std::vector< mrpt::poses::CPose2D > gt_poses, std::vector< mrpt::system::TTimeStamp > gt_timestamps=NULL)

static void	readGTFileRGBD_TUM (const std::string &fname_GT, std::vector< mrpt::poses::CPose2D > gt_poses, std::vector< mrpt::system::TTimeStamp > gt_timestamps=NULL)

static void	readGTFileRGBD_TUM (const std::string &fname_GT, std::vector< mrpt::poses::CPose2D > gt_poses, std::vector< mrpt::system::TTimeStamp > gt_timestamps=NULL)

Public Attributes inherited from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >
ros::NodeHandle *	m_nh

Protected Member Functions inherited from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >
void	readParams ()
	Read the problem configuration parameters. More...

void	readROSParameters ()
	Read configuration parameters from the ROS parameter server. More...

Protected Member Functions inherited from mrpt::graphslam::CGraphSlamEngine< GRAPH_T >
void	alignOpticalWithMRPTFrame ()

void	computeSlamMetric (mrpt::utils::TNodeID nodeID, size_t gt_index)

void	decimateLaserScan (mrpt::obs::CObservation2DRangeScan &laser_scan_in, mrpt::obs::CObservation2DRangeScan *laser_scan_out, const int keep_every_n_entries=2)

void	dumpVisibilityErrorMsg (std::string viz_flag, int sleep_time=500)

void	execDijkstraNodesEstimation ()

void	getDescriptiveReport (std::string *report_str) const

void	initClass ()

void	initCurrPosViewport ()

void	initEstimatedTrajectoryVisualization ()

void	initGTVisualization ()

void	initIntensityImageViewport ()

void	initIntensityImageViewport ()

void	initMapVisualization ()

void	initMapVisualization ()

void	initOdometryVisualization ()

void	initRangeImageViewport ()

void	initRangeImageViewport ()

void	initResultsFile (const std::string &fname)

mrpt::opengl::CSetOfObjectsPtr	initRobotModelVisualization ()

mrpt::opengl::CSetOfObjectsPtr	initRobotModelVisualization ()

mrpt::opengl::CSetOfObjectsPtr	initRobotModelVisualizationInternal (const mrpt::poses::CPose2D &p_unused)

mrpt::opengl::CSetOfObjectsPtr	initRobotModelVisualizationInternal (const mrpt::poses::CPose2D &p_unused)

mrpt::opengl::CSetOfObjectsPtr	initRobotModelVisualizationInternal (const mrpt::poses::CPose3D &p_unused)

mrpt::opengl::CSetOfObjectsPtr	initRobotModelVisualizationInternal (const mrpt::poses::CPose3D &p_unused)

void	initSlamMetricVisualization ()

void	initVisualization ()

void	initVisualization ()

void	queryObserverForEvents ()

mrpt::opengl::CSetOfObjectsPtr	setCurrentPositionModel (const std::string &model_name, const mrpt::utils::TColor &model_color=mrpt::utils::TColor(0, 0, 0), const size_t model_size=1, const pose_t &init_pose=pose_t())

virtual void	setObjectPropsFromNodeID (const mrpt::utils::TNodeID nodeID, mrpt::opengl::CSetOfObjectsPtr &viz_object)

virtual void	setObjectPropsFromNodeID (const mrpt::utils::TNodeID nodeID, mrpt::opengl::CSetOfObjectsPtr &viz_object)

void	toggleEstimatedTrajectoryVisualization ()

void	toggleEstimatedTrajectoryVisualization ()

void	toggleGTVisualization ()

void	toggleGTVisualization ()

void	toggleMapVisualization ()

void	toggleMapVisualization ()

void	toggleOdometryVisualization ()

void	toggleOdometryVisualization ()

void	updateAllVisuals ()

void	updateAllVisuals ()

virtual void	updateCurrPosViewport ()

virtual void	updateCurrPosViewport ()

void	updateEstimatedTrajectoryVisualization (bool full_update=false)

void	updateEstimatedTrajectoryVisualization (bool full_update=false)

void	updateGTVisualization ()

void	updateGTVisualization ()

void	updateIntensityImageViewport ()

void	updateIntensityImageViewport ()

void	updateMapVisualization (const std::map< mrpt::utils::TNodeID, mrpt::obs::CObservation2DRangeScanPtr > &nodes_to_laser_scans2D, bool full_update=false)

void	updateMapVisualization (const std::map< mrpt::utils::TNodeID, mrpt::obs::CObservation2DRangeScanPtr > &nodes_to_laser_scans2D, bool full_update=false)

void	updateOdometryVisualization ()

void	updateOdometryVisualization ()

void	updateRangeImageViewport ()

void	updateRangeImageViewport ()

void	updateSlamMetricVisualization ()

void	updateSlamMetricVisualization ()

Static Protected Member Functions inherited from mrpt::graphslam::CGraphSlamEngine< GRAPH_T >
static double	accumulateAngleDiffs (const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)

static double	accumulateAngleDiffs (const mrpt::poses::CPose3D &p1, const mrpt::poses::CPose3D &p2)

static double	accumulateAngleDiffs (const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)

static double	accumulateAngleDiffs (const mrpt::poses::CPose3D &p1, const mrpt::poses::CPose3D &p2)

static mrpt::system::TTimeStamp	getTimeStamp (const mrpt::obs::CActionCollectionPtr action, const mrpt::obs::CSensoryFramePtr observations, const mrpt::obs::CObservationPtr observation)

Protected Attributes inherited from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >
ros::CallbackQueue	custom_service_queue
	Custom Callback queue for processing requests for the services outside the standard CallbackQueue. More...

int	m_queue_size

Protected Attributes inherited from mrpt::graphslam::CGraphSlamEngine< GRAPH_T >
std::string	m_class_name

std::string	m_config_fname

double	m_curr_deformation_energy

pose_t	m_curr_odometry_only_pose

mrpt::system::TTimeStamp	m_curr_timestamp

std::string	m_current_constraint_type

mrpt::utils::TColor	m_current_constraint_type_color

double	m_dataset_grab_time

std::vector< double >	m_deformation_energy_vec

mrpt::graphslam::detail::CEdgeCounter	m_edge_counter

mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > *	m_edge_reg

bool	m_enable_curr_pos_viewport

bool	m_enable_intensity_viewport

bool	m_enable_range_viewport

const bool	m_enable_visuals

mrpt::utils::TColor	m_estimated_traj_color

mrpt::obs::CObservation2DRangeScanPtr	m_first_laser_scan2D

std::string	m_fname_GT

GRAPH_T	m_graph

mrpt::synch::CCriticalSection	m_graph_section

mrpt::maps::COccupancyGridMap2DPtr	m_gridmap_cached

mrpt::utils::TColor	m_GT_color

std::string	m_GT_file_format

std::vector< pose_t >	m_GT_poses

size_t	m_GT_poses_index

size_t	m_GT_poses_step

bool	m_has_read_config

std::string	m_img_external_storage_dir

std::string	m_img_prev_path_base

struct mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams	m_info_params

mrpt::system::TTimeStamp	m_init_timestamp

bool	m_is_first_time_node_reg

bool	m_is_paused

std::string	m_keystroke_estimated_trajectory

std::string	m_keystroke_GT

std::string	m_keystroke_map

std::string	m_keystroke_odometry

std::string	m_keystroke_pause_exec

mrpt::obs::CObservation2DRangeScanPtr	m_last_laser_scan2D

mrpt::obs::CObservation3DRangeScanPtr	m_last_laser_scan3D

mrpt::system::TTimeStamp	m_map_acq_time

bool	m_map_is_cached

mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > *	m_node_reg

mrpt::utils::TNodeID	m_nodeID_max

std::map< mrpt::utils::TNodeID, size_t >	m_nodeID_to_gt_indices

nodes_to_scans2D_t	m_nodes_to_laser_scans2D

bool	m_observation_only_dataset

mrpt::maps::COctoMapPtr	m_octomap_cached

mrpt::utils::TColor	m_odometry_color

std::vector< pose_t >	m_odometry_poses

double	m_offset_x_left

double	m_offset_y_current_constraint_type

double	m_offset_y_estimated_traj

double	m_offset_y_GT

double	m_offset_y_odometry

double	m_offset_y_paused_message

double	m_offset_y_timestamp

mrpt::utils::TColor	m_optimized_map_color

mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > *	m_optimizer

fstreams_out	m_out_streams

const std::string	m_paused_message

std::string	m_rawlog_fname

bool	m_request_to_exit

size_t	m_robot_model_size

mrpt::math::CMatrixDouble33	m_rot_TUM_to_MRPT

mrpt::maps::CSimpleMap	m_simple_map_cached

std::vector< std::string >	m_supported_constraint_types

int	m_text_index_current_constraint_type

int	m_text_index_estimated_traj

int	m_text_index_GT

int	m_text_index_odometry

int	m_text_index_paused_message

int	m_text_index_timestamp

mrpt::utils::CTimeLogger	m_time_logger

bool	m_use_GT

bool	m_user_decides_about_output_dir

bool	m_visualize_estimated_trajectory

bool	m_visualize_GT

bool	m_visualize_map

bool	m_visualize_odometry_poses

bool	m_visualize_SLAM_metric

mrpt::gui::CDisplayWindow3D *	m_win

mrpt::graphslam::CWindowManager *	m_win_manager

mrpt::graphslam::CWindowObserver *	m_win_observer

mrpt::gui::CDisplayWindowPlots *	m_win_plot

Static Protected Attributes inherited from mrpt::graphslam::CGraphSlamEngine< GRAPH_T >
static const std::string	header_sep

static const std::string	report_sep

Detailed Description

template<class GRAPH_T>
class mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >

mrpt::graphslam::CGraphSlamEngine derived class for executing multi-robot graphSLAM

Definition at line 67 of file CGraphSlamEngine_MR.h.

Member Typedef Documentation

template<class GRAPH_T>

typedef GRAPH_T::constraint_t mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::constraint_t

Definition at line 72 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef mrpt::graphslam::deciders::CEdgeRegistrationDecider_MR<GRAPH_T> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::edge_reg_mr_t

Definition at line 88 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef GRAPH_T::global_pose_t mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::global_pose_t

Definition at line 84 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef mrpt::graphs::detail::THypothesis<GRAPH_T> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::hypot_t

Definition at line 81 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef std::vector<hypot_t> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::hypots_t

Definition at line 82 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef std::vector<hypot_t*> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::hypotsp_t

Definition at line 83 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef std::pair< TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::MRPT_NodeIDWithLaserScan

Definition at line 76 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef std::vector<TNeighborAgentProps*> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::neighbors_t

Definition at line 287 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef mrpt::graphslam::detail::TNodeProps<GRAPH_T> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::node_props_t

Definition at line 85 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef std::map< TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::nodes_to_scans2D_t

Definition at line 79 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef CGraphSlamEngine_ROS<GRAPH_T> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::parent_t

Definition at line 70 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef std::vector<std::vector<TNodeID> > mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::partitions_t

Definition at line 80 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef mrpt::graphslam::TUncertaintyPath<GRAPH_T> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::path_t

Definition at line 86 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef std::vector<path_t> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::paths_t

Definition at line 87 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef constraint_t::type_value mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::pose_t

Definition at line 73 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

typedef CGraphSlamEngine_MR<GRAPH_T> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::self_t

Definition at line 71 of file CGraphSlamEngine_MR.h.

Constructor & Destructor Documentation

template<class GRAPH_T >

mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::CGraphSlamEngine_MR	(	ros::NodeHandle *	nh,
		const std::string &	config_file,
		const std::string &	rawlog_fname = `""`,
		const std::string &	fname_GT = `""`,
		mrpt::graphslam::CWindowManager *	win_manager = `NULL`,
		mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > *	node_reg = `NULL`,
		mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > *	edge_reg = `NULL`,
		mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > *	optimizer = `NULL`
	)

Definition at line 29 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::~CGraphSlamEngine_MR ( )

Definition at line 51 of file CGraphSlamEngine_MR_impl.h.

Member Function Documentation

template<class GRAPH_T >

bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::_execGraphSlamStep	(	mrpt::obs::CActionCollection::Ptr &	action,
		mrpt::obs::CSensoryFrame::Ptr &	observations,
		mrpt::obs::CObservation::Ptr &	observation,
		size_t &	rawlog_entry
	)

virtual

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 434 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::addNodeBatchesFromAllNeighbors ( )

private

Traverse all neighbors for which I know the inter-graph transformation, run addNodeBatchFromNeighbor.

Definition at line 63 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::addNodeBatchFromNeighbor ( TNeighborAgentProps * neighbor )

private

neighbors for which I know the inter-graph transformation, add new batches of fetches nodeIDs and scans

Try to integrate the newly received nodeIDs and laser Scans in own graph as a batch

Note: Batch integration (instead of individual nodeID + LaserScan every time a new one is received) is preferred since the neighbor is given time to optimize the node positions

Definition at line 87 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::findTFsWithAllNeighbors ( )

private

Using map-merging find a potentuial transofrmation between own graph map to another agent's map and using that transformation add other agent's nodes to own graph.

Method delegates task to the findMatchesWithNeighboor method.

Returns: True on successful graph integration with any neighbor, false otherwise

Definition at line 210 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::findTFWithNeighbor ( TNeighborAgentProps * neighbor )

private

Method is used only when I haven't found any transformation between mine and the current neighbor's graph. When I do find one, I just add every new nodeID + LS directly in own graph since I can now connect the new ones with the already integrated nodes of the neighbor.

See also: findTFsWithAllNeighbors

Definition at line 253 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getAllOwnNodes ( std::set< TNodeID > * nodes_set ) const

private

Fill given set with the nodeIDs that were initially registered by the current graphSLAM engine (and not by any neighboring agent.

Definition at line 980 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getCMGraph	(	mrpt_msgs::GetCMGraph::Request &	req,
		mrpt_msgs::GetCMGraph::Response &	res
	)

private

Compute and fill the Condensed Measurements Graph.

Definition at line 900 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

mrpt::poses::CPose3D mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getLSPoseForGridMapVisualization ( const TNodeID nodeID ) const

privatevirtual

Reimplemented from mrpt::graphslam::CGraphSlamEngine< GRAPH_T >.

Definition at line 593 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getNeighborByAgentID	(	const std::string &	agent_ID_str,
		TNeighborAgentProps *&	neighbor
	)		const

private

Fill the TNeighborAgentProps instance based on the given agent_ID_str string that is provided.

Returns: False if the agent_ID_str doesn't correspond to any registered TNeighborAgentProps, True otherwise

Definition at line 621 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getNodeIDsOfEstimatedTrajectory ( std::set< TNodeID > * nodes_set ) const

private

Definition at line 993 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getRobotEstimatedTrajectory ( typename GRAPH_T::global_poses_t * graph_poses ) const

privatevirtual

Reimplemented from mrpt::graphslam::CGraphSlamEngine< GRAPH_T >.

Definition at line 965 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T>

const neighbors_t& mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getVecOfNeighborAgentProps ( ) const

inline

Definition at line 289 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T >

void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::initClass ( )

Definition at line 469 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::isOwnNodeID	(	const TNodeID	nodeID,
		const global_pose_t *	pose_out = `NULL`
	)		const

Return true if current CGraphSlamEngine_MR object initially registered this nodeID, false otherwise.

Parameters

[in] nodeID nodeID for which the query is made. runtime_error In case given nodeID isn't registered at all in the graph

Definition at line 802 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::monitorNodeRegistration	(	bool	registered = `false`,
		std::string	class_name = `"Class"`
	)

privatevirtual

Overriden method that takes in account registration of multiple nodes of other running graphSLAM agents.

Reimplemented from mrpt::graphslam::CGraphSlamEngine< GRAPH_T >.

Definition at line 948 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::printParams ( ) const

private

Definition at line 859 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::pubLastRegdIDScan ( )

private

Update the last registered NodeID and LaserScan of the current agent.

Note: Method is automatically called from usePublishersBroadcasters, but it publishes to the corresponding topics only on new node/LS additions

Returns: true on publication to corresponding topics, false otherwise

See also: pubUpdatedNodesList

Definition at line 755 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::pubUpdatedNodesList ( )

private

Update the last registered NodeIDs and corresponding positions of the current agent.

Note: Method is automatically called from usePublishersBroadcasters, but it publishes to the corresponding topics only on new node additions. Node positions may change due to optimization but we will be notified of this change anyway after a new node addition.

Returns: true on publication to corresponding topics, false otherwise

See also: pubLastRegdIDScan

Definition at line 697 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::readParams ( )

private

Definition at line 834 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::readROSParameters ( )

private

Definition at line 852 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::setObjectPropsFromNodeID	(	const TNodeID	nodeID,
		mrpt::opengl::CSetOfObjects::Ptr &	viz_object
	)

private

Definition at line 924 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::setupPubs ( )

privatevirtual

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 867 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::setupSrvs ( )

privatevirtual

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 887 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::setupSubs ( )

privatevirtual

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 865 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >

void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::usePublishersBroadcasters ( )

privatevirtual

Provide feedback about the SLAM operation.

Method makes the necessary calls to all the publishers of the class and broadcaster instances

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 641 of file CGraphSlamEngine_MR_impl.h.

Member Data Documentation

template<class GRAPH_T>

ros::AsyncSpinner mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::cm_graph_async_spinner

private

AsyncSpinner that is used to query the CM-Graph service in case a new request arrives.

Definition at line 508 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

mrpt::slam::CGridMapAligner::TConfigParams mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_alignment_options

private

Parameters used during the alignment operation.

Definition at line 512 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_cm_graph_service

private

Name of the server which is to be called when other agent wants to have a subgraph of certain nodes returned.

Definition at line 466 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

ros::ServiceServer mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_cm_graph_srvserver

private

Definition at line 439 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

mrpt::graphslam::detail::CConnectionManager mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_conn_manager

private

CConnectionManager instance.

Definition at line 474 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

size_t mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_graph_nodes_last_size

private

Last known size of the m_nodes map.

Definition at line 482 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

ros::Publisher mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_last_regd_id_scan_pub

private

Publisher of the laserScan + the corresponding (last) registered node.

Definition at line 431 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_last_regd_id_scan_topic

private

Name of the topic that the last registered laser scan (+ corresponding nodeID) is published at.

Definition at line 457 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

ros::Publisher mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_last_regd_nodes_pub

private

Publisher of the last registered nodeIDs and positions.

Note: see m_num_last_regd_nodes variable for the exact number of published nodeID and position pairs.

Definition at line 437 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_last_regd_nodes_topic

private

Name of the topic that the last X registered nodes + positions are going to be published at.

Definition at line 461 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

ros::Publisher mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_list_neighbors_pub

private

Definition at line 428 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_list_neighbors_topic

private

Name of topic at which we publish information about the agents that we can currently communicate with.

Definition at line 453 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_mr_ns

private

Condensed Measurements topic namespace.

Definition at line 449 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

TColorManager mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_neighbor_colors_manager

private

Definition at line 514 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

std::map<TNeighborAgentProps*, bool> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_neighbor_to_found_initial_tf

private

Map from neighbor instance to transformation from own to neighbor's graph.

As long as the corresponding variable for a specific agent is False current engine keeps trying the map-matching proc to find a common transformation between own and neighbor graphs.

Definition at line 420 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>

neighbors_t mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_neighbors

private

GraphSlamAgent instance pointer to TNeighborAgentProps.