PointFeatureTracker.h
Go to the documentation of this file.
1 /*
2  * PointFeatureTracker.h
3  *
4  * Author: roberto
5  *
6  * This is a modified implementation of the method for online estimation of kinematic structures described in our paper
7  * "Online Interactive Perception of Articulated Objects with Multi-Level Recursive Estimation Based on Task-Specific Priors"
8  * (Martín-Martín and Brock, 2014).
9  * This implementation can be used to reproduce the results of the paper and to be applied to new research.
10  * The implementation allows also to be extended to perceive different information/models or to use additional sources of information.
11  * A detail explanation of the method and the system can be found in our paper.
12  *
13  * If you are using this implementation in your research, please consider citing our work:
14  *
15 @inproceedings{martinmartin_ip_iros_2014,
16 Title = {Online Interactive Perception of Articulated Objects with Multi-Level Recursive Estimation Based on Task-Specific Priors},
17 Author = {Roberto {Mart\'in-Mart\'in} and Oliver Brock},
18 Booktitle = {Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems},
19 Pages = {2494-2501},
20 Year = {2014},
21 Location = {Chicago, Illinois, USA},
22 Note = {http://www.robotics.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martinmartin_ip_iros_2014.pdf},
23 Url = {http://www.robotics.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martinmartin_ip_iros_2014.pdf},
24 Projectname = {Interactive Perception}
25 }
26  * If you have questions or suggestions, contact us:
27  * roberto.martinmartin@tu-berlin.de
28  *
29  * Enjoy!
30  */
31 
32 #ifndef REAL_FEATURE_TRACKER_H_
33 #define REAL_FEATURE_TRACKER_H_
34 
36 
37 #include <ros/ros.h>
41 #include <sensor_msgs/PointCloud2.h>
42 #include <sensor_msgs/Image.h>
43 #include <pcl/point_types.h>
44 #include <cv_bridge/cv_bridge.h>
46 #include <opencv2/features2d/features2d.hpp>
47 #include <opencv2/core/core.hpp>
48 //#include <opencv2/core/gpumat.hpp>
49 //#include <opencv2/gpu/gpu.hpp>
50 #include <camera_info_manager/camera_info_manager.h>
51 
52 #include <iostream>
53 #include <fstream>
54 
55 namespace omip
56 {
57 
64 {
65 public:
66 
70  PointFeatureTracker(double loop_period_ns, bool using_pc = false, std::string ft_ns = std::string(""));
71 
75  virtual ~PointFeatureTracker();
76 
82  virtual void predictState(double time_interval_ns);
83 
92  virtual void predictMeasurement();
93 
100  virtual void correctState();
101 
108  virtual void addPredictedState(const ft_state_t& predicted_state, const double& predicted_state_timestamp_ns);
109 
115  virtual void setDynamicReconfigureValues(feature_tracker::FeatureTrackerDynReconfConfig &config);
116 
117 
123  virtual void setFullRGBDPC(PointCloudPCL::ConstPtr full_rgb_pc);
124 
131  virtual void setOcclusionMaskImg(cv_bridge::CvImagePtr occ_mask_img);
132 
138  virtual void setCameraInfoMsg(const sensor_msgs::CameraInfo* camera_info);
139 
145  virtual ft_state_t getState() const;
146 
148 
150 
156 
163 
169 
175 
181 
187 
193 
200  virtual void setSelfOcclusionPositive(bool so_positive);
201 
202 
203 protected:
204 
208  void _ReadParameters();
209 
213  void _InitializeVariables();
214 
215  void _EstimateDetectionMask();
216 
217  void _DetectNewFeatures();
218 
222  void _TrackFeatures();
223 
227  void _ProcessDepthImg();
228 
232  void _ProcessRGBImg();
233 
238 
243 
253  int _Retrieve3DCoords(
254  std::vector<cv::Point2f>& points_in_2d, std::vector<bool>& points_status,
255  std::vector<cv::Point3d>& points_in_3d);
256 
263  int _Compare3DCoords();
264 
266  sensor_msgs::CameraInfo::Ptr _camera_info_ptr;
267 
268  // PCL variables
269  PointCloudPCL::ConstPtr _received_point_cloud;
270 
271  // OpenCV variables
277 
278  Eigen::Matrix<double, 3, 4> _image_plane_proj_mat_eigen;
279 
280  std::vector<std::vector<cv::Point2f> > _predicted_measurements;
283  std::vector<bool> _feat_status_v;
285  std::vector<int> _received_prediction_ids;
286 
289 
290  // Flags
294 
295  // Parameters
299 
301  std::string _ft_ns;
302 
303  std::vector<std::vector<float> > _feat_quality;
304 
305  std::ofstream _features_file;
306 
308 };
309 }
310 
311 #endif /* REAL_FEATURE_TRACKER_H_ */
cv_bridge::CvImagePtr _occlusion_msk_img_ptr
std::vector< std::vector< float > > _feat_quality
std::vector< std::vector< cv::Point2f > > _predicted_measurements
virtual cv_bridge::CvImagePtr getPredictedAndLastFeaturesImg()
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
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
FeatureCloudPCLwc::Ptr ft_state_t
virtual cv_bridge::CvImagePtr getDetectingMaskImg()
virtual void setFullRGBDPC(PointCloudPCL::ConstPtr full_rgb_pc)
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...
cv_bridge::CvImagePtr _detecting_mask_img
virtual void setCameraInfoMsg(const sensor_msgs::CameraInfo *camera_info)
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
cv_bridge::CvImagePtr _tracked_features_with_pred_msk_img
Interface to be implemented by feature trackers It derives from the RecursiveEstimator interface...
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()
virtual cv_bridge::CvImagePtr getTrackedFeaturesImg()
virtual cv_bridge::CvImagePtr getTrackingMaskImg()
cv_bridge::CvImagePtr _rgb_img_ptr
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.
std::vector< int > _received_prediction_ids
cv_bridge::CvImagePtr _depth_edges_img
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)
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
std::vector< int > _current_feat_ids_v


feature_tracker
Author(s): Roberto Martín-Martín
autogenerated on Mon Jun 10 2019 14:06:08