Estimates 6DOF pose from stereo image pairs and IMU data. More...

#include <stereodom.hpp>

Public Member Functions
	Stereodom (std::string &nodeName, std::string &leftCam, std::string &rightCam, std::string &imuTopic)
	Constructor.
	~Stereodom (void)
	Destructor.
Private Member Functions
tf::Transform	baselink2camera (const tf::Transform &odom)
	Convert odometry from the robot base_link frame to camera frame.
tf::Transform	camera2baselink (const tf::Transform &odomC)
	Convert odometry from camera frame to the robot base_link frame.
void	cameraInfoCallback (const sensor_msgs::CameraInfoConstPtr &leftInfo, const sensor_msgs::CameraInfoConstPtr &rightInfo)
	Camera calibration info callback.
void	compensateIMU (tf::Transform &odom)
	Compensate IMU roll and pitch angles in base_link frame.
double	computeFeatureFlow (const std::vector< cv::DMatch > &matches, const std::vector< cv::KeyPoint > &trainKpts, const std::vector< cv::KeyPoint > &queryKpts)
	Compute feature flow between matched key-points.
bool	convertRectifyImages (const sensor_msgs::ImageConstPtr &leftImg, const sensor_msgs::ImageConstPtr &rightImg, cv::Mat &imgL, cv::Mat &imgR)
	Converts images to OpenCV format and rectify both images.
void	dynRecCallback (viodom::stereodomConfig &config, uint32_t level)
	Dynamic reconfigure callback.
void	extractDescriptors (cv::Mat &imgL, cv::Mat &imgR, std::vector< cv::KeyPoint > &keypointsLeft, std::vector< cv::KeyPoint > &keypointsRight, cv::Mat &descriptorsLeft, cv::Mat &descriptorsRight)
	Extract feature descriptors from image key-points.
void	kptsBucketing (std::vector< cv::KeyPoint > &srcKpts, std::vector< cv::KeyPoint > &dstKpts, int width, int height)
	Distribute maxFeatures_ among all buckets. We are considering 6 buckets organized in 2 rows and 3 columns.
bool	leftMatching (std::vector< cv::KeyPoint > &keypointsCurr, std::vector< cv::KeyPoint > &keypointsPrev, cv::Mat &descriptorsCurr, cv::Mat &descriptorsPrev, std::vector< cv::DMatch > &stereoMatchesPrev, std::vector< cv::Point3f > &pclPrev, double &flow, std::vector< cv::Point3f > &points3d, std::vector< cv::Point2f > &projections)
	Match key-points left prev-curr descriptors and generate 3D point cloud.
bool	leftPnP (std::vector< cv::Point3f > &points3d, std::vector< cv::Point2f > &projections, tf::Transform &deltaT)
	Solve PnP problem between previous and current left images.
void	openCVToTf (const cv::Mat &t, const cv::Mat &R, tf::Transform &tf)
	Converts OpenCV translation and rotation into tf::Transform.
void	publishPointCloud (const std::vector< cv::Point3f > pcl, const std_msgs::Header header)
	Publish 3D point cloud.
void	publishTf (const tf::Transform &odom)
	Publish 6DOF pose as TF and geometry_msgs::TransformStamped.
void	selectKeypoints (cv::Mat &imgLeft, cv::Mat &imgRight, std::vector< cv::KeyPoint > &keypointsLeft, std::vector< cv::KeyPoint > &keypointsRight)
	Detects key-points in the images Key-points are sorted based on their score to get the best maxFeatures_ Features are included in buckets in order to force sparseness.
void	stereoCallback (const sensor_msgs::ImageConstPtr &leftImg, const sensor_msgs::ImageConstPtr &rightImg)
	Stereo callback with image rectification.
bool	stereoMatching (std::vector< cv::KeyPoint > &keypointsLeft, std::vector< cv::KeyPoint > &keypointsRight, cv::Mat &descriptorsLeft, cv::Mat &descriptorsRight, std::vector< cv::DMatch > &stereoMatches, std::vector< cv::Point3f > &pcl)
	Match key-points L-R descriptors and generate 3D point cloud.
