Motion estimation based on aligning sparse features against a persistent, dynamic model. More...

#include <motion_estimation_icp_prob_model.h>

Inheritance diagram for ccny_rgbd::MotionEstimationICPProbModel:

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	MotionEstimationICPProbModel (const ros::NodeHandle &nh, const ros::NodeHandle &nh_private)
	Constructor from ROS noehandles.
bool	saveSrvCallback (ccny_rgbd::Save::Request &request, ccny_rgbd::Save::Response &response)
	ROS service to save model to a file.
virtual	~MotionEstimationICPProbModel ()
	Default destructor.
Private Member Functions
void	addToModel (const Vector3f &data_mean, const Matrix3f &data_cov)
	Adds a new point to the model.
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.
bool	getNNMahalanobis (const Vector3f &data_mean, const Matrix3f &data_cov, int &mah_nn_idx, double &mah_dist_sq, IntVector &indices, FloatVector &dists_sq)
	Finds the nearest Mahalanobis neighbor.
void	initializeModelFromData (const Vector3fVector &data_means, const Matrix3fVector &data_covariances)
	Initializes the (empty) model from a set of data means and covariances.
void	publishCovariances ()
	Publish covariance markers of the mdoel for visualization.
bool	saveModel (const std::string &filename)
	ROS service to save model to a file.
void	updateModelFromData (const Vector3fVector &data_means, const Matrix3fVector &data_covariances)
	Updates the (non-empty) model from a set of data means and covariances.
Private Attributes
std::string	base_frame_
	The moving frame (usually camera_link or base_link)
Matrix3fVector	covariances_
	Vector of model feature covariances.
ros::Publisher	covariances_publisher_
	The publisher for the covariance markers.
tf::Transform	f2b_
	Transform from fixed to moving frame.
std::string	fixed_frame_
	The fixed frame (usually odom)
Matrix3f	I_
	3x3 Identity matrix
double	max_assoc_dist_mah_
	Maximum Mahalanobis distance for associating points between the data and the model.
double	max_assoc_dist_mah_sq_
	Maximum squared Mahalanobis distance for associating points between the data and the model, derived.
double	max_corresp_dist_eucl_
	Maximum Euclidean correspondce distance for ICP.
double	max_corresp_dist_eucl_sq_
	Maximum Euclidean correspondce distance for ICP, derived.
int	max_iterations_
	Maximum number of ICP iterations.
int	max_model_size_
	Upper bound for how many features to store in the model.
Vector3fVector	means_
	Vector of model feature mean.
int	min_correspondences_
	Minimum number of correspondences for ICP to contuinue.
int	model_idx_
	Current intex in the ring buffer.
PointCloudFeature::Ptr	model_ptr_
	The model point cloud.
ros::Publisher	model_publisher_
	The publisher for the model point cloud.
int	model_size_
	Current model size.
KdTree	model_tree_
	Kdtree of model_ptr_.
int	n_nearest_neighbors_
	How many euclidean neighbors to go through, in a brute force search of the closest Mahalanobis neighbor.
bool	publish_model_
	If true, model point cloud will be published for visualization.
bool	publish_model_cov_
	If true, covariance markers will be published for visualization.
ros::ServiceServer	save_service_
	Service to save the model to file.
double	tf_epsilon_angular_
	Angular convergence criteria for ICP.
double	tf_epsilon_linear_
	Linear convergence criteria for ICP.

Detailed Description

Motion estimation based on aligning sparse features against a persistent, dynamic model.

The model is build from incoming features through a Kalman Filter update step.

Definition at line 48 of file motion_estimation_icp_prob_model.h.

Constructor & Destructor Documentation

