2 #ifndef _TARGET_ADAPTIVE_TRACKING_H_ 3 #define _TARGET_ADAPTIVE_TRACKING_H_ 14 #include <sensor_msgs/Image.h> 16 #include <sensor_msgs/PointCloud2.h> 19 #include <opencv2/core/core.hpp> 20 #include <opencv2/opencv.hpp> 21 #include <boost/thread/mutex.hpp> 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> 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> 94 typedef std::vector<ReferenceModel>
Models;
99 sensor_msgs::PointCloud2,
105 sensor_msgs::PointCloud2,
106 sensor_msgs::PointCloud2,
163 typedef jsk_pcl_ros::TargetAdaptiveTrackingConfig
Config;
175 const sensor_msgs::PointCloud2::ConstPtr &,
176 const geometry_msgs::PoseStamped::ConstPtr &);
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 &);
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);
199 const pcl::PointCloud<pcl::Normal>::Ptr,
const Eigen::Vector4f &,
203 Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f);
206 const pcl::PointCloud<PointT>::Ptr, pcl::PointCloud<pcl::Normal>::Ptr,
207 T = 0.05
f,
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;
215 const pcl::PointCloud<PointT>::Ptr,
216 pcl::PointCloud<pcl::Normal>::Ptr, cv::Mat &,
bool =
true)
const;
218 const pcl::PointCloud<PointT>::Ptr, Eigen::Vector4f &)
const;
220 const pcl::PointCloud<pcl::Normal>::Ptr,
bool =
true);
223 const Eigen::Vector4f &,
const Eigen::Vector4f &,
const cv::Scalar);
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>,
236 const ModelsPtr &, ModelsPtr,
const Eigen::Affine3f &);
238 const pcl::PointCloud<PointT>::Ptr);
240 pcl::PointCloud<PointT>::Ptr,
241 const Eigen::Vector4f,
const ModelsPtr,
const float);
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.6
f);
249 const pcl::PointCloud<PointT>::Ptr,
250 const Eigen::Vector4f);
251 template<
typename T,
typename U,
typename 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);
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 &);
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 &)
virtual void unsubscribe()
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 &)
std::string child_frame_id_
ModelsPtr background_reference_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_bkgd_cloud_
double normal_importance_
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)
Eigen::Vector4f previous_pose_
virtual void configCallback(Config &, uint32_t)
ros::Publisher pub_centroids_
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 &)
message_filters::Subscriber< geometry_msgs::PoseStamped > sub_obj_pose_
bool update_tracker_reference_
void processInitCloud(const pcl::PointCloud< PointT >::Ptr, ModelsPtr)
jsk_pcl_ros::TargetAdaptiveTrackingConfig Config
std::vector< PointXYZRPY > MotionHistory
ModelsPtr object_reference_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_obj_cloud_
ros::Publisher pub_inliers_
void getRotationMatrixFromRPY(const PointXYZRPY &, Eigen::Matrix< T, 3, 3 > &)
cv::Scalar plotJetColour(T, U, V)
MotionHistory motion_history_
std::map< uint32_t, std::vector< uint32_t > > adjacent_voxel_indices
Eigen::Vector4f current_pose_
void computeScatterMatrix(const pcl::PointCloud< PointT >::Ptr, const Eigen::Vector4f)
double spatial_importance_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
cv::Mat cluster_color_hist
T localVoxelConvexityLikelihood(Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f)
ros::Publisher pub_cloud_
tf::Transform previous_transform_
pcl::tracking::ParticleXYZRPY PointXYZRPY
Eigen::Vector3f centroid_distance
ros::Publisher pub_templ_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
ros::Publisher pub_sindices_
pcl::PointCloud< PointT >::Ptr cluster_cloud
void backgroundReferenceLikelihood(const ModelsPtr, const ModelsPtr, std::map< uint32_t, float >)
std::vector< int > history_window
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
Eigen::Vector4f cluster_centroid
float computeCoherency(const float, const float)
boost::shared_ptr< Models > ModelsPtr
std::vector< ReferenceModel > Models
virtual std::vector< pcl::PointIndices::Ptr > clusterPointIndicesToPointIndices(const jsk_recognition_msgs::ClusterPointIndicesConstPtr &)
PointXYZRPY tracker_pose_
bool update_filter_template_
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
uint32_t supervoxel_index
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_
pcl::PointCloud< pcl::Normal >::Ptr cluster_normals
AdjacentInfo cluster_neigbors
float templateCloudFilterLenght(const pcl::PointCloud< PointT >::Ptr)
bool filterPointCloud(pcl::PointCloud< PointT >::Ptr, const Eigen::Vector4f, const ModelsPtr, const float)
ros::Publisher pub_scloud_
ros::Publisher pub_normal_
std::string parent_frame_id_
void compute3DCentroids(const pcl::PointCloud< PointT >::Ptr, Eigen::Vector4f &) const