void	tfToOpenCV (const tf::Transform &tf, cv::Mat &t, cv::Mat &R)
	Converts OpenCV translation and rotation into tf::Transform.
void	updatePreviousStuff (const cv::Mat &imgL, const cv::Mat &imgR)
	Update previous-current keypoints, images, descriptors, matches and point clouds.
Private Attributes
bool	calibInit_
message_filters::TimeSynchronizer < sensor_msgs::CameraInfo, sensor_msgs::CameraInfo >	cameraInfoSync_
cv::Mat	descLeftC_
cv::Mat	descLeftP_
cv::Mat	descRightC_
cv::Mat	descRightP_
int	downsampling_
dynamic_reconfigure::Server < viodom::stereodomConfig > ::CallbackType	dyn_rec_f_
dynamic_reconfigure::Server < viodom::stereodomConfig >	dyn_rec_server_
cv::FastFeatureDetector	fDetector_
cv::BriefDescriptorExtractor	fExtractor_
double	flowThreshold_
cv::Mat	imgC_
cv::Mat	imgCkf_
cv::Mat	imgLP_
cv::Mat	imgRP_
ImuFilter	imu_
image_transport::ImageTransport	it_
double	kfRotTh_
double	kfTrTh_
cv::Mat	KL_
std::vector< cv::KeyPoint >	kptsLeftC_
std::vector< cv::KeyPoint >	kptsLeftP_
std::vector< cv::KeyPoint >	kptsRightC_
std::vector< cv::KeyPoint >	kptsRightP_
cv::Mat	KR_
image_transport::SubscriberFilter	leftImgSubs_
message_filters::Subscriber < sensor_msgs::CameraInfo >	leftInfoSubs_
cv::Mat	mapL1_
cv::Mat	mapL2_
cv::Mat	mapR1_
cv::Mat	mapR2_
RobustMatcher	matcher_
int	maxFeatures_
int	minMatches_
ros::NodeHandle	nh_
tf::Transform	odom_
tf::Transform	odomC_
tf::Transform	odomCkf_
bool	odomInit_
std::vector< cv::Point3f >	pclC_
std::vector< cv::Point3f >	pclP_
ros::Publisher	pcPub_
cv::Mat	PL_
cv::Mat	PR_
bool	publishPc_
image_transport::SubscriberFilter	rightImgSubs_
message_filters::Subscriber < sensor_msgs::CameraInfo >	rightInfoSubs_
cv::Mat	RL_
cv::Mat	RR_
std::string	srcFrameId_
std::vector< cv::DMatch >	stereoMatchesC_
std::vector< cv::DMatch >	stereoMatchesP_
message_filters::TimeSynchronizer < sensor_msgs::Image, sensor_msgs::Image >	stereoSync_
tf::Transform	Tcam22imu0_
tf::TransformBroadcaster	tfBr_
std::string	tgtFrameId_
tf::Transform	Timu02base_
ros::Publisher	transformPub_

Detailed Description

Estimates 6DOF pose from stereo image pairs and IMU data.

Definition at line 78 of file stereodom.hpp.

Constructor & Destructor Documentation

Stereodom::Stereodom	(	std::string &	nodeName,
		std::string &	leftCam,
		std::string &	rightCam,
		std::string &	imuTopic
	)		`[inline]`

Constructor.

Parameters:

nodeName	Node name for publishing topics
leftCam	Name of the left camera
rightCam	Name of the right camera
imuTopic	Name of the IMU data topic

Definition at line 87 of file stereodom.hpp.

Stereodom::~Stereodom ( void ) [inline]

Destructor.

Definition at line 160 of file stereodom.hpp.

Member Function Documentation

tf::Transform Stereodom::baselink2camera ( const tf::Transform & odom ) [inline, private]

Convert odometry from the robot base_link frame to camera frame.

Parameters:

odom	Odometry transform in base_link frame

Returns:: Odometry transform in camera frame

Definition at line 511 of file stereodom.hpp.

tf::Transform Stereodom::camera2baselink ( const tf::Transform & odomC ) [inline, private]