ccny_rgbd::MotionEstimationICPProbModel::MotionEstimationICPProbModel	(	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_prob_model.cpp.

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

Default destructor.

Definition at line 96 of file motion_estimation_icp_prob_model.cpp.

Member Function Documentation

void ccny_rgbd::MotionEstimationICPProbModel::addToModel	(	const Vector3f &	data_mean,
		const Matrix3f &	data_cov
	)		`[private]`

Adds a new point to the model.

The model is implemented using a rign buffer. If the number of points in the model has reached the maximum, the new point will overwrite the oldest model point

Parameters:

data_mean	3x1 data point means
data_cov	3x3 data point covariances

Definition at line 101 of file motion_estimation_icp_prob_model.cpp.

bool ccny_rgbd::MotionEstimationICPProbModel::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 194 of file motion_estimation_icp_prob_model.cpp.

void ccny_rgbd::MotionEstimationICPProbModel::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 250 of file motion_estimation_icp_prob_model.cpp.

int ccny_rgbd::MotionEstimationICPProbModel::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 81 of file motion_estimation_icp_prob_model.h.

bool ccny_rgbd::MotionEstimationICPProbModel::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:: : currently ignores prediction

Implements ccny_rgbd::MotionEstimation.

Definition at line 133 of file motion_estimation_icp_prob_model.cpp.

bool ccny_rgbd::MotionEstimationICPProbModel::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:

true	a neighbor was found
false	a neighbor was not found

Definition at line 272 of file motion_estimation_icp_prob_model.cpp.

bool ccny_rgbd::MotionEstimationICPProbModel::getNNMahalanobis	(	const Vector3f &	data_mean,
		const Matrix3f &	data_cov,
		int &	mah_nn_idx,
		double &	mah_dist_sq,
		IntVector &	indices,
		FloatVector &	dists_sq
	)		`[private]`

Finds the nearest Mahalanobis neighbor.

Requests the K nearest Euclidean neighbors (K = n_nearest_neighbors_) using a kdtree, and performs a brute force search for the closest Mahalanobis nighbor. Reasonable values for K are 4 or 8.

Parameters:

data_mean	3x1 matrix of the query 3D data point mean
data_cov	3x3 matrix of the query 3D data point covariance
mah_nn_idx	reference to the resulting nearest neigbor index in the model
mah_dist_sq	reference to the resulting squared Mahalanobis distance
indices	cache vector, pre-allocated with n_nearest_neighbors_ size
dists_sq	cache vector, pre-allocated with n_nearest_neighbors_ size

Return values:

true	a neighbor was found
false	a neighbor was not found

Definition at line 294 of file motion_estimation_icp_prob_model.cpp.

void ccny_rgbd::MotionEstimationICPProbModel::initializeModelFromData	(	const Vector3fVector &	data_means,
		const Matrix3fVector &	data_covariances
	)		`[private]`

Initializes the (empty) model from a set of data means and covariances.

Parameters:

data_means	vector of 3x1 data point means
data_covariances	vector of 3x3 data point covariances

Definition at line 343 of file motion_estimation_icp_prob_model.cpp.

void ccny_rgbd::MotionEstimationICPProbModel::publishCovariances ( ) [private]

Publish covariance markers of the mdoel for visualization.

Definition at line 413 of file motion_estimation_icp_prob_model.cpp.

bool ccny_rgbd::MotionEstimationICPProbModel::saveModel ( const std::string & filename ) [private]

ROS service to save model to a file.

Todo:: this is only saving the point cloud, not the actual distributions

Parameters:

filename filename to save mdoel to

Return values:

true	saved succesfully
false	saving failed

Todo:: also save Eigen means and covariances

Definition at line 493 of file motion_estimation_icp_prob_model.cpp.

bool ccny_rgbd::MotionEstimationICPProbModel::saveSrvCallback	(	ccny_rgbd::Save::Request &	request,
		ccny_rgbd::Save::Response &	response
	)

ROS service to save model to a file.

Parameters:

request	ROS service request
response	ROS service response

Return values:

true	saved succesfully
false	saving failed

Definition at line 477 of file motion_estimation_icp_prob_model.cpp.

void ccny_rgbd::MotionEstimationICPProbModel::updateModelFromData	(	const Vector3fVector &	data_means,
		const Matrix3fVector &	data_covariances
	)		`[private]`