35 #define BOOST_PARAMETER_MAX_ARITY 7 36 #include <pcl/kdtree/kdtree_flann.h> 37 #include <pcl/kdtree/impl/kdtree_flann.hpp> 40 #include <pcl/recognition/cg/geometric_consistency.h> 46 DiagnosticNodelet::onInit();
47 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
48 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
50 srv_->setCallback (f);
51 pub_output_ = advertise<geometry_msgs::PoseStamped>(*
pnh_,
"output", 1);
54 reference_sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
58 reference_sync_->registerCallback(
66 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
81 Config& config, uint32_t level)
89 const sensor_msgs::PointCloud2::ConstPtr& model_cloud_msg,
90 const sensor_msgs::PointCloud2::ConstPtr& model_feature_msg)
102 const sensor_msgs::PointCloud2::ConstPtr& scene_cloud_msg,
103 const sensor_msgs::PointCloud2::ConstPtr& scene_feature_msg)
111 pcl::PointCloud<pcl::SHOT352>::Ptr
112 scene_feature (
new pcl::PointCloud<pcl::SHOT352>);
113 pcl::PointCloud<pcl::PointNormal>::Ptr
114 scene_cloud (
new pcl::PointCloud<pcl::PointNormal>);
118 pcl::CorrespondencesPtr model_scene_corrs (
new pcl::Correspondences ());
119 pcl::KdTreeFLANN<pcl::SHOT352> match_search;
122 for (
size_t i = 0; i < scene_feature->size(); ++i)
124 std::vector<int> neigh_indices(1);
125 std::vector<float> neigh_sqr_dists(1);
126 if (!pcl_isfinite (scene_feature->at(i).descriptor[0])) {
130 = match_search.nearestKSearch(scene_feature->at(i), 1,
131 neigh_indices, neigh_sqr_dists);
134 if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25
f) {
135 pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
136 model_scene_corrs->push_back (corr);
140 pcl::GeometricConsistencyGrouping<pcl::PointNormal, pcl::PointNormal> gc_clusterer;
145 gc_clusterer.setSceneCloud(scene_cloud);
146 gc_clusterer.setModelSceneCorrespondences(model_scene_corrs);
149 std::vector<pcl::Correspondences> clustered_corrs;
150 std::vector<Eigen::Matrix4f,
151 Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
152 gc_clusterer.recognize(rototranslations, clustered_corrs);
153 if (rototranslations.size() > 0) {
154 NODELET_INFO(
"detected %lu objects", rototranslations.size());
155 Eigen::Matrix4f result_mat = rototranslations[0];
156 Eigen::Affine3f affine(result_mat);
157 geometry_msgs::PoseStamped ros_pose;
159 ros_pose.header = scene_cloud_msg->header;
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::GeometricConsistencyGrouping, nodelet::Nodelet)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > reference_sync_
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
pcl::PointCloud< pcl::SHOT352 >::Ptr reference_feature_
#define NODELET_ERROR_THROTTLE(rate,...)
virtual void referenceCallback(const sensor_msgs::PointCloud2::ConstPtr &model_cloud_msg, const sensor_msgs::PointCloud2::ConstPtr &model_feature_msg)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
ros::Publisher pub_output_cloud_
ros::Publisher pub_output_
pcl::PointCloud< pcl::PointNormal >::Ptr reference_cloud_
virtual void unsubscribe()
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_feature_
GeometricConsistencyGroupingConfig Config
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
#define NODELET_INFO(...)
Nodelet implementation of jsk_pcl/GeometricConsistencyGrouping.
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_reference_cloud_
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
virtual void recognize(const sensor_msgs::PointCloud2::ConstPtr &scene_cloud_msg, const sensor_msgs::PointCloud2::ConstPtr &scene_feature_msg)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void configCallback(Config &config, uint32_t level)
callback for dynamic_reconfigure
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_reference_feature_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_