#include <PointFeatureTracker.h>

Inheritance diagram for omip::PointFeatureTracker:

Public Member Functions
virtual void	addPredictedState (const ft_state_t &predicted_state, const double &predicted_state_timestamp_ns)
	Add a new prediction about the state generated in the higher level (as prediction about the next measurement)
virtual void	correctState ()
	Corrects the predicted state(s). First we track the features using both the prediction with the system model (copy of the previous feature locations) and if there is a prediction from the higher level (RBT) we track features using it too. We then decide which prediction is the "most likely" based on the quality measurement that the feature tracker returns and use the new location to update the belief state.
virtual cv_bridge::CvImagePtr	getDepthEdgesImg ()
virtual cv_bridge::CvImagePtr	getDepthImg ()
virtual cv_bridge::CvImagePtr	getDetectingMaskImg ()
virtual cv_bridge::CvImagePtr	getPredictedAndLastFeaturesImg ()
virtual cv_bridge::CvImagePtr	getPredictingMaskImg ()
virtual cv_bridge::CvImagePtr	getRGBImg ()
virtual ft_state_t	getState () const
virtual cv_bridge::CvImagePtr	getTrackedFeaturesImg ()
virtual cv_bridge::CvImagePtr	getTrackedFeaturesWithPredictionMaskImg ()
virtual cv_bridge::CvImagePtr	getTrackingMaskImg ()
	PointFeatureTracker (double loop_period_ns, bool using_pc=false, std::string ft_ns=std::string(""))
virtual void	predictMeasurement ()
	Second step when updating the filter. The next measurement is predicted from the predicted next state (part of the RecursiveEstimatorInterface)
virtual void	predictState (double time_interval_ns)
	First step when updating the filter. The next state is predicted from current state and system model (part of the RecursiveEstimatorInterace) In the FeatureTracker the system model is the most simple one: no change in the state. So the predicted state is just the current state.
virtual void	setCameraInfoMsg (const sensor_msgs::CameraInfo *camera_info)
virtual void	setDynamicReconfigureValues (feature_tracker::FeatureTrackerDynReconfConfig &config)
	Set the messages that have to be estimated and published.
virtual void	setFullRGBDPC (PointCloudPCL::ConstPtr full_rgb_pc)
virtual void	setOcclusionMaskImg (cv_bridge::CvImagePtr occ_mask_img)
virtual void	setSelfOcclusionPositive (bool so_positive)
	Set if the self occlusion image will be black in the background and white for the occluded area (so_positive = true, new) or white in the background and black in the occluded area (so_positive = false, old)
virtual	~PointFeatureTracker ()
Protected Member Functions
int	_Compare3DCoords ()
void	_DetectNewFeatures ()
void	_EstimateDetectionMask ()
void	_InitializeVariables ()
void	_ProcessDepthImg ()
void	_ProcessOcclusionMaskImg ()
void	_ProcessPredictedLocationsPC ()
void	_ProcessRGBImg ()
void	_ReadParameters ()
int	_Retrieve3DCoords (std::vector< cv::Point2f > &points_in_2d, std::vector< bool > &points_status, std::vector< cv::Point3d > &points_in_3d)
void	_TrackFeatures ()
Protected Attributes
bool	_attention_to_motion
cv::Mat	_aux1_mat
bool	_camera_info_msg_rcvd
sensor_msgs::CameraInfo::Ptr	_camera_info_ptr
cv::Mat	_canny_edges_mat
int	_canny_kernel_size
double	_canny_low_threshold
int	_canny_ratio
std::vector< cv::Point3d >	_corrected_belief_state
std::vector< cv::Point2f >	_corrected_measurement
cv_bridge::CvImagePtr	_current_bw_img_ptr
std::vector< int >	_current_feat_ids_v
cv::Mat	_depth_difference_mask
cv_bridge::CvImagePtr	_depth_edges_img
cv_bridge::CvImagePtr	_depth_img_ptr
cv::Mat	_depth_mat
cv_bridge::CvImagePtr	_detecting_mask_img
cv::Mat	_erosion_element_detecting_mat
cv::Mat	_erosion_element_tracking_mat
int	_erosion_size_detecting
int	_erosion_size_tracking
std::vector< std::vector< float > >	_feat_quality
std::vector< bool >	_feat_status_v
cv::Mat	_feats_detection_msk_mat
cv::Mat	_feats_tracking_msk_mat
Feature::Id	_feature_id_counter
std::ofstream	_features_file
bool	_first_frame
int	_frame_counter
std::string	_ft_ns
Eigen::Matrix< double, 3, 4 >	_image_plane_proj_mat_eigen
double	_max_allowed_depth
double	_max_interframe_jump
double	_min_allowed_depth
int	_min_area_size_pixels
double	_min_depth_difference
double	_min_feat_quality
int	_min_number_of_features
double	_min_time_to_detect_motion
ros::NodeHandle	_node_handle
int	_number_of_features
cv_bridge::CvImagePtr	_occlusion_msk_img_ptr
bool	_occlusion_msk_img_rcvd
cv::Mat	_occlusion_msk_mat
cv_bridge::CvImagePtr	_predicted_and_previous_features_img
cv::Mat	_predicted_feats_msk_mat
std::vector< std::vector < cv::Point2f > >	_predicted_measurements
bool	_predicted_state_processed
bool	_predicted_state_rcvd
ros::Time	_predicted_state_rcvd_time
double	_predicted_state_rcvd_time_ns
cv_bridge::CvImagePtr	_predicting_mask_img
std::vector< cv::Point3d >	_previous_belief_state
cv_bridge::CvImagePtr	_previous_bw_img_ptr
cv::Mat	_previous_depth_mat
std::vector< int >	_previous_feat_ids_v
std::vector< cv::Point2f >	_previous_measurement
ros::Time	_previous_time_of_motion_detection
int	_processing_factor
bool	_publishing_predicted_and_past_feats_img
bool	_publishing_tracked_feats_img
bool	_publishing_tracked_feats_with_pred_msk_img
PointCloudPCL::ConstPtr	_received_point_cloud
std::vector< int >	_received_prediction_ids
cv_bridge::CvImagePtr	_rgb_img_ptr
double	_sensor_fps
bool	_so_positive
cv_bridge::CvImagePtr	_tracked_features_img
cv_bridge::CvImagePtr	_tracked_features_with_pred_msk_img
cv_bridge::CvImagePtr	_tracking_mask_img
bool	_use_motion_mask
bool	_using_pc

Detailed Description

Class PointFeatureTracker It implements the interface FeatureTracker Tracks features in the RGB stream and returns a PCL point cloud with their new 3D coordinates

Definition at line 63 of file PointFeatureTracker.h.

Constructor & Destructor Documentation

PointFeatureTracker::PointFeatureTracker	(	double	loop_period_ns,
		bool	using_pc = `false`,
		std::string	ft_ns = `std::string("")`
	)

Constructor

Definition at line 33 of file PointFeatureTracker.cpp.

PointFeatureTracker::~PointFeatureTracker ( ) [virtual]

Destructor

Definition at line 71 of file PointFeatureTracker.cpp.

Member Function Documentation

int PointFeatureTracker::_Compare3DCoords ( ) [protected]

Compare current and previous 3D coordinates of the tracked Features to reject jumping ones. This limits the maximum velocity of a pixel in 3D space. 3D locations is too large) its status changes to false

Returns:: - Number of 3D Features that jumped (rejected)

Definition at line 966 of file PointFeatureTracker.cpp.

void PointFeatureTracker::_DetectNewFeatures ( ) [protected]

Definition at line 666 of file PointFeatureTracker.cpp.

void PointFeatureTracker::_EstimateDetectionMask ( ) [protected]

Definition at line 655 of file PointFeatureTracker.cpp.

void PointFeatureTracker::_InitializeVariables ( ) [protected]

Initialize matrices and variables used during the execution

Definition at line 599 of file PointFeatureTracker.cpp.

void PointFeatureTracker::_ProcessDepthImg ( ) [protected]

Process the previously received depth img

Definition at line 229 of file PointFeatureTracker.cpp.

void PointFeatureTracker::_ProcessOcclusionMaskImg ( ) [protected]

Process the previously received occlusion mask img

Definition at line 379 of file PointFeatureTracker.cpp.

void omip::PointFeatureTracker::_ProcessPredictedLocationsPC ( ) [protected]