Convert odometry from camera frame to the robot base_link frame.

Parameters:

odomC Odometry transform in camera frame

Returns:: Odometry transform in the robot base_link frame

Definition at line 501 of file stereodom.hpp.

void Stereodom::cameraInfoCallback	(	const sensor_msgs::CameraInfoConstPtr &	leftInfo,
		const sensor_msgs::CameraInfoConstPtr &	rightInfo
	)		`[inline, private]`

Camera calibration info callback.

Parameters:

leftInfo	Left camera calibration data
rightInfo	Right camera calibration data

Definition at line 695 of file stereodom.hpp.

void Stereodom::compensateIMU ( tf::Transform & odom ) [inline, private]

Compensate IMU roll and pitch angles in base_link frame.

Definition at line 487 of file stereodom.hpp.

double Stereodom::computeFeatureFlow	(	const std::vector< cv::DMatch > &	matches,
		const std::vector< cv::KeyPoint > &	trainKpts,
		const std::vector< cv::KeyPoint > &	queryKpts
	)		`[inline, private]`

Compute feature flow between matched key-points.

Parameters:

matches	List of matches
trainKpts	Key-points from 'train' image
queryKpts	Key-points fraom 'query' image

Returns:: Computed feature flow

Definition at line 337 of file stereodom.hpp.

bool Stereodom::convertRectifyImages	(	const sensor_msgs::ImageConstPtr &	leftImg,
		const sensor_msgs::ImageConstPtr &	rightImg,
		cv::Mat &	imgL,
		cv::Mat &	imgR
	)		`[inline, private]`

Converts images to OpenCV format and rectify both images.

Parameters:

[in]	leftImg	Left image in ROS format
[in]	rightImg	Right image in ROS format
[out]	imgL	Rectified left image in OpenCV format
[out]	imgR	Rectified right image in OpenCV format

Definition at line 172 of file stereodom.hpp.

void Stereodom::dynRecCallback	(	viodom::stereodomConfig &	config,
		uint32_t	level
	)		`[inline, private]`

Dynamic reconfigure callback.

Definition at line 747 of file stereodom.hpp.

void Stereodom::extractDescriptors	(	cv::Mat &	imgL,
		cv::Mat &	imgR,
		std::vector< cv::KeyPoint > &	keypointsLeft,
		std::vector< cv::KeyPoint > &	keypointsRight,
		cv::Mat &	descriptorsLeft,
		cv::Mat &	descriptorsRight
	)		`[inline, private]`

Extract feature descriptors from image key-points.

Parameters:

[in]	imgLeft	Left image
[in]	imgRight	Right image
[in]	keypointsLeft	Key-points extracted from left image
[in]	keypointsRight	Key-points extracted from right image
[out]	descriptorsLeft	Feature descriptors for key-points from left image
[out]	descriptorsRight	Feature descriptors for key-points from right image

Definition at line 270 of file stereodom.hpp.

void Stereodom::kptsBucketing	(	std::vector< cv::KeyPoint > &	srcKpts,
		std::vector< cv::KeyPoint > &	dstKpts,
		int	width,
		int	height
	)		`[inline, private]`

Distribute maxFeatures_ among all buckets. We are considering 6 buckets organized in 2 rows and 3 columns.

Parameters:

[in]	srcKpts	Sorted key-points
[out]	dstKpts	Output bucketed key-points
[in]	width	Image width (cols)
[in]	height	Image height (rows)

Definition at line 202 of file stereodom.hpp.

bool Stereodom::leftMatching	(	std::vector< cv::KeyPoint > &	keypointsCurr,
		std::vector< cv::KeyPoint > &	keypointsPrev,
		cv::Mat &	descriptorsCurr,
		cv::Mat &	descriptorsPrev,
		std::vector< cv::DMatch > &	stereoMatchesPrev,
		std::vector< cv::Point3f > &	pclPrev,
		double &	flow,
		std::vector< cv::Point3f > &	points3d,
		std::vector< cv::Point2f > &	projections
	)		`[inline, private]`

