32 #ifndef REAL_FEATURE_TRACKER_H_ 33 #define REAL_FEATURE_TRACKER_H_ 41 #include <sensor_msgs/PointCloud2.h> 42 #include <sensor_msgs/Image.h> 43 #include <pcl/point_types.h> 46 #include <opencv2/features2d/features2d.hpp> 47 #include <opencv2/core/core.hpp> 50 #include <camera_info_manager/camera_info_manager.h> 70 PointFeatureTracker(
double loop_period_ns,
bool using_pc =
false, std::string ft_ns = std::string(
""));
123 virtual void setFullRGBDPC(PointCloudPCL::ConstPtr full_rgb_pc);
254 std::vector<cv::Point2f>& points_in_2d, std::vector<bool>& points_status,
255 std::vector<cv::Point3d>& points_in_3d);
cv_bridge::CvImagePtr _occlusion_msk_img_ptr
std::vector< std::vector< float > > _feat_quality
std::vector< std::vector< cv::Point2f > > _predicted_measurements
bool _predicted_state_processed
double _min_depth_difference
virtual cv_bridge::CvImagePtr getPredictedAndLastFeaturesImg()
double _max_allowed_depth
virtual void setOcclusionMaskImg(cv_bridge::CvImagePtr occ_mask_img)
PointCloudPCL::ConstPtr _received_point_cloud
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 meas...
sensor_msgs::CameraInfo::Ptr _camera_info_ptr
cv_bridge::CvImagePtr _tracked_features_img
std::vector< int > _previous_feat_ids_v
std::vector< cv::Point2f > _previous_measurement
void _ProcessPredictedLocationsPC()
cv::Mat _erosion_element_tracking_mat
void _EstimateDetectionMask()
virtual void correctState()
Corrects the predicted state(s). First we track the features using both the prediction with the syste...
virtual cv_bridge::CvImagePtr getPredictingMaskImg()
Eigen::Matrix< double, 3, 4 > _image_plane_proj_mat_eigen
std::vector< cv::Point2f > _corrected_measurement
cv::Mat _feats_detection_msk_mat
FeatureCloudPCLwc::Ptr ft_state_t
int _min_area_size_pixels
virtual cv_bridge::CvImagePtr getDetectingMaskImg()
void _ProcessOcclusionMaskImg()
std::ofstream _features_file
virtual void setFullRGBDPC(PointCloudPCL::ConstPtr full_rgb_pc)
cv::Mat _previous_depth_mat
cv::Mat _predicted_feats_msk_mat
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_p...
double _predicted_state_rcvd_time_ns
cv_bridge::CvImagePtr _detecting_mask_img
double _min_time_to_detect_motion
void _DetectNewFeatures()
ros::NodeHandle _node_handle
ros::Time _previous_time_of_motion_detection
Feature::Id _feature_id_counter
virtual void setCameraInfoMsg(const sensor_msgs::CameraInfo *camera_info)
bool _predicted_state_rcvd
bool _publishing_tracked_feats_with_pred_msk_img
int _erosion_size_detecting
cv::Mat _occlusion_msk_mat
virtual void predictState(double time_interval_ns)
First step when updating the filter. The next state is predicted from current state and system model ...
PointFeatureTracker(double loop_period_ns, bool using_pc=false, std::string ft_ns=std::string(""))
cv_bridge::CvImagePtr _predicting_mask_img
int _erosion_size_tracking
cv_bridge::CvImagePtr _tracked_features_with_pred_msk_img
bool _publishing_predicted_and_past_feats_img
Interface to be implemented by feature trackers It derives from the RecursiveEstimator interface...
ros::Time _predicted_state_rcvd_time
cv_bridge::CvImagePtr _current_bw_img_ptr
virtual cv_bridge::CvImagePtr getDepthEdgesImg()
virtual ft_state_t getState() const
std::vector< cv::Point3d > _corrected_belief_state
cv_bridge::CvImagePtr _depth_img_ptr
virtual cv_bridge::CvImagePtr getRGBImg()
cv::Mat _depth_difference_mask
virtual cv_bridge::CvImagePtr getTrackedFeaturesImg()
double _min_allowed_depth
virtual cv_bridge::CvImagePtr getTrackingMaskImg()
void _InitializeVariables()
cv_bridge::CvImagePtr _rgb_img_ptr
bool _publishing_tracked_feats_img
double _max_interframe_jump
cv_bridge::CvImagePtr _tracking_mask_img
std::vector< bool > _feat_status_v
virtual cv_bridge::CvImagePtr getTrackedFeaturesWithPredictionMaskImg()
virtual void setDynamicReconfigureValues(feature_tracker::FeatureTrackerDynReconfConfig &config)
Set the messages that have to be estimated and published.
virtual ~PointFeatureTracker()
bool _camera_info_msg_rcvd
std::vector< int > _received_prediction_ids
double _canny_low_threshold
bool _occlusion_msk_img_rcvd
bool _attention_to_motion
cv_bridge::CvImagePtr _depth_edges_img
cv::Mat _erosion_element_detecting_mat
virtual cv_bridge::CvImagePtr getDepthImg()
int _Retrieve3DCoords(std::vector< cv::Point2f > &points_in_2d, std::vector< bool > &points_status, std::vector< cv::Point3d > &points_in_3d)
int _min_number_of_features
cv_bridge::CvImagePtr _previous_bw_img_ptr
virtual void predictMeasurement()
Second step when updating the filter. The next measurement is predicted from the predicted next state...
std::vector< cv::Point3d > _previous_belief_state
cv_bridge::CvImagePtr _predicted_and_previous_features_img
cv::Mat _feats_tracking_msk_mat
std::vector< int > _current_feat_ids_v