00001
00002 #ifndef _TARGET_ADAPTIVE_TRACKING_H_
00003 #define _TARGET_ADAPTIVE_TRACKING_H_
00004
00005 #include <ros/ros.h>
00006 #include <ros/console.h>
00007
00008 #include <message_filters/subscriber.h>
00009 #include <message_filters/synchronizer.h>
00010 #include <message_filters/sync_policies/approximate_time.h>
00011
00012 #include <sensor_msgs/Image.h>
00013 #include <sensor_msgs/image_encodings.h>
00014 #include <sensor_msgs/PointCloud2.h>
00015 #include <cv_bridge/cv_bridge.h>
00016
00017 #include <opencv2/core/core.hpp>
00018 #include <opencv2/opencv.hpp>
00019 #include <boost/thread/mutex.hpp>
00020
00021 #include <pcl_conversions/pcl_conversions.h>
00022 #include <pcl/point_cloud.h>
00023 #include <pcl/point_types.h>
00024 #include <pcl/io/pcd_io.h>
00025 #include <pcl/correspondence.h>
00026 #include <pcl/recognition/cg/hough_3d.h>
00027 #include <pcl/recognition/cg/geometric_consistency.h>
00028 #include <pcl/kdtree/kdtree_flann.h>
00029 #include <pcl/kdtree/impl/kdtree_flann.hpp>
00030 #include <pcl/common/transforms.h>
00031 #include <pcl/filters/extract_indices.h>
00032 #include <pcl/segmentation/segment_differences.h>
00033 #include <pcl/octree/octree.h>
00034 #include <pcl/surface/concave_hull.h>
00035 #include <pcl/segmentation/extract_clusters.h>
00036 #include <pcl/filters/filter.h>
00037 #include <pcl/common/centroid.h>
00038 #include <pcl/features/fpfh_omp.h>
00039 #include <pcl/features/vfh.h>
00040 #include <pcl/features/gfpfh.h>
00041 #include <pcl/features/pfh.h>
00042 #include <pcl/features/cvfh.h>
00043 #include <pcl/features/normal_3d_omp.h>
00044 #include <pcl/tracking/tracking.h>
00045 #include <pcl/common/common.h>
00046 #include <pcl/registration/distances.h>
00047 #include <pcl/filters/passthrough.h>
00048 #include <pcl/segmentation/supervoxel_clustering.h>
00049
00050 #include <tf/transform_listener.h>
00051 #include <tf/transform_broadcaster.h>
00052 #include <tf_conversions/tf_eigen.h>
00053 #include <geometry_msgs/Pose.h>
00054 #include <geometry_msgs/PoseStamped.h>
00055 #include <std_msgs/Header.h>
00056 #include <dynamic_reconfigure/server.h>
00057 #include <jsk_recognition_msgs/ClusterPointIndices.h>
00058 #include <jsk_topic_tools/diagnostic_nodelet.h>
00059 #include <jsk_pcl_ros/pcl_conversion_util.h>
00060 #include <jsk_pcl_ros/TargetAdaptiveTrackingConfig.h>
00061
00062 namespace jsk_pcl_ros
00063 {
00064 class TargetAdaptiveTracking: public jsk_topic_tools::DiagnosticNodelet
00065 {
00066 typedef pcl::PointXYZRGB PointT;
00067
00068 struct AdjacentInfo
00069 {
00070 uint32_t voxel_index;
00071 std::map<uint32_t, std::vector<uint32_t> > adjacent_voxel_indices;
00072 };
00073
00074 struct ReferenceModel
00075 {
00076 pcl::PointCloud<PointT>::Ptr cluster_cloud;
00077 cv::Mat cluster_vfh_hist;
00078 cv::Mat cluster_color_hist;
00079 AdjacentInfo cluster_neigbors;
00080 pcl::PointCloud<pcl::Normal>::Ptr cluster_normals;
00081 Eigen::Vector4f cluster_centroid;
00082 Eigen::Vector3f centroid_distance;
00083 cv::Mat neigbour_pfh;
00084 int query_index;
00085 bool flag;
00086 uint32_t supervoxel_index;
00087 std::vector<int> history_window;
00088 int match_counter;
00089 };
00090
00091 private:
00092 typedef std::vector<ReferenceModel> Models;
00093 typedef boost::shared_ptr<Models> ModelsPtr;
00094 typedef pcl::tracking::ParticleXYZRPY PointXYZRPY;
00095 typedef std::vector<PointXYZRPY> MotionHistory;
00096 typedef message_filters::sync_policies::ApproximateTime<
00097 sensor_msgs::PointCloud2,
00098 geometry_msgs::PoseStamped> SyncPolicy;
00099 message_filters::Subscriber<sensor_msgs::PointCloud2> sub_cloud_;
00100 message_filters::Subscriber<geometry_msgs::PoseStamped> sub_pose_;
00101 boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> >sync_;
00102 typedef message_filters::sync_policies::ApproximateTime<
00103 sensor_msgs::PointCloud2,
00104 sensor_msgs::PointCloud2,
00105 geometry_msgs::PoseStamped> ObjectSyncPolicy;
00106 message_filters::Subscriber<sensor_msgs::PointCloud2> sub_obj_cloud_;
00107 message_filters::Subscriber<sensor_msgs::PointCloud2> sub_bkgd_cloud_;
00108 message_filters::Subscriber<geometry_msgs::PoseStamped> sub_obj_pose_;
00109 boost::shared_ptr<
00110 message_filters::Synchronizer<ObjectSyncPolicy> >obj_sync_;
00111
00112
00113 ros::Publisher pub_cloud_;
00114 ros::Publisher pub_templ_;
00115 ros::Publisher pub_sindices_;
00116 ros::Publisher pub_scloud_;
00117 ros::Publisher pub_normal_;
00118 ros::Publisher pub_tdp_;
00119 ros::Publisher pub_inliers_;
00120 ros::Publisher pub_centroids_;
00121 ros::Publisher pub_pose_;
00122 ros::Publisher pub_prob_;
00123
00124 int init_counter_;
00125 ModelsPtr object_reference_;
00126 ModelsPtr background_reference_;
00127 MotionHistory motion_history_;
00128 int update_counter_;
00129 Eigen::Vector4f current_pose_;
00130 Eigen::Vector4f previous_pose_;
00131 PointXYZRPY tracker_pose_;
00132 tf::Transform previous_transform_;
00133 pcl::PointCloud<PointT>::Ptr previous_template_;
00134 float growth_rate_;
00135 float previous_distance_;
00136
00137 double color_importance_;
00138 double spatial_importance_;
00139 double normal_importance_;
00140 double voxel_resolution_;
00141 double seed_resolution_;
00142 bool use_transform_;
00143
00144 int min_cluster_size_;
00145 float threshold_;
00146 int bin_size_;
00147 float eps_distance_;
00148 int eps_min_samples_;
00149 float vfh_scaling_;
00150 float color_scaling_;
00151 float structure_scaling_;
00152 bool update_tracker_reference_;
00153 bool update_filter_template_;
00154 int history_window_size_;
00155 boost::mutex mutex_;
00156
00157 typedef jsk_pcl_ros::TargetAdaptiveTrackingConfig Config;
00158 virtual void configCallback(Config &, uint32_t);
00159 boost::shared_ptr<dynamic_reconfigure::Server<Config> > srv_;
00160
00161 protected:
00162 virtual void onInit();
00163 virtual void subscribe();
00164 virtual void unsubscribe();
00165
00166 public:
00167 TargetAdaptiveTracking();
00168 virtual void callback(
00169 const sensor_msgs::PointCloud2::ConstPtr &,
00170 const geometry_msgs::PoseStamped::ConstPtr &);
00171 virtual void objInitCallback(
00172 const sensor_msgs::PointCloud2::ConstPtr &,
00173 const sensor_msgs::PointCloud2::ConstPtr &,
00174 const geometry_msgs::PoseStamped::ConstPtr &);
00175 virtual std::vector<pcl::PointIndices::Ptr>
00176 clusterPointIndicesToPointIndices(
00177 const jsk_recognition_msgs::ClusterPointIndicesConstPtr &);
00178 void estimatedPFPose(
00179 const geometry_msgs::PoseStamped::ConstPtr &, PointXYZRPY &);
00180 void voxelizeAndProcessPointCloud(
00181 const pcl::PointCloud<PointT>::Ptr cloud,
00182 const std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr> &,
00183 const std::multimap<uint32_t, uint32_t> &,
00184 std::vector<AdjacentInfo> &,
00185 ModelsPtr &, bool = true, bool = true, bool = true, bool = false);
00186 void targetDescriptiveSurfelsEstimationAndUpdate(
00187 pcl::PointCloud<PointT>::Ptr, const Eigen::Affine3f &,
00188 const TargetAdaptiveTracking::PointXYZRPY &,
00189 const std_msgs::Header);
00190 template<class T>
00191 T targetCandidateToReferenceLikelihood(
00192 const ReferenceModel &, const pcl::PointCloud<PointT>::Ptr,
00193 const pcl::PointCloud<pcl::Normal>::Ptr, const Eigen::Vector4f &,
00194 ReferenceModel * = NULL);
00195 template<class T>
00196 T localVoxelConvexityLikelihood(
00197 Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f);
00198 template<class T>
00199 void estimatePointCloudNormals(
00200 const pcl::PointCloud<PointT>::Ptr, pcl::PointCloud<pcl::Normal>::Ptr,
00201 T = 0.05f, bool = false) const;
00202 void computeCloudClusterRPYHistogram(
00203 const pcl::PointCloud<PointT>::Ptr,
00204 const pcl::PointCloud<pcl::Normal>::Ptr, cv::Mat &) const;
00205 void computeColorHistogram(
00206 const pcl::PointCloud<PointT>::Ptr, cv::Mat &, const int = 16,
00207 const int = 16, bool = true) const;
00208 void computePointFPFH(
00209 const pcl::PointCloud<PointT>::Ptr,
00210 pcl::PointCloud<pcl::Normal>::Ptr, cv::Mat &, bool = true) const;
00211 void compute3DCentroids(
00212 const pcl::PointCloud<PointT>::Ptr, Eigen::Vector4f &) const;
00213 Eigen::Vector4f cloudMeanNormal(
00214 const pcl::PointCloud<pcl::Normal>::Ptr, bool = true);
00215 float computeCoherency(const float, const float);
00216 pcl::PointXYZRGBNormal convertVector4fToPointXyzRgbNormal(
00217 const Eigen::Vector4f &, const Eigen::Vector4f &, const cv::Scalar);
00218 template<typename T>
00219 void getRotationMatrixFromRPY(
00220 const PointXYZRPY &, Eigen::Matrix<T, 3, 3> &);
00221 void computeLocalPairwiseFeautures(
00222 const std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr> &,
00223 const std::map<uint32_t, std::vector<uint32_t> >&,
00224 cv::Mat &, const int = 3);
00225 void processVoxelForReferenceModel(
00226 const std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr>,
00227 const std::multimap<uint32_t, uint32_t>,
00228 const uint32_t, TargetAdaptiveTracking::ReferenceModel *);
00229 void transformModelPrimitives(
00230 const ModelsPtr &, ModelsPtr, const Eigen::Affine3f &);
00231 float templateCloudFilterLenght(
00232 const pcl::PointCloud<PointT>::Ptr);
00233 bool filterPointCloud(
00234 pcl::PointCloud<PointT>::Ptr,
00235 const Eigen::Vector4f, const ModelsPtr, const float);
00236 void processInitCloud(
00237 const pcl::PointCloud<PointT>::Ptr, ModelsPtr);
00238 void backgroundReferenceLikelihood(
00239 const ModelsPtr, const ModelsPtr, std::map<uint32_t, float>);
00240 void filterCloudForBoundingBoxViz(
00241 pcl::PointCloud<PointT>::Ptr, const ModelsPtr, const float = 0.6f);
00242 void computeScatterMatrix(
00243 const pcl::PointCloud<PointT>::Ptr,
00244 const Eigen::Vector4f);
00245 template<typename T, typename U, typename V>
00246 cv::Scalar plotJetColour(T, U, V);
00247 void supervoxelSegmentation(
00248 const pcl::PointCloud<PointT>::Ptr,
00249 std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr > &,
00250 std::multimap<uint32_t, uint32_t> &);
00251 void supervoxelSegmentation(
00252 const pcl::PointCloud<PointT>::Ptr,
00253 std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr > &,
00254 std::multimap<uint32_t, uint32_t> &, const float);
00255 void publishSupervoxel(
00256 const std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr>,
00257 sensor_msgs::PointCloud2 &,
00258 jsk_recognition_msgs::ClusterPointIndices &,
00259 const std_msgs::Header &);
00260 void targetDescriptiveSurfelsIndices(
00261 const jsk_recognition_msgs::ClusterPointIndices &,
00262 const std::vector<uint32_t> &,
00263 jsk_recognition_msgs::ClusterPointIndices &);
00264
00265 };
00266 }
00267
00268 #endif // _TARGET_ADAPTIVE_TRACKING_H_