43 #include <pcl/common/centroid.h> 44 #include <pcl/features/boundary.h> 45 #include <pcl/filters/extract_indices.h> 46 #include <pcl/filters/project_inliers.h> 47 #include <pcl/sample_consensus/method_types.h> 48 #include <pcl/sample_consensus/model_types.h> 49 #include <pcl/segmentation/sac_segmentation.h> 56 DiagnosticNodelet::onInit();
58 srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
59 dynamic_reconfigure::Server<Config>::CallbackType
f =
63 pub_class_ = advertise<jsk_recognition_msgs::ClassificationResult>(*
pnh_,
"output", 1);
65 advertise<jsk_recognition_msgs::ClusterPointIndices>(*
pnh_,
"debug/boundary_indices", 1);
67 advertise<sensor_msgs::PointCloud2>(*
pnh_,
"debug/projected_cloud", 1);
79 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(
queue_size_);
99 if (config.sac_radius_limit_min < config.sac_radius_limit_max) {
121 const sensor_msgs::PointCloud2::ConstPtr& ros_normal,
122 const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& ros_indices,
123 const jsk_recognition_msgs::PolygonArray::ConstPtr& ros_polygons)
147 const sensor_msgs::PointCloud2::ConstPtr& ros_normal,
148 const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& ros_indices,
149 const jsk_recognition_msgs::PolygonArray::ConstPtr& ros_polygons)
153 if (!
checkFrameId(ros_cloud, ros_normal, ros_indices, ros_polygons))
return;
155 pcl::PointCloud<PointT>::Ptr input(
new pcl::PointCloud<PointT>);
158 pcl::PointCloud<pcl::Normal>::Ptr normal(
new pcl::PointCloud<pcl::Normal>);
161 pcl::ExtractIndices<PointT> ext_input;
162 ext_input.setInputCloud(input);
163 pcl::ExtractIndices<pcl::Normal> ext_normal;
164 ext_normal.setInputCloud(normal);
166 std::vector<jsk_recognition_utils::Polygon::Ptr>
polygons 169 jsk_recognition_msgs::ClassificationResult
result;
170 result.header = ros_cloud->header;
171 result.classifier =
"primitive_shape_classifier";
172 result.target_names.push_back(
"box");
173 result.target_names.push_back(
"circle");
174 result.target_names.push_back(
"other");
176 pcl::PointCloud<PointT>::Ptr projected_cloud(
new pcl::PointCloud<PointT>);
177 std::vector<pcl::PointIndices::Ptr> boundary_indices;
180 for (
size_t i = 0; i < ros_indices->cluster_indices.size(); ++i)
182 pcl::PointIndices::Ptr indices(
new pcl::PointIndices);
183 indices->indices = ros_indices->cluster_indices[i].indices;
184 NODELET_DEBUG_STREAM(
"Estimating cluster #" << i <<
" (" << indices->indices.size() <<
" points)");
186 pcl::PointCloud<PointT>::Ptr cluster_cloud(
new pcl::PointCloud<PointT>);
187 ext_input.setIndices(indices);
188 ext_input.filter(*cluster_cloud);
190 pcl::PointCloud<pcl::Normal>::Ptr cluster_normal(
new pcl::PointCloud<pcl::Normal>);
191 ext_normal.setIndices(indices);
192 ext_normal.filter(*cluster_normal);
194 pcl::ModelCoefficients::Ptr support_plane(
new pcl::ModelCoefficients);
201 pcl::PointIndices::Ptr
b(
new pcl::PointIndices);
202 pcl::PointCloud<PointT>::Ptr pc(
new pcl::PointCloud<PointT>);
203 float circle_likelihood, box_likelihood;
204 estimate(cluster_cloud, cluster_normal, support_plane,
206 circle_likelihood, box_likelihood);
207 boundary_indices.push_back(std::move(b));
208 *projected_cloud += *pc;
212 result.labels.push_back(1);
213 result.label_names.push_back(
"circle");
214 result.label_proba.push_back(circle_likelihood);
217 result.labels.push_back(0);
218 result.label_names.push_back(
"box");
219 result.label_proba.push_back(box_likelihood);
222 result.labels.push_back(3);
223 result.label_names.push_back(
"other");
224 result.label_proba.push_back(0.0);
230 sensor_msgs::PointCloud2 ros_projected_cloud;
232 ros_projected_cloud.header = ros_cloud->header;
237 jsk_recognition_msgs::ClusterPointIndices ros_boundary_indices;
238 ros_boundary_indices.header = ros_cloud->header;
239 for (
size_t i = 0; i < boundary_indices.size(); ++i)
241 pcl_msgs::PointIndices ri;
243 ros_boundary_indices.cluster_indices.push_back(ri);
253 const pcl::PointCloud<pcl::Normal>::Ptr& normal,
254 const pcl::ModelCoefficients::Ptr& plane,
255 pcl::PointIndices::Ptr& boundary_indices,
256 pcl::PointCloud<PointT>::Ptr& projected_cloud,
257 float& circle_likelihood,
258 float& box_likelihood)
261 pcl::PointCloud<pcl::Boundary>::Ptr boundaries(
new pcl::PointCloud<pcl::Boundary>);
262 pcl::BoundaryEstimation<PointT, pcl::Normal, pcl::Boundary>
b;
263 b.setInputCloud(cloud);
264 b.setInputNormals(normal);
265 b.setSearchMethod(
typename pcl::search::KdTree<PointT>::Ptr(
new pcl::search::KdTree<PointT>));
266 b.setAngleThreshold(
DEG2RAD(70));
267 b.setRadiusSearch(0.03);
268 b.compute(*boundaries);
271 for (
size_t i = 0; i < boundaries->points.size(); ++i)
272 if ((
int)boundaries->points[i].boundary_point)
273 boundary_indices->indices.push_back(i);
276 pcl::PointCloud<PointT>::Ptr boundary_cloud(
new pcl::PointCloud<PointT>);
277 pcl::ExtractIndices<PointT> ext;
278 ext.setInputCloud(cloud);
279 ext.setIndices(boundary_indices);
280 ext.filter(*boundary_cloud);
287 pcl::ProjectInliers<PointT> proj;
288 proj.setModelType(pcl::SACMODEL_PLANE);
289 proj.setInputCloud(boundary_cloud);
290 proj.setModelCoefficients(plane);
291 proj.filter(*projected_cloud);
294 pcl::PointIndices::Ptr circle_inliers(
new pcl::PointIndices);
295 pcl::ModelCoefficients::Ptr circle_coeffs(
new pcl::ModelCoefficients);
296 pcl::PointIndices::Ptr line_inliers(
new pcl::PointIndices);
297 pcl::ModelCoefficients::Ptr line_coeffs(
new pcl::ModelCoefficients);
299 pcl::SACSegmentation<PointT> seg;
300 seg.setInputCloud(projected_cloud);
302 seg.setOptimizeCoefficients(
true);
303 seg.setMethodType(pcl::SAC_RANSAC);
305 seg.setModelType(pcl::SACMODEL_CIRCLE3D);
308 seg.segment(*circle_inliers, *circle_coeffs);
310 seg.setOptimizeCoefficients(
true);
311 seg.setMethodType(pcl::SAC_RANSAC);
313 seg.setModelType(pcl::SACMODEL_LINE);
315 seg.segment(*line_inliers, *line_coeffs);
319 1.0f * circle_inliers->indices.size() / projected_cloud->points.size();
321 1.0f * line_inliers->indices.size() / projected_cloud->points.size();
326 NODELET_DEBUG_STREAM(line_inliers->indices.size() <<
" line inliers found");
327 NODELET_DEBUG_STREAM(
"box confidence: " << box_likelihood);
334 const std::vector<jsk_recognition_utils::Polygon::Ptr>& polygons,
335 pcl::ModelCoefficients::Ptr& coeff)
338 pcl::compute3DCentroid(*cloud, c);
339 Eigen::Vector3f centroid(c[0], c[1], c[2]);
340 Eigen::Vector3f projected;
341 for (
size_t i = 0; i < polygons.size(); ++i)
344 p->project(centroid, projected);
345 if (p->isInside(projected)) {
346 p->toCoefficients(coeff->values);
PrimitiveShapeClassifierConfig Config
ros::Publisher pub_class_
std::string getTopic() const
void publish(const boost::shared_ptr< M > &message) const
ros::Publisher pub_boundary_indices_
virtual void configCallback(Config &config, uint32_t level)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
ros::Publisher pub_projected_cloud_
virtual bool estimate(const pcl::PointCloud< PointT >::Ptr &cloud, const pcl::PointCloud< pcl::Normal >::Ptr &normal, const pcl::ModelCoefficients::Ptr &plane, pcl::PointIndices::Ptr &boundary_indices, pcl::PointCloud< PointT >::Ptr &projected_cloud, float &circle_likelihood, float &box_likelihood)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
static Polygon fromROSMsg(const geometry_msgs::Polygon &polygon)
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
message_filters::Subscriber< jsk_recognition_msgs::ClusterPointIndices > sub_indices_
virtual void unsubscribe()
#define NODELET_ERROR_STREAM(...)
double sac_distance_threshold_
message_filters::Subscriber< jsk_recognition_msgs::PolygonArray > sub_polygons_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_cloud_
virtual void process(const sensor_msgs::PointCloud2::ConstPtr &ros_cloud, const sensor_msgs::PointCloud2::ConstPtr &ros_normal, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &ros_indices, const jsk_recognition_msgs::PolygonArray::ConstPtr &ros_polygons)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
#define NODELET_DEBUG_STREAM(...)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_normal_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::PrimitiveShapeClassifier, nodelet::Nodelet)
uint32_t getNumSubscribers() const
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 bool checkFrameId(const sensor_msgs::PointCloud2::ConstPtr &ros_cloud, const sensor_msgs::PointCloud2::ConstPtr &ros_normal, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &ros_indices, const jsk_recognition_msgs::PolygonArray::ConstPtr &ros_polygons)
bool isSameFrameId(const std::string &a, const std::string &b)
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
double sac_radius_limit_min_
virtual bool getSupportPlane(const pcl::PointCloud< PointT >::Ptr &cloud, const std::vector< jsk_recognition_utils::Polygon::Ptr > &polygons, pcl::ModelCoefficients::Ptr &coeff)
double sac_radius_limit_max_