Helper class that will publish results onto the ROS framework. More...

#include <ROS1Visualizer.h>

Public Member Functions
void	callback_inertial (const sensor_msgs::Imu::ConstPtr &msg)
	Callback for inertial information. More...

void	callback_monocular (const sensor_msgs::ImageConstPtr &msg0, int cam_id0)
	Callback for monocular cameras information. More...

void	callback_stereo (const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, int cam_id1)
	Callback for synchronized stereo camera information. More...

	ROS1Visualizer (std::shared_ptr< ros::NodeHandle > nh, std::shared_ptr< VioManager > app, std::shared_ptr< Simulator > sim=nullptr)
	Default constructor. More...

void	setup_subscribers (std::shared_ptr< ov_core::YamlParser > parser)
	Will setup ROS subscribers and callbacks. More...

void	visualize ()
	Will visualize the system if we have new things. More...

void	visualize_final ()
	After the run has ended, print results. More...

void	visualize_odometry (double timestamp)
	Will publish our odometry message for the current timestep. This will take the current state estimate and get the propagated pose to the desired time. This can be used to get pose estimates on systems which require high frequency pose estimates. More...

Protected Types
typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image >	sync_pol

Protected Member Functions
void	publish_features ()
	Publish current features. More...

void	publish_groundtruth ()
	Publish groundtruth (if we have it) More...

void	publish_images ()
	Publish the active tracking image. More...

void	publish_loopclosure_information ()
	Publish loop-closure information of current pose and active track information. More...

void	publish_state ()
	Publish the current state. More...

Protected Attributes
std::shared_ptr< VioManager >	_app
	Core application of the filter system. More...

std::shared_ptr< ros::NodeHandle >	_nh
	Global node handler. More...

