Frame-to-frame ICP motion estimation. More...

#include <motion_estimation_icp.h>

Inheritance diagram for ccny_rgbd::MotionEstimationICP:

Public Member Functions
int	getModelSize () const
	Returns the number of points in the model built from the feature buffer.
bool	getMotionEstimationImpl (RGBDFrame &frame, const tf::Transform &prediction, tf::Transform &motion)
	Main method for estimating the motion given an RGBD frame.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	MotionEstimationICP (const ros::NodeHandle &nh, const ros::NodeHandle &nh_private)
	Constructor from ROS noehandles.
virtual	~MotionEstimationICP ()
	Default destructor.
Private Member Functions
bool	alignICPEuclidean (const Vector3fVector &data_means, tf::Transform &correction)
	Performs ICP alignment using the Euclidean distance for corresopndences.
void	getCorrespEuclidean (const PointCloudFeature &data_cloud, IntVector &data_indices, IntVector &model_indices)
	Performs ICP alignment using the Euclidean distance for corresopndences.
bool	getNNEuclidean (const PointFeature &data_point, int &eucl_nn_idx, double &eucl_dist_sq)
	Finds the nearest Euclidean neighbor.
Private Attributes
std::string	base_frame_
	the moving frame (typically base_link or camera_link)
tf::Transform	f2b_
	fixed frame to base (moving) frame
FeatureHistory	feature_history_
	ring buffer of all the frames
std::string	fixed_frame_
	the fixed frame (typically odom)
double	max_corresp_dist_eucl_
	maximum Euclidean correspondce distance for ICP
double	max_corresp_dist_eucl_sq_
	max squared correspondce distance, derived
int	max_iterations_
	max ICP iterations
int	min_correspondences_
	minimum correspondeces for ICP to continue
PointCloudFeature::Ptr	model_ptr_
	the PointCloud which holds the aggregated history
ros::Publisher	model_publisher_
	the ROS published for the model pointcloud
KdTree	model_tree_
	kdtree of model_ptr_
bool	publish_model_
	if true, the model will be published
double	tf_epsilon_angular_
	angular convergence criteria for ICP
double	tf_epsilon_linear_
	linear convergence criteria for ICP

Detailed Description

Frame-to-frame ICP motion estimation.

The motion is estimated by aligning the current features to the features from a number of previous frames.

Definition at line 43 of file motion_estimation_icp.h.

Constructor & Destructor Documentation

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

Constructor from ROS noehandles.

Parameters:

nh	the public nodehandle
nh_private	the private notehandle

Definition at line 28 of file motion_estimation_icp.cpp.

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

Default destructor.

Definition at line 79 of file motion_estimation_icp.cpp.

Member Function Documentation

bool ccny_rgbd::MotionEstimationICP::alignICPEuclidean	(	const Vector3fVector &	data_means,
		tf::Transform &	correction
	)		`[private]`

Performs ICP alignment using the Euclidean distance for corresopndences.

Parameters:

data_means	a vector of 3x1 matrices, repesenting the 3D positions of the features
correction	reference to the resulting transformation

Return values:

true	the motion estimation was successful
false	the motion estimation failed

Definition at line 152 of file motion_estimation_icp.cpp.

void ccny_rgbd::MotionEstimationICP::getCorrespEuclidean	(	const PointCloudFeature &	data_cloud,
		IntVector &	data_indices,
		IntVector &	model_indices
	)		`[private]`

Performs ICP alignment using the Euclidean distance for corresopndences.

Parameters:

data_cloud	a pointcloud of the 3D positions of the features
data_indices	reference to a vector containting the resulting data indices
model_indices	reference to a vector containting the resulting model indices

Definition at line 208 of file motion_estimation_icp.cpp.

int ccny_rgbd::MotionEstimationICP::getModelSize ( ) const [inline, virtual]

Returns the number of points in the model built from the feature buffer.

Returns:: number of points in model

Reimplemented from ccny_rgbd::MotionEstimation.

Definition at line 75 of file motion_estimation_icp.h.

bool ccny_rgbd::MotionEstimationICP::getMotionEstimationImpl	(	RGBDFrame &	frame,
		const tf::Transform &	prediction,
		tf::Transform &	motion
	)		`[virtual]`

Main method for estimating the motion given an RGBD frame.

Parameters:

frame	the current RGBD frame
prediction	the predicted motion (currently ignored)
motion	the (output) incremental motion, wrt the fixed frame

Return values:

true	the motion estimation was successful
false	the motion estimation failed

Todo:: ignores prediction

Implements ccny_rgbd::MotionEstimation.

Definition at line 84 of file motion_estimation_icp.cpp.

bool ccny_rgbd::MotionEstimationICP::getNNEuclidean	(	const PointFeature &	data_point,
		int &	eucl_nn_idx,
		double &	eucl_dist_sq
	)		`[private]`

Finds the nearest Euclidean neighbor.

Parameters:

data_point	the query 3D data point
eucl_nn_idx	reference to the resulting nearest neigbor index in the model
eucl_dist_sq	reference to the resulting squared distance

Return values: