35 #define BOOST_PARAMETER_MAX_ARITY 7 39 #include <pcl/sample_consensus/method_types.h> 40 #include <pcl/sample_consensus/model_types.h> 41 #include <pcl/segmentation/sac_segmentation.h> 46 delta_(
Eigen::Vector3f(0, 0, 0)),
57 Eigen::Vector3f new_delta = segment->toSegment()->getDirection();
58 if (new_delta.dot(
delta_) < 0) {
59 new_delta = - new_delta;
61 delta_ = ((1 - tau) *
delta_ + tau * new_delta).normalized();
64 pcl::PointCloud<pcl::PointXYZ>::Ptr new_cloud = segment->getPoints();
65 for (
size_t i = 0; i < new_cloud->points.size(); i++) {
66 points_->points.push_back(new_cloud->points[i]);
69 pcl::PointCloud<pcl::PointXYZ>::Ptr new_raw_cloud = segment->getRawPoints();
70 for (
size_t i = 0; i < new_raw_cloud->points.size(); i++) {
71 raw_points_->points.push_back(new_raw_cloud->points[i]);
88 for (std::vector<LineSegment::Ptr>::iterator it =
segments_.begin();
90 if (((*it)->header.stamp - stamp).toSec() < 0) {
100 points_.reset(
new pcl::PointCloud<pcl::PointXYZ>);
102 raw_points_.reset(
new pcl::PointCloud<pcl::PointXYZ>);
103 for (std::vector<LineSegment::Ptr>::iterator it =
segments_.begin();
106 pcl::PointCloud<pcl::PointXYZ>::Ptr segment_points = (*it)->getPoints();
107 for (
size_t i = 0; i < segment_points->points.size(); i++) {
108 points_->points.push_back(segment_points->points[i]);
112 pcl::PointCloud<pcl::PointXYZ>::Ptr segment_points = (*it)->getRawPoints();
113 for (
size_t i = 0; i < segment_points->points.size(); i++) {
114 raw_points_->points.push_back(segment_points->points[i]);
128 DiagnosticNodelet::onInit();
129 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
130 dynamic_reconfigure::Server<Config>::CallbackType
f =
132 srv_->setCallback (f);
134 std::string rotate_type_str;
135 pnh_->param(
"rotate_type", rotate_type_str, std::string(
"tilt_two_way"));
136 if (rotate_type_str ==
"tilt") {
137 rotate_type_ = ROTATION_TILT;
139 else if (rotate_type_str ==
"tilt_two_way") {
140 rotate_type_ = ROTATION_TILT_TWO_WAY;
142 else if (rotate_type_str ==
"spindle") {
143 rotate_type_ = ROTATION_SPINDLE;
146 NODELET_ERROR(
"unknown ~rotate_type: %s", rotate_type_str.c_str());
151 = advertise<sensor_msgs::PointCloud2>(*pnh_,
"output/cloud", 1);
152 pub_inliers_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_,
"output/inliers", 1);
154 = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*pnh_,
"output/coefficients", 1);
156 = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_,
"output/polygons", 1);
157 debug_pub_inliers_before_plane_
158 = advertise<jsk_recognition_msgs::ClusterPointIndices>(
159 *pnh_,
"debug/connect_segments/inliers", 1);
167 ewma_tau_ = config.ewma_tau;
168 segment_connect_normal_threshold_ = config.segment_connect_normal_threshold;
169 outlier_threshold_ = config.outlier_threshold;
174 sub_input_.subscribe(*pnh_,
"input", 1);
175 sub_indices_.subscribe(*pnh_,
"input_indices", 1);
176 sub_coefficients_.subscribe(*pnh_,
"input_coefficients", 1);
177 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
178 sync_->connectInput(sub_input_, sub_indices_, sub_coefficients_);
181 sub_trigger_ = pnh_->subscribe(
"trigger", 1,
187 sub_input_.unsubscribe();
188 sub_indices_.unsubscribe();
189 sub_coefficients_.unsubscribe();
190 sub_trigger_.shutdown();
196 pointclouds_buffer_.removeBefore(stamp);
197 indices_buffer_.removeBefore(stamp);
198 coefficients_buffer_.removeBefore(stamp);
199 segments_buffer_.removeBefore(stamp);
200 for (std::vector<LineSegmentCluster::Ptr>::iterator it = segment_clusters_.begin();
201 it != segment_clusters_.end();) {
202 (*it)->removeBefore(stamp);
203 if ((*it)->isEmpty()) {
204 it = segment_clusters_.erase(it);
213 const jsk_recognition_msgs::TimeRange::ConstPtr& trigger)
216 time_range_ = trigger;
217 cleanupBuffers(time_range_->start);
221 const std_msgs::Header&
header,
222 const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
223 const std::vector<pcl::PointIndices::Ptr>& connected_indices)
225 sensor_msgs::PointCloud2 ros_cloud;
227 ros_cloud.header =
header;
228 pub_point_cloud_.publish(ros_cloud);
229 jsk_recognition_msgs::ClusterPointIndices ros_indices;
230 ros_indices.header =
header;
231 ros_indices.cluster_indices
233 debug_pub_inliers_before_plane_.publish(ros_indices);
240 double max_dot = - DBL_MAX;
241 for (
size_t i = 0; i < segment_clusters_.size(); i++) {
243 Eigen::Vector3f delta_cluster = cluster->getDelta();
244 Eigen::Vector3f delta = segment->toSegment()->getDirection();
245 double delta_dot = std::abs(delta_cluster.dot(delta));
246 if (delta_dot > segment_connect_normal_threshold_) {
247 if (max_dot < delta_dot) {
258 if (max_index == -1) {
264 return segment_clusters_[max_index];
269 const std_msgs::Header&
header,
270 std::vector<LineSegment::Ptr> new_segments)
272 for (
size_t i = 0; i < new_segments.size(); i++) {
276 cluster->addLineSegmentEWMA(segment, ewma_tau_);
280 cluster->addLineSegmentEWMA(segment, 1.0);
281 segment_clusters_.push_back(cluster);
285 pcl::PointCloud<pcl::PointXYZ>::Ptr
286 connected_cloud(
new pcl::PointCloud<pcl::PointXYZ>);
287 std::vector<pcl::PointIndices::Ptr> connected_indices;
288 std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > clouds_list;
289 for (
size_t i = 0; i < segment_clusters_.size(); i++) {
291 pcl::PointIndices::Ptr current_indices (
new pcl::PointIndices);
292 pcl::PointCloud<pcl::PointXYZ>::Ptr current_cloud
293 = cluster->getRawPoints();
294 for (
size_t j = 0; j < current_cloud->points.size(); j++) {
295 current_indices->indices.push_back(connected_cloud->points.size() + j);
297 connected_indices.push_back(current_indices);
298 clouds_list.push_back(current_cloud);
299 *connected_cloud = *connected_cloud + *current_cloud;
302 publishBeforePlaneSegmentation(
309 const std_msgs::Header&
header,
310 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
311 std::vector<pcl::ModelCoefficients::Ptr> all_coefficients,
312 std::vector<pcl::PointIndices::Ptr> all_indices)
314 jsk_recognition_msgs::ClusterPointIndices ros_indices;
315 ros_indices.header =
header;
316 ros_indices.cluster_indices
319 pub_inliers_.publish(ros_indices);
320 jsk_recognition_msgs::ModelCoefficientsArray ros_coefficients;
321 ros_coefficients.header =
header;
322 ros_coefficients.coefficients
326 pub_coefficients_.publish(ros_coefficients);
327 jsk_recognition_msgs::PolygonArray ros_polygon;
328 ros_polygon.header =
header;
329 for (
size_t i = 0; i < all_indices.size(); i++) {
331 = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointXYZ>(
332 cloud, all_indices[i], all_coefficients[i]);
333 geometry_msgs::PolygonStamped polygon_stamped;
334 polygon_stamped.header =
header;
335 polygon_stamped.polygon = convex->toROSMsg();
336 ros_polygon.polygons.push_back(polygon_stamped);
338 pub_polygons_.publish(ros_polygon);
342 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
343 const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& indices_msg,
344 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg)
348 pointclouds_buffer_.push_back(cloud_msg);
349 indices_buffer_.push_back(indices_msg);
350 coefficients_buffer_.push_back(coefficients_msg);
351 pcl::PointCloud<pcl::PointXYZ>::Ptr
352 input_cloud(
new pcl::PointCloud<pcl::PointXYZ>);
355 std::vector<pcl::PointIndices::Ptr> input_indices
357 std::vector<pcl::ModelCoefficients::Ptr> input_coefficients
359 coefficients_msg->coefficients);
360 std::vector<LineSegment::Ptr> new_segments;
361 for (
size_t i = 0; i < indices_msg->cluster_indices.size(); i++) {
364 input_coefficients[i],
366 segments_buffer_.push_back(segment);
367 new_segments.push_back(segment);
369 collectFromBuffers(cloud_msg->header, new_segments);
virtual LineSegmentCluster::Ptr lookupNearestSegment(LineSegment::Ptr segment)
virtual pcl::PointCloud< pcl::PointXYZ >::Ptr getRawPoints()
virtual void addLineSegmentEWMA(LineSegment::Ptr segment, const double tau)
#define NODELET_ERROR(...)
virtual void publishResult(const std_msgs::Header &header, pcl::PointCloud< pcl::PointXYZ >::Ptr cloud, std::vector< pcl::ModelCoefficients::Ptr > all_coefficients, std::vector< pcl::PointIndices::Ptr > all_indices)
virtual void removeBefore(const ros::Time &stamp)
virtual void collect(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &indices_msg, const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &coefficients_msg)
std::vector< PCLModelCoefficientMsg > convertToROSModelCoefficients(const std::vector< pcl::ModelCoefficients::Ptr > &coefficients, const std_msgs::Header &header)
std::vector< LineSegment::Ptr > segments_
std::vector< pcl::ModelCoefficients::Ptr > convertToPCLModelCoefficients(const std::vector< PCLModelCoefficientMsg > &coefficients)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void cleanupBuffers(const ros::Time &stamp)
jsk_pcl_ros::LineSegmentCollectorConfig Config
std::vector< pcl::PointIndices::Ptr > convertToPCLPointIndices(const std::vector< PCLIndicesMsg > &cluster_indices)
virtual void configCallback(Config &config, uint32_t level)
sensor_msgs::PointCloud2 PointCloud
virtual void unsubscribe()
pcl::PointCloud< pcl::PointXYZ >::Ptr points_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::LineSegmentCollector, nodelet::Nodelet)
virtual void collectFromBuffers(const std_msgs::Header &header, std::vector< LineSegment::Ptr > new_segments)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
std::vector< PCLIndicesMsg > convertToROSPointIndices(const std::vector< pcl::PointIndices::Ptr > cluster_indices, const std_msgs::Header &header)
virtual void publishBeforePlaneSegmentation(const std_msgs::Header &header, const pcl::PointCloud< pcl::PointXYZ >::Ptr cloud, const std::vector< pcl::PointIndices::Ptr > &connected_indices)
boost::shared_ptr< LineSegmentCluster > Ptr
pcl::PointCloud< pcl::PointXYZ >::Ptr raw_points_
virtual void triggerCallback(const jsk_recognition_msgs::TimeRange::ConstPtr &trigger)
virtual pcl::PointCloud< pcl::PointXYZ >::Ptr getPoints()