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
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 (RGBDFrame &frame)
	Publish the feature point cloud.
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	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.
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.
ros::Publisher	cloud_publisher_
	ROS feature cloud publisher.
boost::shared_ptr< ImageTransport >	depth_it_
	Image transport for depth message subscription.
std::string	detector_type_
	Feature detector type parameter.
tf::Transform	f2b_
	Transform from the fixed to the base frame, wrt fixed frame.
boost::shared_ptr < 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.
MotionEstimation *	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.
bool	publish_cloud_
	If true, publish the pcl feature cloud.
bool	publish_odom_
	Parameter whether to publish an odom message.
bool	publish_path_
	Parameter whether to publish a path 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.
boost::shared_ptr< ImageTransport >	rgb_it_
	Image transport for RGB message subscription.
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.

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 61 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 28 of file visual_odometry.cpp.

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

Default destructor.

Definition at line 71 of file visual_odometry.cpp.

Member Function Documentation

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 302 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 324 of file visual_odometry.cpp.

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

Initializes all the parameters from the ROS param server.

Definition at line 76 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 350 of file visual_odometry.cpp.

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

Publish the feature point cloud.

Note: this might decrease performance

Definition at line 294 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 271 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 280 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 264 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 127 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]`