Match key-points left prev-curr descriptors and generate 3D point cloud.

Parameters:

[in]	keypointsCurr	Key-points extracted from current left image
[in]	keypointsPrev	Key-points extracted from previous left image
[in]	descriptorsCurr	Feature descriptors for key-points from current left image
[in]	descriptorsPrev	Feature descriptors for key-points from previous left image
[in]	stereoMatchesPrev	Matches between features from previous left image
[in]	pclPrev	3D point cloud based on stereo matches from previous left image
[out]	flow	Feature flow between previous and current left images
[out]	points3d	3D points matched between previous and current left images, taken from pclPrev
[out]	projections	2D projections of matched 3D points in current left image

Definition at line 367 of file stereodom.hpp.

bool Stereodom::leftPnP	(	std::vector< cv::Point3f > &	points3d,
		std::vector< cv::Point2f > &	projections,
		tf::Transform &	deltaT
	)		`[inline, private]`

Solve PnP problem between previous and current left images.

Parameters:

[out]	points3d	3D points matched between previous and current left images, taken from previous 3D point cloud
[out]	projections	2D projections of matched 3D points in current left image
[out]	deltaT	Estimated transform between previous and current left frames

Definition at line 449 of file stereodom.hpp.

void Stereodom::openCVToTf	(	const cv::Mat &	t,
		const cv::Mat &	R,
		tf::Transform &	tf
	)		`[inline, private]`

Converts OpenCV translation and rotation into tf::Transform.

Parameters:

[in]	t	OpenCV translation vector
[in]	R	OpenCV rotation matrix
[out]	transform	ROS tf::Transform element

Definition at line 414 of file stereodom.hpp.

void Stereodom::publishPointCloud	(	const std::vector< cv::Point3f >	pcl,
		const std_msgs::Header	header
	)		`[inline, private]`

Publish 3D point cloud.

Parameters:

[in] header Header for the point cloud message

Definition at line 545 of file stereodom.hpp.

void Stereodom::publishTf ( const tf::Transform & odom ) [inline, private]

Publish 6DOF pose as TF and geometry_msgs::TransformStamped.

Parameters:

odom	Odometry transform to be published

Definition at line 520 of file stereodom.hpp.

void Stereodom::selectKeypoints	(	cv::Mat &	imgLeft,
		cv::Mat &	imgRight,
		std::vector< cv::KeyPoint > &	keypointsLeft,
		std::vector< cv::KeyPoint > &	keypointsRight
	)		`[inline, private]`

Detects key-points in the images Key-points are sorted based on their score to get the best maxFeatures_ Features are included in buckets in order to force sparseness.

Parameters:

[in]	imgLeft	Left image
[in]	imgRight	Right image
[out]	keypointsLeft	Key-points extracted from left image
[out]	keypointsRight	Key-points extracted from right image

Definition at line 242 of file stereodom.hpp.

void Stereodom::stereoCallback	(	const sensor_msgs::ImageConstPtr &	leftImg,
		const sensor_msgs::ImageConstPtr &	rightImg
	)		`[inline, private]`

Stereo callback with image rectification.

Parameters:

leftImg	Left image
rightImg	Right image

Definition at line 594 of file stereodom.hpp.

bool Stereodom::stereoMatching	(	std::vector< cv::KeyPoint > &	keypointsLeft,
		std::vector< cv::KeyPoint > &	keypointsRight,
		cv::Mat &	descriptorsLeft,
		cv::Mat &	descriptorsRight,
		std::vector< cv::DMatch > &	stereoMatches,
		std::vector< cv::Point3f > &	pcl
	)		`[inline, private]`

Match key-points L-R descriptors and generate 3D point cloud.

Parameters:

[in]	keypointsLeft	Key-points extracted from left image
[in]	keypointsRight	Key-points extracted from right image
[in]	descriptorsLeft	Feature descriptors for key-points from left image
[in]	descriptorsRight	Feature descriptors for key-points from right image
[out]	stereoMatches	Resulting matches between features
[out]	pcl	Resulting 3D point cloud based on stereo matches between features