Subscribes to incoming RGBD images and outputs the position of the moving (base) frame wrt some fixed frame. More...

#include <visual_odometry.h>

Public Member Functions
	VisualOdometry (const ros::NodeHandle &nh, const ros::NodeHandle &nh_private)
	Constructor from ROS nodehandles.
virtual	~VisualOdometry ()
	Default destructor.
Private Member Functions
void	configureMotionEstimation ()
void	diagnostics (int n_features, int n_valid_features, int n_model_pts, double d_frame, double d_features, double d_reg, double d_total)
	Saves computed running times to file (or print on screen)
bool	getBaseToCameraTf (const std_msgs::Header &header)
	Caches the transform from the base frame to the camera frame.
void	gftReconfigCallback (GftDetectorConfig &config, uint32_t level)
	ROS dynamic reconfigure callback function for GFT.
void	initParams ()
	Initializes all the parameters from the ROS param server.
void	orbReconfigCallback (OrbDetectorConfig &config, uint32_t level)
	ROS dynamic reconfigure callback function for ORB.
void	publishFeatureCloud (rgbdtools::RGBDFrame &frame)
	Publish the feature point cloud.
void	publishFeatureCovariances (rgbdtools::RGBDFrame &frame)
void	publishModelCloud ()
void	publishModelCovariances ()
void	publishOdom (const std_msgs::Header &header)
	publishes the f2b_ (fixed-to-base) transform as an Odom message
void	publishPath (const std_msgs::Header &header)
	publishes the path of f2b_ (fixed-to-base) transform as an Path message
void	publishPoseStamped (const std_msgs::Header &header)
	publishes the f2b_ (fixed-to-base) transform as an pose stamped message
void	publishTf (const std_msgs::Header &header)
	publishes the f2b_ (fixed-to-base) transform as a tf
void	resetDetector ()
	Re-instantiates the feature detector based on the detector type parameter.
bool	resetposSrvCallback (Save::Request &request, Save::Response &response)
void	RGBDCallback (const ImageMsg::ConstPtr &rgb_msg, const ImageMsg::ConstPtr &depth_msg, const CameraInfoMsg::ConstPtr &info_msg)
	Main callback for RGB, Depth, and CameraInfo messages.
void	starReconfigCallback (StarDetectorConfig &config, uint32_t level)
	ROS dynamic reconfigure callback function for STAR.
void	surfReconfigCallback (SurfDetectorConfig &config, uint32_t level)
	ROS dynamic reconfigure callback function for SURF.
Private Attributes
tf::Transform	b2c_
	Transform from the base to the camera frame, wrt base frame.
std::string	base_frame_
	Moving frame parameter.
boost::shared_ptr< ImageTransport >	depth_it_
	Image transport for depth message subscription.
std::string	detector_type_
	Feature detector type parameter.
FILE *	diagnostics_file_
	File for time recording statistics.
std::string	diagnostics_file_name_
	File name for time recording statistics.
tf::Transform	f2b_
	Transform from the fixed to the base frame, wrt fixed frame.
ros::Publisher	feature_cloud_publisher_
ros::Publisher	feature_cov_publisher_
boost::shared_ptr < rgbdtools::FeatureDetector >	feature_detector_
	The feature detector object.
std::string	fixed_frame_
	Fixed frame parameter.
int	frame_count_
	RGBD frame counter.
GftDetectorConfigServerPtr	gft_config_server_
	ROS dynamic reconfigure server for GFT params.
ros::Time	init_time_
	Time of first RGBD message.
bool	initialized_
	Whether the init_time and b2c_ variables have been set.
ros::Publisher	model_cloud_publisher_
ros::Publisher	model_cov_publisher_
rgbdtools::MotionEstimationICPProbModel	motion_estimation_
	The motion estimation object.
ros::NodeHandle	nh_
	the public nodehandle
ros::NodeHandle	nh_private_
	the private nodehandle
ros::Publisher	odom_publisher_
	ROS Odometry publisher.
OrbDetectorConfigServerPtr	orb_config_server_
	ROS dynamic reconfigure server for ORB params.
PathMsg	path_msg_
	contains a vector of positions of the Base frame.
ros::Publisher	path_pub_
	ROS publisher for the VO path.
ros::Publisher	pose_stamped_publisher_
	ROS pose stamped publisher.
bool	publish_cloud_
	If true, publish the pcl feature cloud.
bool	publish_feature_cloud_
bool	publish_feature_cov_
bool	publish_model_cloud_
bool	publish_model_cov_
bool	publish_odom_
	Parameter whether to publish an odom message.
bool	publish_path_
	Parameter whether to publish a path message.
bool	publish_pose_
	Parameter whether to publish a pose message.
bool	publish_tf_
	Parameter whether to publish a ros tf.
int	queue_size_
	Subscription queue size.
std::string	reg_type_
	Motion estimation (registration) type parameter.
ros::ServiceServer	reset_pos_
boost::shared_ptr< ImageTransport >	rgb_it_
	Image transport for RGB message subscription.
