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 virtual void updateDiagnostic(
00166 diagnostic_updater::DiagnosticStatusWrapper &);
00167
00168 public:
00169 TargetAdaptiveTracking();
00170 virtual void callback(
00171 const sensor_msgs::PointCloud2::ConstPtr &,
00172 const geometry_msgs::PoseStamped::ConstPtr &);
00173 virtual void objInitCallback(
00174 const sensor_msgs::PointCloud2::ConstPtr &,
00175 const sensor_msgs::PointCloud2::ConstPtr &,
00176 const geometry_msgs::PoseStamped::ConstPtr &);
00177 virtual std::vector<pcl::PointIndices::Ptr>
00178 clusterPointIndicesToPointIndices(
00179 const jsk_recognition_msgs::ClusterPointIndicesConstPtr &);
00180 void estimatedPFPose(
00181 const geometry_msgs::PoseStamped::ConstPtr &, PointXYZRPY &);
00182 void voxelizeAndProcessPointCloud(
00183 const pcl::PointCloud<PointT>::Ptr cloud,
00184 const std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr> &,
00185 const std::multimap<uint32_t, uint32_t> &,
00186 std::vector<AdjacentInfo> &,
00187 ModelsPtr &, bool = true, bool = true, bool = true, bool = false);
00188 void targetDescriptiveSurfelsEstimationAndUpdate(
00189 pcl::PointCloud<PointT>::Ptr, const Eigen::Affine3f &,
00190 const TargetAdaptiveTracking::PointXYZRPY &,
00191 const std_msgs::Header);
00192 template<class T>
00193 T targetCandidateToReferenceLikelihood(
00194 const ReferenceModel &, const pcl::PointCloud<PointT>::Ptr,
00195 const pcl::PointCloud<pcl::Normal>::Ptr, const Eigen::Vector4f &,
00196 ReferenceModel * = NULL);
00197 template<class T>
00198 T localVoxelConvexityLikelihood(
00199 Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f);
00200 template<class T>
00201 void estimatePointCloudNormals(
00202 const pcl::PointCloud<PointT>::Ptr, pcl::PointCloud<pcl::Normal>::Ptr,
00203 T = 0.05f, bool = false) const;
00204 void computeCloudClusterRPYHistogram(
00205 const pcl::PointCloud<PointT>::Ptr,
00206 const pcl::PointCloud<pcl::Normal>::Ptr, cv::Mat &) const;
00207 void computeColorHistogram(
00208 const pcl::PointCloud<PointT>::Ptr, cv::Mat &, const int = 16,
00209 const int = 16, bool = true) const;
00210 void computePointFPFH(
00211 const pcl::PointCloud<PointT>::Ptr,
00212 pcl::PointCloud<pcl::Normal>::Ptr, cv::Mat &, bool = true) const;
00213 void compute3DCentroids(
00214 const pcl::PointCloud<PointT>::Ptr, Eigen::Vector4f &) const;
00215 Eigen::Vector4f cloudMeanNormal(
00216 const pcl::PointCloud<pcl::Normal>::Ptr, bool = true);
00217 float computeCoherency(const float, const float);
00218 pcl::PointXYZRGBNormal convertVector4fToPointXyzRgbNormal(
00219 const Eigen::Vector4f &, const Eigen::Vector4f &, const cv::Scalar);
00220 template<typename T>
00221 void getRotationMatrixFromRPY(
00222 const PointXYZRPY &, Eigen::Matrix<T, 3, 3> &);
00223 void computeLocalPairwiseFeautures(
00224 const std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr> &,
00225 const std::map<uint32_t, std::vector<uint32_t> >&,
00226 cv::Mat &, const int = 3);
00227 void processVoxelForReferenceModel(
00228 const std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr>,
00229 const std::multimap<uint32_t, uint32_t>,
00230 const uint32_t, TargetAdaptiveTracking::ReferenceModel *);
00231 void transformModelPrimitives(
00232 const ModelsPtr &, ModelsPtr, const Eigen::Affine3f &);
00233 float templateCloudFilterLenght(
00234 const pcl::PointCloud<PointT>::Ptr);
00235 bool filterPointCloud(
00236 pcl::PointCloud<PointT>::Ptr,
00237 const Eigen::Vector4f, const ModelsPtr, const float);
00238 void processInitCloud(
00239 const pcl::PointCloud<PointT>::Ptr, ModelsPtr);
00240 void backgroundReferenceLikelihood(
00241 const ModelsPtr, const ModelsPtr, std::map<uint32_t, float>);
00242 void filterCloudForBoundingBoxViz(
00243 pcl::PointCloud<PointT>::Ptr, const ModelsPtr, const float = 0.6f);
00244 void computeScatterMatrix(
00245 const pcl::PointCloud<PointT>::Ptr,
00246 const Eigen::Vector4f);
00247 template<typename T, typename U, typename V>
00248 cv::Scalar plotJetColour(T, U, V);
00249 void supervoxelSegmentation(
00250 const pcl::PointCloud<PointT>::Ptr,
00251 std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr > &,
00252 std::multimap<uint32_t, uint32_t> &);
00253 void supervoxelSegmentation(
00254 const pcl::PointCloud<PointT>::Ptr,
00255 std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr > &,
00256 std::multimap<uint32_t, uint32_t> &, const float);
00257 void publishSupervoxel(
00258 const std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr>,
00259 sensor_msgs::PointCloud2 &,
00260 jsk_recognition_msgs::ClusterPointIndices &,
00261 const std_msgs::Header &);
00262 void targetDescriptiveSurfelsIndices(
00263 const jsk_recognition_msgs::ClusterPointIndices &,
00264 const std::vector<uint32_t> &,
00265 jsk_recognition_msgs::ClusterPointIndices &);
00266
00267 };
00268 }
00269
00270 #endif // _TARGET_ADAPTIVE_TRACKING_H_