The EKFslamWrapper class provides the ROS wrapper for EKF SLAM 3d from MRPT libraries. More...

#include <mrpt_ekf_slam_3d_wrapper.h>

Inheritance diagram for EKFslamWrapper:

Public Member Functions
void	computeEllipseOrientationScale (tf2::Quaternion &orientation, Eigen::Vector3d &scale, const mrpt::math::CMatrixDouble33 &covariance)
	compute the orientation and scale of covariance ellipsoids More...

	EKFslamWrapper ()
	constructor More...

void	get_param ()
	read the parameters from launch file More...

void	init ()
	initialize publishers subscribers and EKF 3d slam More...

bool	is_file_exists (const std::string &name)
	check the existance of the file More...

void	landmarkCallback (const mrpt_msgs::ObservationRangeBearing &_msg)
	callback function for the landmarks More...

void	makeRightHanded (Eigen::Matrix3d &eigenvectors, Eigen::Vector3d &eigenvalues)
	compute the correct orientation and scale of covariance ellipsoids (make sure that we output covariance ellipsoids for right handed system of coordinates) More...

void	odometryForCallback (mrpt::obs::CObservationOdometry::Ptr &_odometry, const std_msgs::Header &_msg_header)
	get the odometry for received observation More...

void	publishTF ()
	publis tf tree More...

bool	rawlogPlay ()
	play rawlog file More...

void	updateSensorPose (std::string _frame_id)
	update the pose of the sensor with respect to the robot More...

void	viz_dataAssociation ()
	visualize the data associations for the landmarks observed by robot at the each step More...

void	viz_state ()
	visualize the covariance ellipsoids for robot and landmarks More...

bool	waitForTransform (mrpt::poses::CPose3D &des, const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01))
	wait for transfor between odometry frame and the robot frame More...

	~EKFslamWrapper ()
	destructor More...

Private Attributes
std::string	base_frame_id
	robot frame More...

ros::Publisher	data_association_viz_pub_

double	ellipse_scale_

std::string	global_frame_id
	/map frame More...

std::string	ini_filename
	name of ini file More...

std::map< std::string, mrpt::poses::CPose3D >	landmark_poses_
	landmark poses with respect to the map More...

tf2_ros::TransformListener	listenerTF_ {tf_buffer_}

ros::NodeHandle	n_
	Node handler. More...

std::string	odom_frame_id
	/odom frame More...

std::string	rawlog_filename
	name of rawlog file More...

bool	rawlog_play_
	true if rawlog file exists More...

double	rawlog_play_delay
	delay of replay from rawlog file More...

std::string	sensor_source
	2D laser scans More...

std::vector< ros::Subscriber >	sensorSub_
	list of sensors topics More...

ros::Publisher	state_viz_pub_

float	t_exec
	the time which take one SLAM update execution More...

tf2_ros::TransformBroadcaster	tf_broadcaster_
	transform broadcaster More...

tf2_ros::Buffer	tf_buffer_

mrpt::system::CTicTac	tictac
	timer for SLAM performance evaluation More...

Private Attributes inherited from EKFslam
mrpt::obs::CActionCollection::Ptr	action
	actions More...

bool	CAMERA_3DSCENE_FOLLOWS_ROBOT

mrpt::math::CMatrixDouble	fullCov_
	full covariance matrix More...

mrpt::math::CVectorDouble	fullState_
	full state vector More...

std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID >	LM_IDs_
	vector of the landmarks ID More...

std::vector< mrpt::math::TPoint3D >	LMs_

mrpt::slam::CRangeBearingKFSLAM	mapping
	EKF slam 3d class. More...

std::vector< mrpt::math::TPose3D >	meanPath

mrpt::obs::CActionRobotMovement3D::TMotionModelOptions	motion_model_options_
	used with odom value motion noise More...

mrpt::poses::CPose3D	odomLastObservation_
	last observation of odometry More...

mrpt::poses::CPose3DQuatPDFGaussian	robotPose_
	current robot pose More...

mrpt::obs::CSensoryFrame::Ptr	sf
	observations More...

bool	SHOW_3D_LIVE

mrpt::system::TTimeStamp	timeLastUpdate_
	last update of the pose and map More...

mrpt::gui::CDisplayWindow3D::Ptr	win3d
	MRPT window. More...

Additional Inherited Members
Private Member Functions inherited from EKFslam
	EKFslam ()
	constructor More...

void	init3Dwindow ()
	init 3D window from mrpt lib More...

void	landmark_to_3d (const mrpt::slam::CRangeBearingKFSLAM::KFArray_FEAT &lm, mrpt::math::TPoint3D &p)
	convert landmark to 3d point More...

void	observation (mrpt::obs::CSensoryFrame::Ptr _sf, mrpt::obs::CObservationOdometry::Ptr _odometry)
	calculate the actions from odometry model for current observation More...

void	read_iniFile (std::string ini_filename)
	read ini file More...

void	run3Dwindow ()
	run 3D window update from mrpt lib More...

virtual	~EKFslam ()
	destructor More...