bool	save_diagnostics_
	indicates whether to save results to file or print to screen
StarDetectorConfigServerPtr	star_config_server_
	ROS dynamic reconfigure server for STAR params.
ImageSubFilter	sub_depth_
	Depth message subscriber.
CameraInfoSubFilter	sub_info_
	Camera info message subscriber.
ImageSubFilter	sub_rgb_
	RGB message subscriber.
SurfDetectorConfigServerPtr	surf_config_server_
	ROS dynamic reconfigure server for SURF params.
boost::shared_ptr < RGBDSynchronizer3 >	sync_
	Callback syncronizer.
tf::TransformBroadcaster	tf_broadcaster_
	ROS transform broadcaster.
tf::TransformListener	tf_listener_
	ROS transform listener.
bool	verbose_
	indicates whether to print diagnostics to screen

Detailed Description

Subscribes to incoming RGBD images and outputs the position of the moving (base) frame wrt some fixed frame.

The class offers a selection of sparse feature detectors (GFT, ORB, SURF, STAR), as well as a selection of registration algorithms. The default registration method (ICPProbModel) aligns the incoming 3D sparse features against a persistent 3D feature model, which is continuously updated using a Kalman Filer.

Definition at line 57 of file visual_odometry.h.

Constructor & Destructor Documentation

ccny_rgbd::VisualOdometry::VisualOdometry	(	const ros::NodeHandle &	nh,
		const ros::NodeHandle &	nh_private
	)

Constructor from ROS nodehandles.

Parameters:

nh	the public nodehandle
nh_private	the private nodehandle

Definition at line 29 of file visual_odometry.cpp.

ccny_rgbd::VisualOdometry::~VisualOdometry ( ) [virtual]

Default destructor.

Definition at line 83 of file visual_odometry.cpp.

Member Function Documentation

void ccny_rgbd::VisualOdometry::configureMotionEstimation ( ) [private]

Definition at line 225 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::diagnostics	(	int	n_features,
		int	n_valid_features,
		int	n_model_pts,
		double	d_frame,
		double	d_features,
		double	d_reg,
		double	d_total
	)		`[private]`

Saves computed running times to file (or print on screen)

Returns:: 1 if write to file was successful

Definition at line 513 of file visual_odometry.cpp.

bool ccny_rgbd::VisualOdometry::getBaseToCameraTf ( const std_msgs::Header & header ) [private]

Caches the transform from the base frame to the camera frame.

Parameters:

header header of the incoming message, used to stamp things correctly

Definition at line 456 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::gftReconfigCallback	(	GftDetectorConfig &	config,
		uint32_t	level
	)		`[private]`

ROS dynamic reconfigure callback function for GFT.

Definition at line 478 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::initParams ( ) [private]

Initializes all the parameters from the ROS param server.

Definition at line 149 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::orbReconfigCallback	(	OrbDetectorConfig &	config,
		uint32_t	level
	)		`[private]`

ROS dynamic reconfigure callback function for ORB.

Definition at line 504 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::publishFeatureCloud ( rgbdtools::RGBDFrame & frame ) [private]

Publish the feature point cloud.

Note: this might decrease performance

Definition at line 540 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::publishFeatureCovariances ( rgbdtools::RGBDFrame & frame ) [private]

< publish feature covariances

Definition at line 547 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::publishModelCloud ( ) [private]

Definition at line 552 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::publishModelCovariances ( ) [private]

< ppublish model covariances

Definition at line 559 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::publishOdom ( const std_msgs::Header & header ) [private]

publishes the f2b_ (fixed-to-base) transform as an Odom message

Todo:: publish also as PoseWithCovariance

Parameters:

header header of the incoming message, used to stamp things correctly

Definition at line 422 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::publishPath ( const std_msgs::Header & header ) [private]

publishes the path of f2b_ (fixed-to-base) transform as an Path message

Parameters:

header header of the incoming message, used to stamp things correctly

Definition at line 442 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::publishPoseStamped ( const std_msgs::Header & header ) [private]

publishes the f2b_ (fixed-to-base) transform as an pose stamped message

Parameters:

header header of the incoming message, used to stamp things correctly

Definition at line 431 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::publishTf ( const std_msgs::Header & header ) [private]

publishes the f2b_ (fixed-to-base) transform as a tf

Parameters:

header header of the incoming message, used to stamp things correctly

Definition at line 415 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::resetDetector ( ) [private]

Re-instantiates the feature detector based on the detector type parameter.

Definition at line 275 of file visual_odometry.cpp.

bool ccny_rgbd::VisualOdometry::resetposSrvCallback	(	Save::Request &	request,
		Save::Response &	response
	)		`[private]`

Definition at line 89 of file visual_odometry.cpp.

void ccny_rgbd::VisualOdometry::RGBDCallback	(	const ImageMsg::ConstPtr &	rgb_msg,
		const ImageMsg::ConstPtr &	depth_msg,
		const CameraInfoMsg::ConstPtr &	info_msg
	)		`[private]`