00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "jsk_pcl_ros/organized_multi_plane_segmentation.h"
00037 #include "jsk_recognition_msgs/ModelCoefficientsArray.h"
00038 #include <pcl/segmentation/impl/organized_multi_plane_segmentation.hpp>
00039 #include <pcl/filters/extract_indices.h>
00040 #include <pcl/kdtree/kdtree_flann.h>
00041 #include <set>
00042 #include <Eigen/StdVector>
00043 #include <pcl/surface/convex_hull.h>
00044 #include <pcl/filters/project_inliers.h>
00045 #include <pcl/features/integral_image_normal.h>
00046
00047 #include "jsk_pcl_ros/pcl_conversion_util.h"
00048 #include <pluginlib/class_list_macros.h>
00049
00050 #include <boost/format.hpp>
00051 #include <pcl/common/centroid.h>
00052 #include <visualization_msgs/Marker.h>
00053 #include "jsk_pcl_ros/geo_util.h"
00054
00055 #include <pcl/sample_consensus/method_types.h>
00056 #include <pcl/sample_consensus/model_types.h>
00057 #include <pcl/segmentation/sac_segmentation.h>
00058 #include <pcl/surface/convex_hull.h>
00059
00060 #include <jsk_topic_tools/color_utils.h>
00061
00062 namespace jsk_pcl_ros
00063 {
00064
00065 void OrganizedMultiPlaneSegmentation::onInit()
00066 {
00067 ConnectionBasedNodelet::onInit();
00068 pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
00070
00072 diagnostic_updater_.reset(new diagnostic_updater::Updater);
00073 diagnostic_updater_->setHardwareID(getName());
00074 diagnostic_updater_->add(
00075 getName() + "::NormalEstimation",
00076 boost::bind(
00077 &OrganizedMultiPlaneSegmentation::updateDiagnosticNormalEstimation,
00078 this,
00079 _1));
00080 diagnostic_updater_->add(
00081 getName() + "::PlaneSegmentation",
00082 boost::bind(
00083 &OrganizedMultiPlaneSegmentation::updateDiagnosticPlaneSegmentation,
00084 this,
00085 _1));
00086 double vital_rate;
00087 pnh_->param("vital_rate", vital_rate, 1.0);
00088 normal_estimation_vital_checker_.reset(
00089 new jsk_topic_tools::VitalChecker(1 / vital_rate));
00090 plane_segmentation_vital_checker_.reset(
00091 new jsk_topic_tools::VitalChecker(1 / vital_rate));
00092 estimate_normal_ = true;
00093 pnh_->getParam("estimate_normal", estimate_normal_);
00095
00097 pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output", 1);
00098 polygon_pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output_polygon", 1);
00099 coefficients_pub_
00100 = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*pnh_, "output_coefficients", 1);
00101 org_pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output_nonconnected", 1);
00102 org_polygon_pub_
00103 = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output_nonconnected_polygon", 1);
00104 org_coefficients_pub_
00105 = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*pnh_,
00106 "output_nonconnected_coefficients", 1);
00107
00108 refined_pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_,
00109 "output_refined", 1);
00110 refined_polygon_pub_
00111 = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output_refined_polygon", 1);
00112 refined_coefficients_pub_
00113 = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*pnh_,
00114 "output_refined_coefficients", 1);
00115
00116 pub_connection_marker_
00117 = advertise<visualization_msgs::Marker>(*pnh_,
00118 "debug_connection_map", 1);
00119
00120 if (estimate_normal_) {
00121 normal_pub_
00122 = advertise<sensor_msgs::PointCloud2>(*pnh_, "output_normal", 1);
00123 }
00124
00125 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00126 dynamic_reconfigure::Server<Config>::CallbackType f =
00127 boost::bind (
00128 &OrganizedMultiPlaneSegmentation::configCallback, this, _1, _2);
00129 srv_->setCallback (f);
00130
00131 diagnostics_timer_ = pnh_->createTimer(
00132 ros::Duration(1.0),
00133 boost::bind(&OrganizedMultiPlaneSegmentation::updateDiagnostics,
00134 this,
00135 _1));
00136 }
00137
00138 void OrganizedMultiPlaneSegmentation::forceToDirectOrigin(
00139 const std::vector<pcl::ModelCoefficients>& coefficients,
00140 std::vector<pcl::ModelCoefficients>& output_coefficients)
00141 {
00142 output_coefficients.resize(coefficients.size());
00143 for (size_t i = 0; i < coefficients.size(); i++) {
00144 pcl::ModelCoefficients plane_coefficient = coefficients[i];
00145 Plane plane(coefficients[i].values);
00146
00147 Eigen::Vector3f p = plane.getPointOnPlane();
00148 Eigen::Vector3f n = plane.getNormal();
00149 if (p.dot(n) < 0) {
00150 output_coefficients[i] = plane_coefficient;
00151 }
00152 else {
00153 Plane flip = plane.flip();
00154 pcl::ModelCoefficients new_coefficient;
00155 flip.toCoefficients(new_coefficient.values);
00156 output_coefficients[i] = new_coefficient;
00157 }
00158 }
00159 }
00160
00161 void OrganizedMultiPlaneSegmentation::subscribe()
00162 {
00163 sub_ = pnh_->subscribe("input", 1,
00164 &OrganizedMultiPlaneSegmentation::segment, this);
00165 }
00166
00167 void OrganizedMultiPlaneSegmentation::unsubscribe()
00168 {
00169 sub_.shutdown();
00170 }
00171
00172 void OrganizedMultiPlaneSegmentation::configCallback(Config &config, uint32_t level)
00173 {
00174 boost::mutex::scoped_lock lock(mutex_);
00175 min_size_ = config.min_size;
00176 angular_threshold_ = config.angular_threshold;
00177 distance_threshold_ = config.distance_threshold;
00178 max_curvature_ = config.max_curvature;
00179 connect_plane_angle_threshold_ = config.connect_plane_angle_threshold;
00180 connect_distance_threshold_ = config.connect_distance_threshold;
00181 max_depth_change_factor_ = config.max_depth_change_factor;
00182 normal_smoothing_size_ = config.normal_smoothing_size;
00183 depth_dependent_smoothing_ = config.depth_dependent_smoothing;
00184 estimation_method_ = config.estimation_method;
00185 border_policy_ignore_ = config.border_policy_ignore;
00186 publish_normal_ = config.publish_normal;
00187 ransac_refine_coefficients_ = config.ransac_refine_coefficients;
00188 ransac_refine_outlier_distance_threshold_
00189 = config.ransac_refine_outlier_distance_threshold;
00190 min_refined_area_threshold_ = config.min_refined_area_threshold;
00191 max_refined_area_threshold_ = config.max_refined_area_threshold;
00192
00193 }
00194
00195 void OrganizedMultiPlaneSegmentation::connectPlanesMap(
00196 const pcl::PointCloud<PointT>::Ptr& input,
00197 const std::vector<pcl::ModelCoefficients>& model_coefficients,
00198 const std::vector<pcl::PointIndices>& boundary_indices,
00199 IntegerGraphMap& connection_map)
00200 {
00201 JSK_NODELET_DEBUG("size of model_coefficients: %lu", model_coefficients.size());
00202 if (model_coefficients.size() == 0) {
00203 return;
00204 }
00205
00206 if (model_coefficients.size() == 1) {
00207 connection_map[0]= std::vector<int>();
00208 return;
00209 }
00210
00211 pcl::ExtractIndices<PointT> extract;
00212 extract.setInputCloud(input);
00213 for (size_t i = 0; i < model_coefficients.size(); i++) {
00214
00215 connection_map[i] = std::vector<int>();
00216 connection_map[i].push_back(i);
00217 for (size_t j = i + 1; j < model_coefficients.size(); j++) {
00218
00219 pcl::ModelCoefficients a_coefficient = model_coefficients[i];
00220 pcl::ModelCoefficients b_coefficient = model_coefficients[j];
00221 Eigen::Vector3f a_normal(a_coefficient.values[0], a_coefficient.values[1], a_coefficient.values[2]);
00222 Eigen::Vector3f b_normal(b_coefficient.values[0], b_coefficient.values[1], b_coefficient.values[2]);
00223 double a_distance = a_coefficient.values[3];
00224 double b_distance = b_coefficient.values[3];
00225
00226 if (a_normal.norm() != 1.0) {
00227 a_distance = a_distance / a_normal.norm();
00228 a_normal = a_normal / a_normal.norm();
00229 }
00230 if (b_normal.norm() != 1.0) {
00231 b_distance = b_distance / b_normal.norm();
00232 b_normal = b_normal / b_normal.norm();
00233 }
00234 double theta = fabs(acos(a_normal.dot(b_normal)));
00235 JSK_NODELET_DEBUG("%lu - %lu angle: %f", i, j, theta);
00236 if (theta > M_PI / 2.0) {
00237 theta = M_PI - theta;
00238 }
00239 if (theta > connect_plane_angle_threshold_) {
00240 continue;
00241 }
00242
00243
00244
00245
00246 pcl::PointIndices::Ptr a_indices
00247 = boost::make_shared<pcl::PointIndices>(boundary_indices[i]);
00248 pcl::PointIndices::Ptr b_indices
00249 = boost::make_shared<pcl::PointIndices>(boundary_indices[j]);
00250 pcl::PointCloud<PointT> a_cloud, b_cloud;
00251 extract.setIndices(a_indices);
00252 extract.filter(a_cloud);
00253 extract.setIndices(b_indices);
00254 extract.filter(b_cloud);
00255 if (a_cloud.points.size() > 0) {
00256 pcl::KdTreeFLANN<PointT> kdtree;
00257 kdtree.setInputCloud(a_cloud.makeShared());
00258 bool foundp = false;
00259 for (size_t pi = 0; pi < b_cloud.points.size(); pi++) {
00260 PointT p = b_cloud.points[pi];
00261 std::vector<int> k_indices;
00262 std::vector<float> k_sqr_distances;
00263 if (kdtree.radiusSearch(
00264 p, connect_distance_threshold_, k_indices, k_sqr_distances, 1) > 0) {
00265 JSK_NODELET_DEBUG("%lu - %lu connected", i, j);
00266 foundp = true;
00267 break;
00268 }
00269 }
00270 if (foundp) {
00271 connection_map[i].push_back(j);
00272 }
00273 }
00274 }
00275 }
00276 }
00277
00278 void OrganizedMultiPlaneSegmentation::pclIndicesArrayToClusterPointIndices(
00279 const std::vector<pcl::PointIndices>& inlier_indices,
00280 const std_msgs::Header& header,
00281 jsk_recognition_msgs::ClusterPointIndices& output_indices)
00282 {
00283 for (size_t i = 0; i < inlier_indices.size(); i++) {
00284 pcl::PointIndices inlier = inlier_indices[i];
00285 PCLIndicesMsg one_indices;
00286 one_indices.header = header;
00287 one_indices.indices = inlier.indices;
00288 output_indices.cluster_indices.push_back(one_indices);
00289 }
00290 }
00291
00292 void OrganizedMultiPlaneSegmentation::pointCloudToPolygon(
00293 const pcl::PointCloud<PointT>& input,
00294 geometry_msgs::Polygon& polygon)
00295 {
00296 for (size_t i = 0; i < input.points.size(); i++) {
00297 geometry_msgs::Point32 point;
00298 point.x = input.points[i].x;
00299 point.y = input.points[i].y;
00300 point.z = input.points[i].z;
00301 polygon.points.push_back(point);
00302 }
00303 }
00304
00305 void OrganizedMultiPlaneSegmentation::buildConnectedPlanes(
00306 const pcl::PointCloud<PointT>::Ptr& input,
00307 const std_msgs::Header& header,
00308 const std::vector<pcl::PointIndices>& inlier_indices,
00309 const std::vector<pcl::PointIndices>& boundary_indices,
00310 const std::vector<pcl::ModelCoefficients>& model_coefficients,
00311 const IntegerGraphMap& connection_map,
00312 std::vector<pcl::PointIndices>& output_indices,
00313 std::vector<pcl::ModelCoefficients>& output_coefficients,
00314 std::vector<pcl::PointCloud<PointT> >& output_boundary_clouds)
00315 {
00316 std::vector<std::set<int> > cloud_sets;
00317 JSK_NODELET_DEBUG("connection_map:");
00318 for (IntegerGraphMap::const_iterator it = connection_map.begin();
00319 it != connection_map.end();
00320 ++it) {
00321 int from_index = it->first;
00322 std::stringstream ss;
00323 ss << "connection map: " << from_index << " [";
00324 for (size_t i = 0; i < it->second.size(); i++) {
00325 ss << i << ", ";
00326 }
00327 JSK_NODELET_DEBUG("%s", ss.str().c_str());
00328 }
00329
00330 buildAllGroupsSetFromGraphMap(connection_map, cloud_sets);
00331 connected_plane_num_counter_.add(cloud_sets.size());
00332 for (size_t i = 0; i < cloud_sets.size(); i++) {
00333 pcl::PointIndices one_indices;
00334 pcl::PointIndices one_boundaries;
00335 std::vector<float> new_coefficients;
00336 new_coefficients.resize(4, 0);
00337 for (std::set<int>::iterator it = cloud_sets[i].begin();
00338 it != cloud_sets[i].end();
00339 ++it) {
00340 JSK_NODELET_DEBUG("%lu includes %d", i, *it);
00341 new_coefficients = model_coefficients[*it].values;
00342 pcl::PointIndices inlier = inlier_indices[*it];
00343 pcl::PointIndices boundary_inlier = boundary_indices[*it];
00344
00345 one_indices = *addIndices(one_indices, inlier);
00346 one_boundaries = *addIndices(one_boundaries, boundary_inlier);
00347 }
00348 if (one_indices.indices.size() == 0) {
00349 continue;
00350 }
00351
00352 double norm = sqrt(new_coefficients[0] * new_coefficients[0]
00353 + new_coefficients[1] * new_coefficients[1]
00354 + new_coefficients[2] * new_coefficients[2]);
00355 new_coefficients[0] /= norm;
00356 new_coefficients[1] /= norm;
00357 new_coefficients[2] /= norm;
00358 new_coefficients[3] /= norm;
00359
00360
00361 pcl::ModelCoefficients pcl_new_coefficients;
00362 pcl_new_coefficients.values = new_coefficients;
00363
00364 pcl::PointIndices::Ptr indices_ptr
00365 = boost::make_shared<pcl::PointIndices>(one_boundaries);
00366 pcl::ModelCoefficients::Ptr coefficients_ptr
00367 = boost::make_shared<pcl::ModelCoefficients>(pcl_new_coefficients);
00368 ConvexPolygon::Ptr convex
00369 = convexFromCoefficientsAndInliers<PointT>(
00370 input, indices_ptr, coefficients_ptr);
00371 if (convex) {
00372 pcl::PointCloud<PointT> chull_output;
00373 convex->boundariesToPointCloud<PointT>(chull_output);
00374 output_indices.push_back(one_indices);
00375 output_coefficients.push_back(pcl_new_coefficients);
00376 output_boundary_clouds.push_back(chull_output);
00377 }
00378 else {
00379 JSK_NODELET_ERROR("failed to build convex");
00380 }
00381 }
00382
00383 }
00384
00386
00388 void OrganizedMultiPlaneSegmentation::segmentOrganizedMultiPlanes(
00389 pcl::PointCloud<PointT>::Ptr input,
00390 pcl::PointCloud<pcl::Normal>::Ptr normal,
00391 PlanarRegionVector& regions,
00392 std::vector<pcl::ModelCoefficients>& model_coefficients,
00393 std::vector<pcl::PointIndices>& inlier_indices,
00394 pcl::PointCloud<pcl::Label>::Ptr& labels,
00395 std::vector<pcl::PointIndices>& label_indices,
00396 std::vector<pcl::PointIndices>& boundary_indices)
00397 {
00398 plane_segmentation_vital_checker_->poke();
00399 pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
00400 mps.setMinInliers(min_size_);
00401 mps.setAngularThreshold(angular_threshold_);
00402 mps.setDistanceThreshold(distance_threshold_);
00403 mps.setMaximumCurvature(max_curvature_);
00404 mps.setInputCloud(input);
00405 mps.setInputNormals(normal);
00406 {
00407 jsk_topic_tools::ScopedTimer timer = plane_segmentation_time_acc_.scopedTimer();
00408 mps.segmentAndRefine(
00409 regions, model_coefficients, inlier_indices,
00410 labels, label_indices, boundary_indices);
00411 }
00412 }
00413
00414 void OrganizedMultiPlaneSegmentation::publishSegmentationInformation(
00415 const std_msgs::Header& header,
00416 const pcl::PointCloud<PointT>::Ptr input,
00417 ros::Publisher& indices_pub,
00418 ros::Publisher& polygon_pub,
00419 ros::Publisher& coefficients_pub,
00420 const std::vector<pcl::PointIndices>& inlier_indices,
00421 const std::vector<pcl::PointCloud<PointT> >& boundaries,
00422 const std::vector<pcl::ModelCoefficients>& model_coefficients)
00423 {
00424 jsk_recognition_msgs::ClusterPointIndices indices;
00425 jsk_recognition_msgs::ModelCoefficientsArray coefficients_array;
00426 jsk_recognition_msgs::PolygonArray polygon_array;
00427 indices.header = header;
00428 polygon_array.header = header;
00429 coefficients_array.header = header;
00430
00432
00434 pclIndicesArrayToClusterPointIndices(inlier_indices, header, indices);
00435 indices_pub.publish(indices);
00436
00438
00440 for (size_t i = 0; i < boundaries.size(); i++) {
00441 geometry_msgs::PolygonStamped polygon;
00442 pcl::PointCloud<PointT> boundary_cloud = boundaries[i];
00443 pointCloudToPolygon(boundary_cloud, polygon.polygon);
00444 polygon.header = header;
00445 polygon_array.polygons.push_back(polygon);
00446 }
00447 polygon_pub.publish(polygon_array);
00448
00450
00452 for (size_t i = 0; i < model_coefficients.size(); i++) {
00453 PCLModelCoefficientMsg coefficient;
00454 coefficient.values = model_coefficients[i].values;
00455 coefficient.header = header;
00456 coefficients_array.coefficients.push_back(coefficient);
00457 }
00458 coefficients_pub.publish(coefficients_array);
00459 }
00460
00461 void OrganizedMultiPlaneSegmentation::publishSegmentationInformation(
00462 const std_msgs::Header& header,
00463 const pcl::PointCloud<PointT>::Ptr input,
00464 ros::Publisher& indices_pub,
00465 ros::Publisher& polygon_pub,
00466 ros::Publisher& coefficients_pub,
00467 const std::vector<pcl::PointIndices>& inlier_indices,
00468 const std::vector<pcl::PointIndices>& boundary_indices,
00469 const std::vector<pcl::ModelCoefficients>& model_coefficients)
00470 {
00471
00472 std::vector<pcl::PointCloud<PointT> > boundaries;
00473 pcl::ExtractIndices<PointT> extract;
00474 extract.setInputCloud(input);
00475 for (size_t i = 0; i < boundary_indices.size(); i++) {
00476 pcl::PointCloud<PointT> boundary_cloud;
00477 pcl::PointIndices boundary_one_indices = boundary_indices[i];
00478 pcl::PointIndices::Ptr indices_ptr = boost::make_shared<pcl::PointIndices>(boundary_indices[i]);
00479 extract.setIndices(indices_ptr);
00480 extract.filter(boundary_cloud);
00481 boundaries.push_back(boundary_cloud);
00482 }
00483
00484 publishSegmentationInformation(
00485 header, input, indices_pub, polygon_pub, coefficients_pub,
00486 inlier_indices, boundaries, model_coefficients);
00487 }
00488
00489 void OrganizedMultiPlaneSegmentation::publishMarkerOfConnection(
00490 IntegerGraphMap connection_map,
00491 const pcl::PointCloud<PointT>::Ptr cloud,
00492 const std::vector<pcl::PointIndices>& inliers,
00493 const std_msgs::Header& header)
00494 {
00496
00498 visualization_msgs::Marker connection_marker;
00499 connection_marker.type = visualization_msgs::Marker::LINE_LIST;
00500 connection_marker.scale.x = 0.01;
00501 connection_marker.header = header;
00502 connection_marker.pose.orientation.w = 1.0;
00503 connection_marker.color = jsk_topic_tools::colorCategory20(0);
00504
00506
00508 Vertices centroids;
00509 for (size_t i = 0; i < inliers.size(); i++) {
00510 pcl::PointIndices::Ptr target_inliers
00511 = boost::make_shared<pcl::PointIndices>(inliers[i]);
00512 pcl::PointCloud<PointT>::Ptr target_cloud (new pcl::PointCloud<PointT>);
00513 Eigen::Vector4f centroid;
00514 pcl::ExtractIndices<PointT> ex;
00515 ex.setInputCloud(cloud);
00516 ex.setIndices(target_inliers);
00517 ex.filter(*target_cloud);
00518 pcl::compute3DCentroid(*target_cloud, centroid);
00519 Eigen::Vector3f centroid_3f(centroid[0], centroid[1], centroid[2]);
00520 centroids.push_back(centroid_3f);
00521 }
00522
00523 for (IntegerGraphMap::iterator it = connection_map.begin();
00524 it != connection_map.end();
00525 ++it) {
00526
00527 int from_index = it->first;
00528 std::vector<int> the_connection_map = connection_map[from_index];
00529 for (size_t j = 0; j < the_connection_map.size(); j++) {
00530 int to_index = the_connection_map[j];
00531
00532 Eigen::Vector3f from_point = centroids[from_index];
00533 Eigen::Vector3f to_point = centroids[to_index];
00534 geometry_msgs::Point from_point_ros, to_point_ros;
00535 pointFromVectorToXYZ<Eigen::Vector3f, geometry_msgs::Point>(
00536 from_point, from_point_ros);
00537 pointFromVectorToXYZ<Eigen::Vector3f, geometry_msgs::Point>(
00538 to_point, to_point_ros);
00539 connection_marker.points.push_back(from_point_ros);
00540 connection_marker.points.push_back(to_point_ros);
00541 connection_marker.colors.push_back(jsk_topic_tools::colorCategory20(from_index));
00542 connection_marker.colors.push_back(jsk_topic_tools::colorCategory20(from_index));
00543 }
00544 }
00545 pub_connection_marker_.publish(connection_marker);
00546 }
00547
00548 void OrganizedMultiPlaneSegmentation::segmentFromNormals(
00549 pcl::PointCloud<PointT>::Ptr input,
00550 pcl::PointCloud<pcl::Normal>::Ptr normal,
00551 const std_msgs::Header& header)
00552 {
00553 PlanarRegionVector regions;
00554 std::vector<pcl::ModelCoefficients> model_coefficients;
00555 std::vector<pcl::PointIndices> inlier_indices;
00556 pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>());
00557 std::vector<pcl::PointIndices> label_indices;
00558 std::vector<pcl::PointIndices> boundary_indices;
00560
00561
00563 segmentOrganizedMultiPlanes(input, normal, regions, model_coefficients,
00564 inlier_indices, labels, label_indices,
00565 boundary_indices);
00566 std::vector<pcl::ModelCoefficients> fixed_model_coefficients;
00567 forceToDirectOrigin(model_coefficients, fixed_model_coefficients);
00568 model_coefficients = fixed_model_coefficients;
00569
00570 original_plane_num_counter_.add(regions.size());
00571 publishSegmentationInformation(
00572 header, input,
00573 org_pub_, org_polygon_pub_, org_coefficients_pub_,
00574 inlier_indices, boundary_indices, model_coefficients);
00575
00577
00578
00580 IntegerGraphMap connection_map;
00581 connectPlanesMap(input, model_coefficients, boundary_indices, connection_map);
00582 publishMarkerOfConnection(connection_map, input, inlier_indices, header);
00583 std::vector<pcl::PointIndices> output_nonrefined_indices;
00584 std::vector<pcl::ModelCoefficients> output_nonrefined_coefficients;
00585 std::vector<pcl::PointCloud<PointT> > output_nonrefined_boundary_clouds;
00586 buildConnectedPlanes(input, header,
00587 inlier_indices,
00588 boundary_indices,
00589 model_coefficients,
00590 connection_map,
00591 output_nonrefined_indices,
00592 output_nonrefined_coefficients,
00593 output_nonrefined_boundary_clouds);
00594 std::vector<pcl::ModelCoefficients> fixed_output_nonrefined_coefficients;
00595 forceToDirectOrigin(output_nonrefined_coefficients, fixed_output_nonrefined_coefficients);
00596 output_nonrefined_coefficients = fixed_output_nonrefined_coefficients;
00597 publishSegmentationInformation(
00598 header, input,
00599 pub_, polygon_pub_, coefficients_pub_,
00600 output_nonrefined_indices,
00601 output_nonrefined_boundary_clouds,
00602 output_nonrefined_coefficients);
00604
00606 if (ransac_refine_coefficients_) {
00607 std::vector<pcl::PointIndices> refined_inliers;
00608 std::vector<pcl::ModelCoefficients> refined_coefficients;
00609 std::vector<ConvexPolygon::Ptr> refined_convexes;
00610 refineBasedOnRANSAC(
00611 input, output_nonrefined_indices, output_nonrefined_coefficients,
00612 refined_inliers, refined_coefficients, refined_convexes);
00613 std::vector<pcl::PointCloud<PointT> > refined_boundary_clouds;
00614 for (size_t i = 0; i < refined_convexes.size(); i++) {
00615 pcl::PointCloud<PointT> refined_boundary;
00616 refined_convexes[i]->boundariesToPointCloud(refined_boundary);
00617 refined_boundary_clouds.push_back(refined_boundary);
00618 }
00619 std::vector<pcl::ModelCoefficients> fixed_refined_coefficients;
00620 forceToDirectOrigin(refined_coefficients, fixed_refined_coefficients);
00621 refined_coefficients = fixed_refined_coefficients;
00622 publishSegmentationInformation(
00623 header, input,
00624 refined_pub_, refined_polygon_pub_, refined_coefficients_pub_,
00625 refined_inliers, refined_boundary_clouds, refined_coefficients);
00626 }
00627 }
00628
00629 void OrganizedMultiPlaneSegmentation::refineBasedOnRANSAC(
00630 const pcl::PointCloud<PointT>::Ptr input,
00631 const std::vector<pcl::PointIndices>& input_indices,
00632 const std::vector<pcl::ModelCoefficients>& input_coefficients,
00633 std::vector<pcl::PointIndices>& output_indices,
00634 std::vector<pcl::ModelCoefficients>& output_coefficients,
00635 std::vector<ConvexPolygon::Ptr>& output_convexes)
00636 {
00637 jsk_topic_tools::ScopedTimer timer
00638 = ransac_refinement_time_acc_.scopedTimer();
00639 for (size_t i = 0; i < input_indices.size(); i++) {
00640 pcl::PointIndices::Ptr input_indices_ptr
00641 = boost::make_shared<pcl::PointIndices>(input_indices[i]);
00642 Eigen::Vector3f normal(input_coefficients[i].values[0],
00643 input_coefficients[i].values[1],
00644 input_coefficients[i].values[2]);
00645 normal.normalize();
00647
00649 pcl::SACSegmentation<PointT> seg;
00650 seg.setOptimizeCoefficients (true);
00651 seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
00652 seg.setMethodType (pcl::SAC_RANSAC);
00653 seg.setDistanceThreshold (ransac_refine_outlier_distance_threshold_);
00654 seg.setInputCloud(input);
00655 seg.setIndices(input_indices_ptr);
00656 seg.setMaxIterations (10000);
00657 seg.setAxis(normal);
00658 seg.setEpsAngle(pcl::deg2rad(20.0));
00659 pcl::PointIndices::Ptr refined_inliers (new pcl::PointIndices);
00660 pcl::ModelCoefficients::Ptr refined_coefficients(new pcl::ModelCoefficients);
00661 seg.segment(*refined_inliers, *refined_coefficients);
00662 if (refined_inliers->indices.size() > 0) {
00664
00666 ConvexPolygon::Ptr convex = convexFromCoefficientsAndInliers<PointT>(
00667 input, refined_inliers, refined_coefficients);
00668 if (convex) {
00669
00670 double area = convex->area();
00671 if (area > min_refined_area_threshold_ &&
00672 area < max_refined_area_threshold_) {
00673 output_convexes.push_back(convex);
00674 output_indices.push_back(*refined_inliers);
00675 output_coefficients.push_back(*refined_coefficients);
00676 }
00677 }
00678 }
00679 }
00680 }
00681
00682 void OrganizedMultiPlaneSegmentation::estimateNormal(pcl::PointCloud<PointT>::Ptr input,
00683 pcl::PointCloud<pcl::Normal>::Ptr output)
00684 {
00685 jsk_topic_tools::ScopedTimer timer = normal_estimation_time_acc_.scopedTimer();
00686 pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
00687 if (estimation_method_ == 0) {
00688 ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
00689 }
00690 else if (estimation_method_ == 1) {
00691 ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
00692 }
00693 else if (estimation_method_ == 2) {
00694 ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);
00695 }
00696 else {
00697 JSK_NODELET_FATAL("unknown estimation method, force to use COVARIANCE_MATRIX: %d",
00698 estimation_method_);
00699 ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
00700 }
00701
00702 if (border_policy_ignore_) {
00703 ne.setBorderPolicy(pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_IGNORE);
00704 }
00705 else {
00706 ne.setBorderPolicy(pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_MIRROR);
00707 }
00708
00709 ne.setMaxDepthChangeFactor(max_depth_change_factor_);
00710 ne.setNormalSmoothingSize(normal_smoothing_size_);
00711 ne.setDepthDependentSmoothing(depth_dependent_smoothing_);
00712 ne.setInputCloud(input);
00713 ne.compute(*output);
00714 }
00715
00716 void OrganizedMultiPlaneSegmentation::segment
00717 (const sensor_msgs::PointCloud2::ConstPtr& msg)
00718 {
00719 boost::mutex::scoped_lock lock(mutex_);
00720
00721
00722
00723 pcl::PointCloud<PointT>::Ptr input(new pcl::PointCloud<PointT>());
00724 pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>());
00725 pcl::fromROSMsg(*msg, *input);
00726
00727 if (estimate_normal_) {
00728 normal_estimation_vital_checker_->poke();
00729 estimateNormal(input, normal);
00730
00731 if (publish_normal_) {
00732 sensor_msgs::PointCloud2 normal_ros_cloud;
00733 pcl::toROSMsg(*normal, normal_ros_cloud);
00734 normal_ros_cloud.header = msg->header;
00735 normal_pub_.publish(normal_ros_cloud);
00736 }
00737 }
00738 else {
00739 pcl::fromROSMsg(*msg, *normal);
00740 }
00741
00742 segmentFromNormals(input, normal, msg->header);
00743 diagnostic_updater_->update();
00744 }
00745
00746 void OrganizedMultiPlaneSegmentation::updateDiagnosticNormalEstimation(
00747 diagnostic_updater::DiagnosticStatusWrapper &stat)
00748 {
00749 if (estimate_normal_) {
00750 bool alivep = normal_estimation_vital_checker_->isAlive();
00751 if (alivep) {
00752 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "NormalEstimation running");
00753 addDiagnosticInformation(
00754 "Time to estimate normal", normal_estimation_time_acc_, stat);
00755
00756 if (estimation_method_ == 0) {
00757 stat.add("Estimation Method", "AVERAGE_3D_GRADIENT");
00758 }
00759 else if (estimation_method_ == 1) {
00760 stat.add("Estimation Method", "COVARIANCE_MATRIX");
00761 }
00762 else if (estimation_method_ == 2) {
00763 stat.add("Estimation Method", "AVERAGE_DEPTH_CHANGE");
00764 }
00765 if (border_policy_ignore_) {
00766 stat.add("Border Policy", "ignore");
00767 }
00768 else {
00769 stat.add("Border Policy", "mirror");
00770 }
00771 stat.add("Max Depth Change Factor", max_depth_change_factor_);
00772 stat.add("Normal Smoothing Size", normal_smoothing_size_);
00773 if (depth_dependent_smoothing_) {
00774 stat.add("Depth Dependent Smooting", "Enabled");
00775 }
00776 else {
00777 stat.add("Depth Dependent Smooting", "Disabled");
00778 }
00779 if (publish_normal_) {
00780 stat.add("Publish Normal", "Enabled");
00781 }
00782 else {
00783 stat.add("Publish Normal", "Disabled");
00784 }
00785 }
00786 else {
00787 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
00788 (boost::format("NormalEstimation not running for %f sec")
00789 % normal_estimation_vital_checker_->deadSec()).str());
00790 }
00791
00792 }
00793 else {
00794 stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00795 "NormalEstimation is not activated");
00796 }
00797 }
00798
00799 void OrganizedMultiPlaneSegmentation::updateDiagnosticPlaneSegmentation(
00800 diagnostic_updater::DiagnosticStatusWrapper &stat)
00801 {
00802 bool alivep = plane_segmentation_vital_checker_->isAlive();
00803 if (alivep) {
00804 stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00805 "PlaneSegmentation running");
00806 addDiagnosticInformation(
00807 "Time to segment planes", plane_segmentation_time_acc_, stat);
00808 if (ransac_refine_coefficients_) {
00809 addDiagnosticInformation(
00810 "Time to refine by RANSAC", ransac_refinement_time_acc_, stat);
00811 }
00812 stat.add("Minimum Inliers", min_size_);
00813 stat.add("Angular Threshold (rad)", angular_threshold_);
00814 stat.add("Angular Threshold (deg)", angular_threshold_ / M_PI * 180.0);
00815 stat.add("Distance Threshold", distance_threshold_);
00816 stat.add("Max Curvature", max_curvature_);
00817 if (ransac_refine_coefficients_) {
00818 stat.add("Use RANSAC refinement", "True");
00819 stat.add("RANSAC refinement distance threshold",
00820 ransac_refine_outlier_distance_threshold_);
00821 }
00822 else {
00823 stat.add("Use RANSAC refinement", "False");
00824 }
00825
00826 stat.add("Number of original segmented planes (Avg.)",
00827 original_plane_num_counter_.mean());
00828 stat.add("Number of connected segmented planes (Avg.)",
00829 connected_plane_num_counter_.mean());
00830 }
00831 else {
00832 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
00833 (boost::format("PlaneSegmentation not running for %f sec")
00834 % plane_segmentation_vital_checker_->deadSec()).str());
00835 }
00836
00837 }
00838
00839 void OrganizedMultiPlaneSegmentation::updateDiagnostics(
00840 const ros::TimerEvent& event)
00841 {
00842 boost::mutex::scoped_lock lock(mutex_);
00843 diagnostic_updater_->update();
00844 }
00845
00846 }
00847 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::OrganizedMultiPlaneSegmentation,
00848 nodelet::Nodelet);