Detailed Description

The EKFslamWrapper class provides the ROS wrapper for EKF SLAM 3d from MRPT libraries.

Definition at line 58 of file mrpt_ekf_slam_3d_wrapper.h.

Constructor & Destructor Documentation

◆ EKFslamWrapper()

EKFslamWrapper::EKFslamWrapper ( )

constructor

Definition at line 11 of file mrpt_ekf_slam_3d_wrapper.cpp.

◆ ~EKFslamWrapper()

EKFslamWrapper::~EKFslamWrapper ( )

destructor

Definition at line 16 of file mrpt_ekf_slam_3d_wrapper.cpp.

Member Function Documentation

◆ computeEllipseOrientationScale()

void EKFslamWrapper::computeEllipseOrientationScale	(	tf2::Quaternion &	orientation,
		Eigen::Vector3d &	scale,
		const mrpt::math::CMatrixDouble33 &	covariance
	)

compute the orientation and scale of covariance ellipsoids

Parameters

orientation	the orientation of the ellipsoid in Quaternions
scale	the vector of the eigen values for calculating the size of the ellipse
covariance	covariance matrix for current landmarks or robot pose

Definition at line 285 of file mrpt_ekf_slam_3d_wrapper.cpp.

◆ get_param()

void EKFslamWrapper::get_param ( )

read the parameters from launch file

Definition at line 23 of file mrpt_ekf_slam_3d_wrapper.cpp.

◆ init()

void EKFslamWrapper::init ( )

initialize publishers subscribers and EKF 3d slam

Create subscribers///

Definition at line 50 of file mrpt_ekf_slam_3d_wrapper.cpp.

◆ is_file_exists()

bool EKFslamWrapper::is_file_exists ( const std::string & name )

check the existance of the file

Returns: true if file exists

Definition at line 17 of file mrpt_ekf_slam_3d_wrapper.cpp.

◆ landmarkCallback()

void EKFslamWrapper::landmarkCallback ( const mrpt_msgs::ObservationRangeBearing & _msg )

callback function for the landmarks

Given the landmarks wait for odometry, create the pair of action and observation, implement one SLAM update, publish map and pose.

Parameters

_msg	the landmark message

Definition at line 169 of file mrpt_ekf_slam_3d_wrapper.cpp.

◆ makeRightHanded()

void EKFslamWrapper::makeRightHanded	(	Eigen::Matrix3d &	eigenvectors,
		Eigen::Vector3d &	eigenvalues
	)

compute the correct orientation and scale of covariance ellipsoids (make sure that we output covariance ellipsoids for right handed system of coordinates)

Parameters

eigenvectors	the 3x3 matrix of eigenvectors
eigenvalues	the 3d vector of eigen values

Definition at line 259 of file mrpt_ekf_slam_3d_wrapper.cpp.

◆ odometryForCallback()

void EKFslamWrapper::odometryForCallback	(	mrpt::obs::CObservationOdometry::Ptr &	_odometry,
		const std_msgs::Header &	_msg_header
	)

get the odometry for received observation

Parameters

_odometry	odometry for received observation
_msg_header	timestamp of the observation

Definition at line 101 of file mrpt_ekf_slam_3d_wrapper.cpp.

◆ publishTF()

void EKFslamWrapper::publishTF ( )

publis tf tree

Definition at line 488 of file mrpt_ekf_slam_3d_wrapper.cpp.

◆ rawlogPlay()

bool EKFslamWrapper::rawlogPlay ( )

play rawlog file

Returns: true if rawlog file exists and played

Definition at line 210 of file mrpt_ekf_slam_3d_wrapper.cpp.

◆ updateSensorPose()

void EKFslamWrapper::updateSensorPose ( std::string _frame_id )

update the pose of the sensor with respect to the robot

Parameters

frame_id the frame of the sensors

Definition at line 120 of file mrpt_ekf_slam_3d_wrapper.cpp.

◆ viz_dataAssociation()

void EKFslamWrapper::viz_dataAssociation ( )

visualize the data associations for the landmarks observed by robot at the each step

Definition at line 331 of file mrpt_ekf_slam_3d_wrapper.cpp.

◆ viz_state()

void EKFslamWrapper::viz_state ( )

visualize the covariance ellipsoids for robot and landmarks

Definition at line 393 of file mrpt_ekf_slam_3d_wrapper.cpp.

◆ waitForTransform()

bool EKFslamWrapper::waitForTransform	(	mrpt::poses::CPose3D &	des,
		const std::string &	target_frame,
		const std::string &	source_frame,
		const ros::Time &	time,
		const ros::Duration &	timeout,
		const ros::Duration &	polling_sleep_duration = `ros::Duration(0.01)`
	)

wait for transfor between odometry frame and the robot frame

Parameters

des	position of the robot with respect to odometry frame
target_frame	the odometry tf frame
source_frame	the robot tf frame
time	timestamp of the observation for which we want to retrieve the position of the robot
timeout	timeout for odometry waiting
polling_sleep_duration	timeout for transform wait