Process the previously received predicted locations pointcloud

void PointFeatureTracker::_ProcessRGBImg ( ) [protected]

Process the previously received rgb img

Definition at line 372 of file PointFeatureTracker.cpp.

void PointFeatureTracker::_ReadParameters ( ) [protected]

Read parameters from ROS

Definition at line 552 of file PointFeatureTracker.cpp.

int PointFeatureTracker::_Retrieve3DCoords	(	std::vector< cv::Point2f > &	points_in_2d,
		std::vector< bool > &	points_status,
		std::vector< cv::Point3d > &	points_in_3d
	)		`[protected]`

Retrieve the 3D coordinates of a set of 2D Features

Parameters:

points_in_2d	- 2D locations of the Features in image plane
points_ids	- ID of the Features
points_status	- Status of the Features: If the 3D coordinates of a Feature cannot be retrieved, its status changes to false
points_in_3d	- Returning vector with the 3D coordinates and the Id of the Features

Returns:: - Number of 2D Features whose 3D coordinates couldn't be retrieved (rejected)

Definition at line 884 of file PointFeatureTracker.cpp.

void PointFeatureTracker::_TrackFeatures ( ) [protected]

Track the features that were detected previously and detect new features if needed

Definition at line 726 of file PointFeatureTracker.cpp.

void PointFeatureTracker::addPredictedState	(	const ft_state_t &	predicted_state,
		const double &	predicted_state_timestamp_ns
	)		`[virtual]`

Add a new prediction about the state generated in the higher level (as prediction about the next measurement)

Parameters:

predicted_state Predicted next state

Definition at line 212 of file PointFeatureTracker.cpp.

void PointFeatureTracker::correctState ( ) [virtual]

Corrects the predicted state(s). First we track the features using both the prediction with the system model (copy of the previous feature locations) and if there is a prediction from the higher level (RBT) we track features using it too. We then decide which prediction is the "most likely" based on the quality measurement that the feature tracker returns and use the new location to update the belief state.

Implements omip::FeatureTracker.

Definition at line 173 of file PointFeatureTracker.cpp.

cv_bridge::CvImagePtr PointFeatureTracker::getDepthEdgesImg ( ) [virtual]

Get the image with the depth edges, used to reject Features that are on edges

Returns:: - Shared pointer to the image of depth edges

Reimplemented from omip::FeatureTracker.

Definition at line 475 of file PointFeatureTracker.cpp.

cv_bridge::CvImagePtr PointFeatureTracker::getDepthImg ( ) [virtual]

Reimplemented from omip::FeatureTracker.

Definition at line 458 of file PointFeatureTracker.cpp.

cv_bridge::CvImagePtr PointFeatureTracker::getDetectingMaskImg ( ) [virtual]

Get the mask used to detect new features. Combines edges, max depth and mask of current features (to not detect them)

Returns:: - Shared pointer to the image of detecting mask

Reimplemented from omip::FeatureTracker.

Definition at line 499 of file PointFeatureTracker.cpp.

cv_bridge::CvImagePtr PointFeatureTracker::getPredictedAndLastFeaturesImg ( ) [virtual]

Get the image with the last feature locations and the predicted feature locations

Returns:: - Shared pointer to the image of the predicted and last features

Reimplemented from omip::FeatureTracker.

Definition at line 507 of file PointFeatureTracker.cpp.

cv_bridge::CvImagePtr PointFeatureTracker::getPredictingMaskImg ( ) [virtual]

Get the image with the depth edges, used to reject Features that are on edges

Returns:: - Shared pointer to the image of depth edges

Reimplemented from omip::FeatureTracker.

Definition at line 483 of file PointFeatureTracker.cpp.

cv_bridge::CvImagePtr PointFeatureTracker::getRGBImg ( ) [virtual]

Reimplemented from omip::FeatureTracker.

Definition at line 453 of file PointFeatureTracker.cpp.

ft_state_t PointFeatureTracker::getState ( ) const [virtual]

Get a point cloud with the 3D locations and the unique Feature Id (L value) of the last tracked features

Returns:: - Shared pointer to the 3D XYZL point cloud of tracked Features

Implements omip::FeatureTracker.

Definition at line 416 of file PointFeatureTracker.cpp.

