target_adaptive_tracking.h
Go to the documentation of this file.
1 
2 #ifndef _TARGET_ADAPTIVE_TRACKING_H_
3 #define _TARGET_ADAPTIVE_TRACKING_H_
4 
5 #include <string>
6 
7 #include <ros/ros.h>
8 #include <ros/console.h>
9 
13 
14 #include <sensor_msgs/Image.h>
16 #include <sensor_msgs/PointCloud2.h>
17 #include <cv_bridge/cv_bridge.h>
18 
19 #include <opencv2/core/core.hpp>
20 #include <opencv2/opencv.hpp>
21 #include <boost/thread/mutex.hpp>
22 
24 #include <pcl/point_cloud.h>
25 #include <pcl/point_types.h>
26 #include <pcl/io/pcd_io.h>
27 #include <pcl/correspondence.h>
28 #include <pcl/recognition/cg/hough_3d.h>
29 #include <pcl/recognition/cg/geometric_consistency.h>
30 #include <pcl/kdtree/kdtree_flann.h>
31 #include <pcl/kdtree/impl/kdtree_flann.hpp>
32 #include <pcl/common/transforms.h>
33 #include <pcl/filters/extract_indices.h>
34 #include <pcl/segmentation/segment_differences.h>
35 #include <pcl/octree/octree.h>
36 #include <pcl/surface/concave_hull.h>
37 #include <pcl/segmentation/extract_clusters.h>
38 #include <pcl/filters/filter.h>
39 #include <pcl/common/centroid.h>
40 #include <pcl/features/fpfh_omp.h>
41 #include <pcl/features/vfh.h>
42 #include <pcl/features/gfpfh.h>
43 #include <pcl/features/pfh.h>
44 #include <pcl/features/cvfh.h>
45 #include <pcl/features/normal_3d_omp.h>
46 #include <pcl/tracking/tracking.h>
47 #include <pcl/common/common.h>
48 #include <pcl/registration/distances.h>
49 #include <pcl/filters/passthrough.h>
50 #include <pcl/segmentation/supervoxel_clustering.h>
51 
52 #include <tf/transform_listener.h>
55 #include <geometry_msgs/Pose.h>
56 #include <geometry_msgs/PoseStamped.h>
57 #include <std_msgs/Header.h>
58 #include <dynamic_reconfigure/server.h>
59 #include <jsk_recognition_msgs/ClusterPointIndices.h>
62 #include <jsk_pcl_ros/TargetAdaptiveTrackingConfig.h>
63 
64 namespace jsk_pcl_ros
65 {
67  {
68  typedef pcl::PointXYZRGB PointT;
69 
70  struct AdjacentInfo
71  {
72  uint32_t voxel_index;
73  std::map<uint32_t, std::vector<uint32_t> > adjacent_voxel_indices;
74  };
75 
77  {
78  pcl::PointCloud<PointT>::Ptr cluster_cloud;
82  pcl::PointCloud<pcl::Normal>::Ptr cluster_normals;
83  Eigen::Vector4f cluster_centroid;
84  Eigen::Vector3f centroid_distance;
85  cv::Mat neigbour_pfh;
87  bool flag;
88  uint32_t supervoxel_index;
89  std::vector<int> history_window;
91  };
92 
93  private:
94  typedef std::vector<ReferenceModel> Models;
96  typedef pcl::tracking::ParticleXYZRPY PointXYZRPY;
97  typedef std::vector<PointXYZRPY> MotionHistory;
99  sensor_msgs::PointCloud2,
100  geometry_msgs::PoseStamped> SyncPolicy;
105  sensor_msgs::PointCloud2,
106  sensor_msgs::PointCloud2,
107  geometry_msgs::PoseStamped> ObjectSyncPolicy;
113 
114  // ros::NodeHandle pnh_;
125 
127  ModelsPtr object_reference_;
129  MotionHistory motion_history_;
131  Eigen::Vector4f current_pose_;
132  Eigen::Vector4f previous_pose_;
133  PointXYZRPY tracker_pose_;
135  pcl::PointCloud<PointT>::Ptr previous_template_;
138 
139  bool use_tf_;
140  std::string parent_frame_id_;
141  std::string child_frame_id_;
142 
149 
151  float threshold_;
162 
163  typedef jsk_pcl_ros::TargetAdaptiveTrackingConfig Config;
164  virtual void configCallback(Config &, uint32_t);
166 
167  protected:
168  virtual void onInit();
169  virtual void subscribe();
170  virtual void unsubscribe();
171 
172  public:
174  virtual void callback(
175  const sensor_msgs::PointCloud2::ConstPtr &,
176  const geometry_msgs::PoseStamped::ConstPtr &);
177  virtual void objInitCallback(
178  const sensor_msgs::PointCloud2::ConstPtr &,
179  const sensor_msgs::PointCloud2::ConstPtr &,
180  const geometry_msgs::PoseStamped::ConstPtr &);
181  virtual std::vector<pcl::PointIndices::Ptr>
183  const jsk_recognition_msgs::ClusterPointIndicesConstPtr &);
184  void estimatedPFPose(
185  const geometry_msgs::PoseStamped::ConstPtr &, PointXYZRPY &);
187  const pcl::PointCloud<PointT>::Ptr cloud,
188  const std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr> &,
189  const std::multimap<uint32_t, uint32_t> &,
190  std::vector<AdjacentInfo> &,
191  ModelsPtr &, bool = true, bool = true, bool = true, bool = false);
193  pcl::PointCloud<PointT>::Ptr, const Eigen::Affine3f &,
195  const std_msgs::Header);
196  template<class T>
198  const ReferenceModel &, const pcl::PointCloud<PointT>::Ptr,
199  const pcl::PointCloud<pcl::Normal>::Ptr, const Eigen::Vector4f &,
200  ReferenceModel * = NULL);
201  template<class T>
203  Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f);
204  template<class T>
206  const pcl::PointCloud<PointT>::Ptr, pcl::PointCloud<pcl::Normal>::Ptr,
207  T = 0.05f, bool = false) const;
209  const pcl::PointCloud<PointT>::Ptr,
210  const pcl::PointCloud<pcl::Normal>::Ptr, cv::Mat &) const;
212  const pcl::PointCloud<PointT>::Ptr, cv::Mat &, const int = 16,
213  const int = 16, bool = true) const;
214  void computePointFPFH(
215  const pcl::PointCloud<PointT>::Ptr,
216  pcl::PointCloud<pcl::Normal>::Ptr, cv::Mat &, bool = true) const;
217  void compute3DCentroids(
218  const pcl::PointCloud<PointT>::Ptr, Eigen::Vector4f &) const;
219  Eigen::Vector4f cloudMeanNormal(
220  const pcl::PointCloud<pcl::Normal>::Ptr, bool = true);
221  float computeCoherency(const float, const float);
222  pcl::PointXYZRGBNormal convertVector4fToPointXyzRgbNormal(
223  const Eigen::Vector4f &, const Eigen::Vector4f &, const cv::Scalar);
224  template<typename T>
226  const PointXYZRPY &, Eigen::Matrix<T, 3, 3> &);
228  const std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr> &,
229  const std::map<uint32_t, std::vector<uint32_t> >&,
230  cv::Mat &, const int = 3);
232  const std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr>,
233  const std::multimap<uint32_t, uint32_t>,
234  const uint32_t, TargetAdaptiveTracking::ReferenceModel *);
236  const ModelsPtr &, ModelsPtr, const Eigen::Affine3f &);
238  const pcl::PointCloud<PointT>::Ptr);
239  bool filterPointCloud(
240  pcl::PointCloud<PointT>::Ptr,
241  const Eigen::Vector4f, const ModelsPtr, const float);
242  void processInitCloud(
243  const pcl::PointCloud<PointT>::Ptr, ModelsPtr);
245  const ModelsPtr, const ModelsPtr, std::map<uint32_t, float>);
247  pcl::PointCloud<PointT>::Ptr, const ModelsPtr, const float = 0.6f);
249  const pcl::PointCloud<PointT>::Ptr,
250  const Eigen::Vector4f);
251  template<typename T, typename U, typename V>
252  cv::Scalar plotJetColour(T, U, V);
254  const pcl::PointCloud<PointT>::Ptr,
255  std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr > &,
256  std::multimap<uint32_t, uint32_t> &);
258  const pcl::PointCloud<PointT>::Ptr,
259  std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr > &,
260  std::multimap<uint32_t, uint32_t> &, const float);
261  void publishSupervoxel(
262  const std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr>,
263  sensor_msgs::PointCloud2 &,
264  jsk_recognition_msgs::ClusterPointIndices &,
265  const std_msgs::Header &);
267  const jsk_recognition_msgs::ClusterPointIndices &,
268  const std::vector<uint32_t> &,
269  jsk_recognition_msgs::ClusterPointIndices &);
270 
271  };
272 }
273 
274 #endif // _TARGET_ADAPTIVE_TRACKING_H_
void publishSupervoxel(const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr >, sensor_msgs::PointCloud2 &, jsk_recognition_msgs::ClusterPointIndices &, const std_msgs::Header &)
message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, geometry_msgs::PoseStamped > SyncPolicy
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_cloud_
void estimatedPFPose(const geometry_msgs::PoseStamped::ConstPtr &, PointXYZRPY &)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_bkgd_cloud_
message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, geometry_msgs::PoseStamped > ObjectSyncPolicy
Eigen::Vector4f cloudMeanNormal(const pcl::PointCloud< pcl::Normal >::Ptr, bool=true)
void filterCloudForBoundingBoxViz(pcl::PointCloud< PointT >::Ptr, const ModelsPtr, const float=0.6f)
message_filters::Subscriber< geometry_msgs::PoseStamped > sub_pose_
void transformModelPrimitives(const ModelsPtr &, ModelsPtr, const Eigen::Affine3f &)
pointer T
message_filters::Subscriber< geometry_msgs::PoseStamped > sub_obj_pose_
void processInitCloud(const pcl::PointCloud< PointT >::Ptr, ModelsPtr)
jsk_pcl_ros::TargetAdaptiveTrackingConfig Config
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_obj_cloud_
void getRotationMatrixFromRPY(const PointXYZRPY &, Eigen::Matrix< T, 3, 3 > &)
std::map< uint32_t, std::vector< uint32_t > > adjacent_voxel_indices
void computeScatterMatrix(const pcl::PointCloud< PointT >::Ptr, const Eigen::Vector4f)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
T localVoxelConvexityLikelihood(Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f)
pcl::tracking::ParticleXYZRPY PointXYZRPY
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
void backgroundReferenceLikelihood(const ModelsPtr, const ModelsPtr, std::map< uint32_t, float >)
void computeLocalPairwiseFeautures(const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &, const std::map< uint32_t, std::vector< uint32_t > > &, cv::Mat &, const int=3)
void computePointFPFH(const pcl::PointCloud< PointT >::Ptr, pcl::PointCloud< pcl::Normal >::Ptr, cv::Mat &, bool=true) const
pcl::PointXYZRGBNormal convertVector4fToPointXyzRgbNormal(const Eigen::Vector4f &, const Eigen::Vector4f &, const cv::Scalar)
void computeCloudClusterRPYHistogram(const pcl::PointCloud< PointT >::Ptr, const pcl::PointCloud< pcl::Normal >::Ptr, cv::Mat &) const
std::vector< ReferenceModel > Models
virtual std::vector< pcl::PointIndices::Ptr > clusterPointIndicesToPointIndices(const jsk_recognition_msgs::ClusterPointIndicesConstPtr &)
boost::mutex mutex
global mutex.
void supervoxelSegmentation(const pcl::PointCloud< PointT >::Ptr, std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &, std::multimap< uint32_t, uint32_t > &)
void voxelizeAndProcessPointCloud(const pcl::PointCloud< PointT >::Ptr cloud, const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &, const std::multimap< uint32_t, uint32_t > &, std::vector< AdjacentInfo > &, ModelsPtr &, bool=true, bool=true, bool=true, bool=false)
T targetCandidateToReferenceLikelihood(const ReferenceModel &, const pcl::PointCloud< PointT >::Ptr, const pcl::PointCloud< pcl::Normal >::Ptr, const Eigen::Vector4f &, ReferenceModel *=NULL)
virtual void objInitCallback(const sensor_msgs::PointCloud2::ConstPtr &, const sensor_msgs::PointCloud2::ConstPtr &, const geometry_msgs::PoseStamped::ConstPtr &)
void estimatePointCloudNormals(const pcl::PointCloud< PointT >::Ptr, pcl::PointCloud< pcl::Normal >::Ptr, T=0.05f, bool=false) const
void computeColorHistogram(const pcl::PointCloud< PointT >::Ptr, cv::Mat &, const int=16, const int=16, bool=true) const
pcl::PointCloud< PointT >::Ptr previous_template_
virtual void callback(const sensor_msgs::PointCloud2::ConstPtr &, const geometry_msgs::PoseStamped::ConstPtr &)
void targetDescriptiveSurfelsIndices(const jsk_recognition_msgs::ClusterPointIndices &, const std::vector< uint32_t > &, jsk_recognition_msgs::ClusterPointIndices &)
void targetDescriptiveSurfelsEstimationAndUpdate(pcl::PointCloud< PointT >::Ptr, const Eigen::Affine3f &, const TargetAdaptiveTracking::PointXYZRPY &, const std_msgs::Header)
void processVoxelForReferenceModel(const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr >, const std::multimap< uint32_t, uint32_t >, const uint32_t, TargetAdaptiveTracking::ReferenceModel *)
boost::shared_ptr< message_filters::Synchronizer< ObjectSyncPolicy > > obj_sync_
float templateCloudFilterLenght(const pcl::PointCloud< PointT >::Ptr)
bool filterPointCloud(pcl::PointCloud< PointT >::Ptr, const Eigen::Vector4f, const ModelsPtr, const float)
void compute3DCentroids(const pcl::PointCloud< PointT >::Ptr, Eigen::Vector4f &) const


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47