std::shared_ptr< Simulator >	_sim
	Simulator (is nullptr if we are not sim'ing) More...

std::map< int, double >	camera_last_timestamp

std::deque< ov_core::CameraData >	camera_queue

std::mutex	camera_queue_mtx

std::map< double, Eigen::Matrix< double, 17, 1 > >	gt_states

image_transport::Publisher	it_pub_loop_img_depth

image_transport::Publisher	it_pub_loop_img_depth_color

image_transport::Publisher	it_pub_tracks

double	last_visualization_timestamp = 0

double	last_visualization_timestamp_image = 0

std::shared_ptr< tf::TransformBroadcaster >	mTfBr

std::ofstream	of_state_est

std::ofstream	of_state_gt

std::ofstream	of_state_std

std::vector< geometry_msgs::PoseStamped >	poses_gt

std::vector< geometry_msgs::PoseStamped >	poses_imu

unsigned int	poses_seq_gt = 0

unsigned int	poses_seq_imu = 0

ros::Publisher	pub_loop_extrinsic

ros::Publisher	pub_loop_intrinsics

ros::Publisher	pub_loop_point

ros::Publisher	pub_loop_pose

ros::Publisher	pub_odomimu

ros::Publisher	pub_pathgt

ros::Publisher	pub_pathimu

ros::Publisher	pub_points_aruco

ros::Publisher	pub_points_msckf

ros::Publisher	pub_points_sim

ros::Publisher	pub_points_slam

ros::Publisher	pub_posegt

ros::Publisher	pub_poseimu

bool	publish_calibration_tf = true

bool	publish_global2imu_tf = true

boost::posix_time::ptime	rT1

boost::posix_time::ptime	rT2

bool	save_total_state = false

bool	start_time_set = false

ros::Subscriber	sub_imu

std::vector< ros::Subscriber >	subs_cam

double	summed_mse_ori = 0.0

double	summed_mse_pos = 0.0

double	summed_nees_ori = 0.0

double	summed_nees_pos = 0.0

size_t	summed_number = 0

std::vector< std::shared_ptr< message_filters::Synchronizer< sync_pol > > >	sync_cam

std::vector< std::shared_ptr< message_filters::Subscriber< sensor_msgs::Image > > >	sync_subs_cam

std::atomic< bool >	thread_update_running

Detailed Description

Helper class that will publish results onto the ROS framework.

Also save to file the current total state and covariance along with the groundtruth if we are simulating. We visualize the following things:

State of the system on TF, pose message, and path
Image of our tracker
Our different features (SLAM, MSCKF, ARUCO)
Groundtruth trajectory if we have it

Definition at line 74 of file ROS1Visualizer.h.

Member Typedef Documentation

◆ sync_pol

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> ov_msckf::ROS1Visualizer::sync_pol

protected

Definition at line 152 of file ROS1Visualizer.h.

Constructor & Destructor Documentation

◆ ROS1Visualizer()

ROS1Visualizer::ROS1Visualizer	(	std::shared_ptr< ros::NodeHandle >	nh,
		std::shared_ptr< VioManager >	app,
		std::shared_ptr< Simulator >	sim = `nullptr`
	)

Default constructor.

Parameters

nh	ROS node handler
app	Core estimator manager
sim	Simulator if we are simulating

Definition at line 38 of file ROS1Visualizer.cpp.

Member Function Documentation

◆ callback_inertial()

void ROS1Visualizer::callback_inertial ( const sensor_msgs::Imu::ConstPtr & msg )

Callback for inertial information.

Definition at line 438 of file ROS1Visualizer.cpp.

◆ callback_monocular()

void ROS1Visualizer::callback_monocular	(	const sensor_msgs::ImageConstPtr &	msg0,
		int	cam_id0
	)

Callback for monocular cameras information.

Definition at line 498 of file ROS1Visualizer.cpp.

◆ callback_stereo()

void ROS1Visualizer::callback_stereo	(	const sensor_msgs::ImageConstPtr &	msg0,
		const sensor_msgs::ImageConstPtr &	msg1,
		int	cam_id0,
		int	cam_id1
	)

Callback for synchronized stereo camera information.

Definition at line 537 of file ROS1Visualizer.cpp.

◆ publish_features()

void ROS1Visualizer::publish_features ( )

protected

Publish current features.

Definition at line 679 of file ROS1Visualizer.cpp.

◆ publish_groundtruth()

void ROS1Visualizer::publish_groundtruth ( )

protected

Publish groundtruth (if we have it)

Definition at line 711 of file ROS1Visualizer.cpp.

◆ publish_images()

void ROS1Visualizer::publish_images ( )

protected

Publish the active tracking image.

Definition at line 651 of file ROS1Visualizer.cpp.

◆ publish_loopclosure_information()

void ROS1Visualizer::publish_loopclosure_information ( )

protected

Publish loop-closure information of current pose and active track information.

Definition at line 840 of file ROS1Visualizer.cpp.

◆ publish_state()

void ROS1Visualizer::publish_state ( )

protected

Publish the current state.

Definition at line 591 of file ROS1Visualizer.cpp.

◆ setup_subscribers()

void ROS1Visualizer::setup_subscribers ( std::shared_ptr< ov_core::YamlParser > parser )

Will setup ROS subscribers and callbacks.

Parameters

parser Configuration file parser

Definition at line 151 of file ROS1Visualizer.cpp.

◆ visualize()

void ROS1Visualizer::visualize ( )

Will visualize the system if we have new things.

Definition at line 197 of file ROS1Visualizer.cpp.

◆ visualize_final()

void ROS1Visualizer::visualize_final ( )

After the run has ended, print results.

Definition at line 352 of file ROS1Visualizer.cpp.

◆ visualize_odometry()

void ROS1Visualizer::visualize_odometry ( double timestamp )

Will publish our odometry message for the current timestep. This will take the current state estimate and get the propagated pose to the desired time. This can be used to get pose estimates on systems which require high frequency pose estimates.

Definition at line 245 of file ROS1Visualizer.cpp.

Member Data Documentation

◆ _app

std::shared_ptr<VioManager> ov_msckf::ROS1Visualizer::_app

protected

Core application of the filter system.

Definition at line 137 of file ROS1Visualizer.h.

◆ _nh

std::shared_ptr<ros::NodeHandle> ov_msckf::ROS1Visualizer::_nh

protected

Global node handler.

Definition at line 134 of file ROS1Visualizer.h.

◆ _sim

std::shared_ptr<Simulator> ov_msckf::ROS1Visualizer::_sim

protected

Simulator (is nullptr if we are not sim'ing)

Definition at line 140 of file ROS1Visualizer.h.

◆ camera_last_timestamp

std::map<int, double> ov_msckf::ROS1Visualizer::camera_last_timestamp

protected

Definition at line 183 of file ROS1Visualizer.h.

◆ camera_queue

std::deque<ov_core::CameraData> ov_msckf::ROS1Visualizer::camera_queue

protected

Queue up camera measurements sorted by time and trigger once we have exactly one IMU measurement with timestamp newer than the camera measurement This also handles out-of-order camera measurements, which is rare, but a nice feature to have for general robustness to bad camera drivers.

Definition at line 179 of file ROS1Visualizer.h.

◆ camera_queue_mtx

std::mutex ov_msckf::ROS1Visualizer::camera_queue_mtx

protected

Definition at line 180 of file ROS1Visualizer.h.

◆ gt_states

std::map<double, Eigen::Matrix<double, 17, 1> > ov_msckf::ROS1Visualizer::gt_states

protected

Definition at line 190 of file ROS1Visualizer.h.

◆ it_pub_loop_img_depth

image_transport::Publisher ov_msckf::ROS1Visualizer::it_pub_loop_img_depth

protected

Definition at line 143 of file ROS1Visualizer.h.

◆ it_pub_loop_img_depth_color

image_transport::Publisher ov_msckf::ROS1Visualizer::it_pub_loop_img_depth_color

protected

Definition at line 143 of file ROS1Visualizer.h.

◆ it_pub_tracks

image_transport::Publisher ov_msckf::ROS1Visualizer::it_pub_tracks

protected

Definition at line 143 of file ROS1Visualizer.h.

◆ last_visualization_timestamp

double ov_msckf::ROS1Visualizer::last_visualization_timestamp = 0

protected

Definition at line 186 of file ROS1Visualizer.h.

◆ last_visualization_timestamp_image

double ov_msckf::ROS1Visualizer::last_visualization_timestamp_image = 0

protected

Definition at line 187 of file ROS1Visualizer.h.

◆ mTfBr

std::shared_ptr<tf::TransformBroadcaster> ov_msckf::ROS1Visualizer::mTfBr

protected

Definition at line 147 of file ROS1Visualizer.h.

◆ of_state_est

std::ofstream ov_msckf::ROS1Visualizer::of_state_est

protected

Definition at line 200 of file ROS1Visualizer.h.

◆ of_state_gt

std::ofstream ov_msckf::ROS1Visualizer::of_state_gt

protected

Definition at line 200 of file ROS1Visualizer.h.

◆ of_state_std

std::ofstream ov_msckf::ROS1Visualizer::of_state_std

protected

Definition at line 200 of file ROS1Visualizer.h.

◆ poses_gt

std::vector<geometry_msgs::PoseStamped> ov_msckf::ROS1Visualizer::poses_gt

protected

Definition at line 194 of file ROS1Visualizer.h.

◆ poses_imu

std::vector<geometry_msgs::PoseStamped> ov_msckf::ROS1Visualizer::poses_imu

protected

Definition at line 158 of file ROS1Visualizer.h.

◆ poses_seq_gt

unsigned int ov_msckf::ROS1Visualizer::poses_seq_gt = 0

protected

Definition at line 193 of file ROS1Visualizer.h.

◆ poses_seq_imu

unsigned int ov_msckf::ROS1Visualizer::poses_seq_imu = 0

protected

Definition at line 157 of file ROS1Visualizer.h.

◆ pub_loop_extrinsic

ros::Publisher ov_msckf::ROS1Visualizer::pub_loop_extrinsic

protected

Definition at line 146 of file ROS1Visualizer.h.

◆ pub_loop_intrinsics

ros::Publisher ov_msckf::ROS1Visualizer::pub_loop_intrinsics

protected

Definition at line 146 of file ROS1Visualizer.h.

◆ pub_loop_point

ros::Publisher ov_msckf::ROS1Visualizer::pub_loop_point

protected

Definition at line 146 of file ROS1Visualizer.h.

◆ pub_loop_pose

ros::Publisher ov_msckf::ROS1Visualizer::pub_loop_pose

protected

Definition at line 146 of file ROS1Visualizer.h.

◆ pub_odomimu

ros::Publisher ov_msckf::ROS1Visualizer::pub_odomimu

protected

Definition at line 144 of file ROS1Visualizer.h.

◆ pub_pathgt

ros::Publisher ov_msckf::ROS1Visualizer::pub_pathgt

protected

Definition at line 161 of file ROS1Visualizer.h.

◆ pub_pathimu

ros::Publisher ov_msckf::ROS1Visualizer::pub_pathimu

protected

Definition at line 144 of file ROS1Visualizer.h.

◆ pub_points_aruco

ros::Publisher ov_msckf::ROS1Visualizer::pub_points_aruco

protected

Definition at line 145 of file ROS1Visualizer.h.

◆ pub_points_msckf

ros::Publisher ov_msckf::ROS1Visualizer::pub_points_msckf

protected

Definition at line 145 of file ROS1Visualizer.h.

◆ pub_points_sim

ros::Publisher ov_msckf::ROS1Visualizer::pub_points_sim

protected

Definition at line 145 of file ROS1Visualizer.h.

◆ pub_points_slam

ros::Publisher ov_msckf::ROS1Visualizer::pub_points_slam

protected

Definition at line 145 of file ROS1Visualizer.h.

◆ pub_posegt

ros::Publisher ov_msckf::ROS1Visualizer::pub_posegt

protected

Definition at line 161 of file ROS1Visualizer.h.

◆ pub_poseimu

ros::Publisher ov_msckf::ROS1Visualizer::pub_poseimu

protected

Definition at line 144 of file ROS1Visualizer.h.

◆ publish_calibration_tf

bool ov_msckf::ROS1Visualizer::publish_calibration_tf = true

protected

Definition at line 196 of file ROS1Visualizer.h.

◆ publish_global2imu_tf

bool ov_msckf::ROS1Visualizer::publish_global2imu_tf = true

protected

Definition at line 195 of file ROS1Visualizer.h.

◆ rT1

boost::posix_time::ptime ov_msckf::ROS1Visualizer::rT1

protected

Definition at line 170 of file ROS1Visualizer.h.

◆ rT2

boost::posix_time::ptime ov_msckf::ROS1Visualizer::rT2

protected

Definition at line 170 of file ROS1Visualizer.h.

◆ save_total_state

bool ov_msckf::ROS1Visualizer::save_total_state = false

protected

Definition at line 199 of file ROS1Visualizer.h.

◆ start_time_set

bool ov_msckf::ROS1Visualizer::start_time_set = false

protected

Definition at line 169 of file ROS1Visualizer.h.

◆ sub_imu

ros::Subscriber ov_msckf::ROS1Visualizer::sub_imu

protected

Definition at line 150 of file ROS1Visualizer.h.

◆ subs_cam

std::vector<ros::Subscriber> ov_msckf::ROS1Visualizer::subs_cam

protected

Definition at line 151 of file ROS1Visualizer.h.

◆ summed_mse_ori

double ov_msckf::ROS1Visualizer::summed_mse_ori = 0.0

protected

Definition at line 162 of file ROS1Visualizer.h.

◆ summed_mse_pos

double ov_msckf::ROS1Visualizer::summed_mse_pos = 0.0

protected

Definition at line 163 of file ROS1Visualizer.h.

◆ summed_nees_ori

double ov_msckf::ROS1Visualizer::summed_nees_ori = 0.0

protected

Definition at line 164 of file ROS1Visualizer.h.

◆ summed_nees_pos

double ov_msckf::ROS1Visualizer::summed_nees_pos = 0.0

protected

Definition at line 165 of file ROS1Visualizer.h.

◆ summed_number

size_t ov_msckf::ROS1Visualizer::summed_number = 0

protected

Definition at line 166 of file ROS1Visualizer.h.

◆ sync_cam

std::vector<std::shared_ptr<message_filters::Synchronizer<sync_pol> > > ov_msckf::ROS1Visualizer::sync_cam

protected

Definition at line 153 of file ROS1Visualizer.h.

◆ sync_subs_cam

std::vector<std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image> > > ov_msckf::ROS1Visualizer::sync_subs_cam

protected

Definition at line 154 of file ROS1Visualizer.h.

◆ thread_update_running

std::atomic<bool> ov_msckf::ROS1Visualizer::thread_update_running

protected

Definition at line 173 of file ROS1Visualizer.h.

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

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ sync_pol

Constructor & Destructor Documentation

◆ ROS1Visualizer()

Member Function Documentation

◆ callback_inertial()

◆ callback_monocular()

◆ callback_stereo()

◆ publish_features()

◆ publish_groundtruth()

◆ publish_images()

◆ publish_loopclosure_information()

◆ publish_state()

◆ setup_subscribers()

◆ visualize()

◆ visualize_final()

◆ visualize_odometry()

Member Data Documentation

◆ _app

◆ _nh

◆ _sim

◆ camera_last_timestamp

◆ camera_queue

◆ camera_queue_mtx

◆ gt_states

◆ it_pub_loop_img_depth

◆ it_pub_loop_img_depth_color

◆ it_pub_tracks

◆ last_visualization_timestamp

◆ last_visualization_timestamp_image

◆ mTfBr

◆ of_state_est

◆ of_state_gt

◆ of_state_std

◆ poses_gt

◆ poses_imu

◆ poses_seq_gt

◆ poses_seq_imu

◆ pub_loop_extrinsic

◆ pub_loop_intrinsics

◆ pub_loop_point

◆ pub_loop_pose

◆ pub_odomimu

◆ pub_pathgt

◆ pub_pathimu

◆ pub_points_aruco

◆ pub_points_msckf

◆ pub_points_sim

◆ pub_points_slam

◆ pub_posegt

◆ pub_poseimu

◆ publish_calibration_tf

◆ publish_global2imu_tf

◆ rT1

◆ rT2

◆ save_total_state

◆ start_time_set

◆ sub_imu

◆ subs_cam

◆ summed_mse_ori

◆ summed_mse_pos

◆ summed_nees_ori

◆ summed_nees_pos

◆ summed_number

◆ sync_cam

◆ sync_subs_cam

◆ thread_update_running