cv_bridge::CvImagePtr PointFeatureTracker::getTrackedFeaturesImg ( ) [virtual]

Get the last tracked Features as marks in the RGB image

Returns:: - Shared pointer to the image with the 2D tracked Feature locations marked

Reimplemented from omip::FeatureTracker.

Definition at line 421 of file PointFeatureTracker.cpp.

cv_bridge::CvImagePtr PointFeatureTracker::getTrackedFeaturesWithPredictionMaskImg ( ) [virtual]

Get a pointer to a RGB image with markers where the Features have been tracked in the last step and only showing the RGB area where the features are searched

Returns:: - Shared pointer to the RGB image with markers and searching areas

Reimplemented from omip::FeatureTracker.

Definition at line 463 of file PointFeatureTracker.cpp.

cv_bridge::CvImagePtr PointFeatureTracker::getTrackingMaskImg ( ) [virtual]

Get the mask used to track features. Combines edges, predictions mask and max depth

Returns:: - Shared pointer to the image of tracking mask

Reimplemented from omip::FeatureTracker.

Definition at line 491 of file PointFeatureTracker.cpp.

void PointFeatureTracker::predictMeasurement ( ) [virtual]

Second step when updating the filter. The next measurement is predicted from the predicted next state (part of the RecursiveEstimatorInterface)

In the FeatureTracker it is not clear what are the measurements:

From one side, the measurements are the RGB-D frames coming from the sensor. But we cannot predict entire RGB-D frames from 3D feature locations!
From the other side, the measurements are the 2D locations of the tracked features because we can predict them by projecting the predicted state(s) into the image plane.

Implements omip::RecursiveEstimatorFilterInterface< ft_state_t, ft_measurement_t >.

Definition at line 88 of file PointFeatureTracker.cpp.

void PointFeatureTracker::predictState ( double time_interval_ns ) [virtual]

First step when updating the filter. The next state is predicted from current state and system model (part of the RecursiveEstimatorInterace) In the FeatureTracker the system model is the most simple one: no change in the state. So the predicted state is just the current state.

Implements omip::FeatureTracker.

Definition at line 81 of file PointFeatureTracker.cpp.

void PointFeatureTracker::setCameraInfoMsg ( const sensor_msgs::CameraInfo * camera_info ) [virtual]

Set the camera parameters to be used to project the 3D predicted Feature locations to the image plane and convert them into image coordinates

Parameters:

camera_info - Pointer to the CameraInfo message containing the camera parameters

Reimplemented from omip::FeatureTracker.

Definition at line 391 of file PointFeatureTracker.cpp.

void PointFeatureTracker::setDynamicReconfigureValues ( feature_tracker::FeatureTrackerDynReconfConfig & config ) [virtual]

Set the messages that have to be estimated and published.

Parameters:

config Value from Dynamic Reconfigure

Implements omip::FeatureTracker.

Definition at line 76 of file PointFeatureTracker.cpp.

void PointFeatureTracker::setFullRGBDPC ( PointCloudPCL::ConstPtr full_rgb_pc ) [virtual]

Set a new RGBD point cloud to use for tracking in 3D. The RGBD point cloud is used to retrieve the 3D coordinates of the features tracked in 2D

Parameters:

full_rgb_pc - Shared pointer to the new RGBD point cloud

Reimplemented from omip::FeatureTracker.

Definition at line 218 of file PointFeatureTracker.cpp.

void PointFeatureTracker::setOcclusionMaskImg ( cv_bridge::CvImagePtr occ_mask_img ) [virtual]

Set a new occlusion mask image for tracking in 3D. The occlusion mask predicts the area of the RGB image that will be occluded (where it will be visible) the arm of the robot when it executes an interaction. Features on this area will be rejected.

Parameters:

occ_mask_img - Shared pointer to the new occlusion mask image

Reimplemented from omip::FeatureTracker.

Definition at line 223 of file PointFeatureTracker.cpp.

void PointFeatureTracker::setSelfOcclusionPositive ( bool so_positive ) [virtual]

Set if the self occlusion image will be black in the background and white for the occluded area (so_positive = true, new) or white in the background and black in the occluded area (so_positive = false, old)

Parameters:

so_positive Flag value

Definition at line 547 of file PointFeatureTracker.cpp.