void RegionGrowingMultiplePlaneSegmentation::segment(
const sensor_msgs::PointCloud2::ConstPtr& msg,
const sensor_msgs::PointCloud2::ConstPtr& normal_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!done_initialization_) {
return;
}
vital_checker_->poke();
{
jsk_recognition_utils::ScopedWallDurationReporter r
= timer_.reporter(pub_latest_time_, pub_average_time_);
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
pcl::fromROSMsg(*msg, *cloud);
pcl::fromROSMsg(*normal_msg, *normal);
// concatenate fields
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr
all_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*cloud, *normal, *all_cloud);
pcl::PointIndices::Ptr indices (new pcl::PointIndices);
for (size_t i = 0; i < all_cloud->points.size(); i++) {
if (!isnan(all_cloud->points[i].x) &&
!isnan(all_cloud->points[i].y) &&
!isnan(all_cloud->points[i].z) &&
!isnan(all_cloud->points[i].normal_x) &&
!isnan(all_cloud->points[i].normal_y) &&
!isnan(all_cloud->points[i].normal_z) &&
!isnan(all_cloud->points[i].curvature)) {
if (all_cloud->points[i].curvature < max_curvature_) {
indices->indices.push_back(i);
}
}
}
pcl::ConditionalEuclideanClustering<pcl::PointXYZRGBNormal> cec (true);
// vector of pcl::PointIndices
pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters);
cec.setInputCloud (all_cloud);
cec.setIndices(indices);
cec.setClusterTolerance(cluster_tolerance_);
cec.setMinClusterSize(min_size_);
//cec.setMaxClusterSize(max_size_);
{
boost::mutex::scoped_lock lock2(global_custom_condigion_function_mutex);
setCondifionFunctionParameter(angular_threshold_, distance_threshold_);
cec.setConditionFunction(
&RegionGrowingMultiplePlaneSegmentation::regionGrowingFunction);
//ros::Time before = ros::Time::now();
cec.segment (*clusters);
// ros::Time end = ros::Time::now();
// ROS_INFO("segment took %f sec", (before - end).toSec());
}
// Publish raw cluster information
// pcl::IndicesCluster is typdefed as std::vector<pcl::PointIndices>
jsk_recognition_msgs::ClusterPointIndices ros_clustering_result;
ros_clustering_result.cluster_indices
= pcl_conversions::convertToROSPointIndices(*clusters, msg->header);
ros_clustering_result.header = msg->header;
pub_clustering_result_.publish(ros_clustering_result);
// estimate planes
std::vector<pcl::PointIndices::Ptr> all_inliers;
std::vector<pcl::ModelCoefficients::Ptr> all_coefficients;
jsk_recognition_msgs::PolygonArray ros_polygon;
ros_polygon.header = msg->header;
for (size_t i = 0; i < clusters->size(); i++) {
pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr cluster = boost::make_shared<pcl::PointIndices>((*clusters)[i]);
ransacEstimation(cloud, cluster,
*plane_inliers, *plane_coefficients);
if (plane_inliers->indices.size() > 0) {
jsk_recognition_utils::ConvexPolygon::Ptr convex
= jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointXYZRGB>(
cloud, plane_inliers, plane_coefficients);
if (convex) {
if (min_area_ > convex->area() || convex->area() > max_area_) {
continue;
}
{
// check direction consistency of coefficients and convex
Eigen::Vector3f coefficient_normal(plane_coefficients->values[0],
plane_coefficients->values[1],
plane_coefficients->values[2]);
if (convex->getNormalFromVertices().dot(coefficient_normal) < 0) {
convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex());
}
}
// Normal should direct to origin
{
Eigen::Vector3f p = convex->getPointOnPlane();
Eigen::Vector3f n = convex->getNormal();
if (p.dot(n) > 0) {
convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex());
Eigen::Vector3f new_normal = convex->getNormal();
plane_coefficients->values[0] = new_normal[0];
plane_coefficients->values[1] = new_normal[1];
plane_coefficients->values[2] = new_normal[2];
plane_coefficients->values[3] = convex->getD();
}
}
geometry_msgs::PolygonStamped polygon;
polygon.polygon = convex->toROSMsg();
polygon.header = msg->header;
ros_polygon.polygons.push_back(polygon);
all_inliers.push_back(plane_inliers);
all_coefficients.push_back(plane_coefficients);
}
}
}
jsk_recognition_msgs::ClusterPointIndices ros_indices;
ros_indices.cluster_indices =
pcl_conversions::convertToROSPointIndices(all_inliers, msg->header);
ros_indices.header = msg->header;
pub_inliers_.publish(ros_indices);
jsk_recognition_msgs::ModelCoefficientsArray ros_coefficients;
ros_coefficients.header = msg->header;
ros_coefficients.coefficients =
pcl_conversions::convertToROSModelCoefficients(
all_coefficients, msg->header);
pub_coefficients_.publish(ros_coefficients);
pub_polygons_.publish(ros_polygon);
}
}
bool SnapIt::processModelPlane(jsk_pcl_ros::CallSnapIt::Request& req,
jsk_pcl_ros::CallSnapIt::Response& res) {
// first build plane model
geometry_msgs::PolygonStamped target_plane = req.request.target_plane;
// check the size of the points
if (target_plane.polygon.points.size() < 3) {
NODELET_ERROR("not enough points included in target_plane");
return false;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
EigenVector3fVector points;
Eigen::Vector3f n, p;
if (!extractPointsInsidePlanePole(target_plane, inliers, points, n, p)) {
return false;
}
if (inliers->indices.size() < 3) {
NODELET_ERROR("not enough points inside of the target_plane");
return false;
}
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(input_);
extract.setIndices(inliers);
extract.filter(*points_inside_pole);
publishPointCloud(debug_candidate_points_pub_, points_inside_pole);
// estimate plane
pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices);
extractPlanePoints(points_inside_pole, plane_inliers, plane_coefficients,
n, req.request.eps_angle);
if (plane_inliers->indices.size () == 0)
{
NODELET_ERROR ("Could not estimate a planar model for the given dataset.");
return false;
}
// extract plane points
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_points (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setIndices (plane_inliers);
proj.setInputCloud (points_inside_pole);
proj.setModelCoefficients (plane_coefficients);
proj.filter (*plane_points);
publishPointCloud(debug_candidate_points_pub3_, plane_points);
// next, compute convexhull and centroid
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConcaveHull<pcl::PointXYZ> chull;
chull.setInputCloud (plane_points);
chull.setDimension(2);
chull.setAlpha (0.1);
chull.reconstruct (*cloud_hull);
if (cloud_hull->points.size() < 3) {
NODELET_ERROR("failed to estimate convex hull");
return false;
}
publishConvexHullMarker(cloud_hull);
Eigen::Vector4f C_new_4f;
pcl::compute3DCentroid(*cloud_hull, C_new_4f);
Eigen::Vector3f C_new;
for (size_t i = 0; i < 3; i++) {
C_new[i] = C_new_4f[i];
}
Eigen::Vector3f n_prime;
n_prime[0] = plane_coefficients->values[0];
n_prime[1] = plane_coefficients->values[1];
n_prime[2] = plane_coefficients->values[2];
plane_coefficients->values[3] = plane_coefficients->values[3] / n_prime.norm();
n_prime.normalize();
if (n_prime.dot(n) < 0) {
n_prime = - n_prime;
plane_coefficients->values[3] = - plane_coefficients->values[3];
}
Eigen::Vector3f n_cross = n.cross(n_prime);
double theta = asin(n_cross.norm());
Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized()));
// compute C
Eigen::Vector3f C_orig(0, 0, 0);
for (size_t i = 0; i < points.size(); i++) {
C_orig = C_orig + points[i];
}
C_orig = C_orig / points.size();
// compute C
Eigen::Vector3f C = trans * C_orig;
Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new - C) * trans * Eigen::Translation3f(C_orig).inverse();
tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation);
geometry_msgs::PointStamped centroid;
centroid.point.x = C_orig[0];
centroid.point.y = C_orig[1];
centroid.point.z = C_orig[2];
centroid.header = target_plane.header;
debug_centroid_pub_.publish(centroid);
geometry_msgs::PointStamped centroid_transformed;
centroid_transformed.point.x = C_new[0];
centroid_transformed.point.y = C_new[1];
centroid_transformed.point.z = C_new[2];
centroid_transformed.header = target_plane.header;
debug_centroid_after_trans_pub_.publish(centroid_transformed);
return true;
}
bool SnapIt::processModelCylinder(jsk_pcl_ros::CallSnapIt::Request& req,
jsk_pcl_ros::CallSnapIt::Response& res) {
pcl::PointCloud<pcl::PointXYZ>::Ptr candidate_points (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
Eigen::Vector3f n, C_orig;
if (!extractPointsInsideCylinder(req.request.center,
req.request.direction,
req.request.radius,
req.request.height,
inliers, n, C_orig,
1.3)) {
return false;
}
if (inliers->indices.size() < 3) {
NODELET_ERROR("not enough points inside of the target_plane");
return false;
}
geometry_msgs::PointStamped centroid;
centroid.point.x = C_orig[0];
centroid.point.y = C_orig[1];
centroid.point.z = C_orig[2];
centroid.header = req.request.header;
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(input_);
extract.setIndices(inliers);
extract.filter(*candidate_points);
publishPointCloud(debug_candidate_points_pub_, candidate_points);
// first, to remove plane we estimate the plane
pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices);
extractPlanePoints(candidate_points, plane_inliers, plane_coefficients,
n, req.request.eps_angle);
if (plane_inliers->indices.size() == 0) {
NODELET_ERROR ("plane estimation failed");
return false;
}
// remove the points blonging to the plane
pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole_wo_plane (new pcl::PointCloud<pcl::PointXYZ>);
extract.setInputCloud (candidate_points);
extract.setIndices (plane_inliers);
extract.setNegative (true);
extract.filter (*points_inside_pole_wo_plane);
publishPointCloud(debug_candidate_points_pub2_, points_inside_pole_wo_plane);
// normal estimation
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);
ne.setInputCloud (points_inside_pole_wo_plane);
ne.setKSearch (50);
ne.compute (*cloud_normals);
// segmentation
pcl::ModelCoefficients::Ptr cylinder_coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr cylinder_inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_CYLINDER);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setRadiusLimits (0.01, req.request.radius * 1.2);
seg.setDistanceThreshold (0.05);
seg.setInputCloud(points_inside_pole_wo_plane);
seg.setInputNormals (cloud_normals);
seg.setMaxIterations (10000);
seg.setNormalDistanceWeight (0.1);
seg.setAxis(n);
if (req.request.eps_angle != 0.0) {
seg.setEpsAngle(req.request.eps_angle);
}
else {
seg.setEpsAngle(0.35);
}
seg.segment (*cylinder_inliers, *cylinder_coefficients);
if (cylinder_inliers->indices.size () == 0)
{
NODELET_ERROR ("Could not estimate a cylinder model for the given dataset.");
return false;
}
debug_centroid_pub_.publish(centroid);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_points (new pcl::PointCloud<pcl::PointXYZ>);
extract.setInputCloud (points_inside_pole_wo_plane);
extract.setIndices (cylinder_inliers);
extract.setNegative (false);
extract.filter (*cylinder_points);
publishPointCloud(debug_candidate_points_pub3_, cylinder_points);
Eigen::Vector3f n_prime;
Eigen::Vector3f C_new;
for (size_t i = 0; i < 3; i++) {
C_new[i] = cylinder_coefficients->values[i];
n_prime[i] = cylinder_coefficients->values[i + 3];
}
double radius = fabs(cylinder_coefficients->values[6]);
// inorder to compute centroid, we project all the points to the center line.
// and after that, get the minimum and maximum points in the coordinate system of the center line
double min_alpha = DBL_MAX;
double max_alpha = -DBL_MAX;
for (size_t i = 0; i < cylinder_points->points.size(); i++ ) {
pcl::PointXYZ q = cylinder_points->points[i];
double alpha = (q.getVector3fMap() - C_new).dot(n_prime);
if (alpha < min_alpha) {
min_alpha = alpha;
}
if (alpha > max_alpha) {
max_alpha = alpha;
}
}
// the center of cylinder
Eigen::Vector3f C_new_prime = C_new + (max_alpha + min_alpha) / 2.0 * n_prime;
Eigen::Vector3f n_cross = n.cross(n_prime);
if (n.dot(n_prime)) {
n_cross = - n_cross;
}
double theta = asin(n_cross.norm());
Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized()));
Eigen::Vector3f C = trans * C_orig;
Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new_prime - C) * trans * Eigen::Translation3f(C_orig).inverse();
tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation);
geometry_msgs::PointStamped centroid_transformed;
centroid_transformed.point.x = C_new_prime[0];
centroid_transformed.point.y = C_new_prime[1];
centroid_transformed.point.z = C_new_prime[2];
centroid_transformed.header = req.request.header;
debug_centroid_after_trans_pub_.publish(centroid_transformed);
// publish marker
visualization_msgs::Marker marker;
marker.type = visualization_msgs::Marker::CYLINDER;
marker.scale.x = radius;
marker.scale.y = radius;
marker.scale.z = (max_alpha - min_alpha);
marker.pose.position.x = C_new_prime[0];
marker.pose.position.y = C_new_prime[1];
marker.pose.position.z = C_new_prime[2];
// n_prime -> z
// n_cross.normalized() -> x
Eigen::Vector3f z_axis = n_prime.normalized();
Eigen::Vector3f y_axis = n_cross.normalized();
Eigen::Vector3f x_axis = (y_axis.cross(z_axis)).normalized();
Eigen::Matrix3f M;
for (size_t i = 0; i < 3; i++) {
M(i, 0) = x_axis[i];
M(i, 1) = y_axis[i];
M(i, 2) = z_axis[i];
}
Eigen::Quaternionf q (M);
marker.pose.orientation.x = q.x();
marker.pose.orientation.y = q.y();
marker.pose.orientation.z = q.z();
marker.pose.orientation.w = q.w();
marker.color.g = 1.0;
marker.color.a = 1.0;
marker.header = input_header_;
marker_pub_.publish(marker);
return true;
}
bool seg_cb(bwi_perception::ButtonDetection::Request &req, bwi_perception::ButtonDetection::Response &res)
{
//get the point cloud by aggregating k successive input clouds
waitForCloudK(15);
cloud = cloud_aggregated;
//**Step 1: z-filter and voxel filter**//
// Create the filtering object
pcl::PassThrough<PointT> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 1.15);
pass.filter (*cloud);
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<PointT> vg;
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.0025f, 0.0025f, 0.0025f);
vg.filter (*cloud_filtered);
//publish point cloud for debugging
ROS_INFO("Publishing point cloud...");
/*pcl::toROSMsg(*cloud_filtered,cloud_ros);
cloud_ros.header.frame_id = cloud->header.frame_id;
cloud_pub.publish(cloud_ros);*/
ROS_INFO("After voxel grid filter: %i points",(int)cloud_filtered->points.size());
//**Step 2: plane fitting**//
//find palne
//one cloud contains plane other cloud contains other objects
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Create the segmentation object
pcl::SACSegmentation<PointT> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.02);
// Create the filtering object
pcl::ExtractIndices<PointT> extract;
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
// Extract the plane
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_plane);
//for everything else, cluster extraction; segment extraction
//extract everything else
extract.setNegative (true);
extract.filter (*cloud_blobs);
//get the plane coefficients
Eigen::Vector4f plane_coefficients;
plane_coefficients(0)=coefficients->values[0];
plane_coefficients(1)=coefficients->values[1];
plane_coefficients(2)=coefficients->values[2];
plane_coefficients(3)=coefficients->values[3];
//**Step 3: Eucledian Cluster Extraction**//
std::vector<PointCloudT::Ptr > clusters = computeClusters(cloud_blobs,0.04);
ROS_INFO("clustes found: %i", (int)clusters.size());
clusters_on_plane.clear();
//if clusters are touching the table put them in a vector
for (unsigned int i = 0; i < clusters.size(); i++){
Eigen::Vector4f centroid_i;
pcl::compute3DCentroid(*clusters.at(i), centroid_i);
pcl::PointXYZ center;
center.x=centroid_i(0);center.y=centroid_i(1);center.z=centroid_i(2);
double distance = pcl::pointToPlaneDistance(center, plane_coefficients);
if (distance < 0.1 /*&& clusters.at(i).get()->points.size() >*/ ){
clusters_on_plane.push_back(clusters.at(i));
}
}
ROS_INFO("clustes_on_plane found: %i", (int)clusters_on_plane.size());
// if the clousters size == 0 return false
if(clusters_on_plane.size() == 0) {
cloud_mutex.unlock ();
res.button_found = false;
return true;
}
//**Step 4: detect the button among the remaining clusters**//
int max_index = -1;
double max_red = 0.0;
// Find the max red value
for (unsigned int i = 0; i < clusters_on_plane.size(); i++){
double red_i = computeAvgRedValue(clusters_on_plane.at(i));
//ROS_INFO("Cluster %i: %i points, red_value = %f",i,(int)clusters_on_plane.at(i)->points.size(),red_i);
if (red_i > max_red){
max_red = red_i;
max_index = i;
}
}
//publish cloud if we think it's a button
/*max_red > 170 && max_red < 250 && */
ROS_INFO("max_red=%f", max_red);
if (max_index >= 0 && max_red > red_min) {
ROS_INFO("Button_found");
pcl::toROSMsg(*clusters_on_plane.at(max_index),cloud_ros);
cloud_ros.header.frame_id = cloud->header.frame_id;
cloud_pub.publish(cloud_ros);
//fill in response
res.button_found = true;
res.cloud_button = cloud_ros;
/*Eigen::Vector4f centroid;
pcl::compute3DCentroid(*clusters_on_plane.at(max_index), centroid);
//transforms the pose into /map frame
geometry_msgs::Pose pose_i;
pose_i.position.x=centroid(0);
pose_i.position.y=centroid(1);
pose_i.position.z=centroid(2);
pose_i.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,0,-3.14/2);
geometry_msgs::PoseStamped stampedPose;
stampedPose.header.frame_id = cloud->header.frame_id;
stampedPose.header.stamp = ros::Time(0);
stampedPose.pose = pose_i;*/
//geometry_msgs::PoseStamped stampOut;
//listener.waitForTransform(cloud->header.frame_id, "m1n6s200_link_base", ros::Time(0), ros::Duration(3.0));
//listener.transformPose("m1n6s200_link_base", stampedPose, stampOut);
//stampOut.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,-3.14/2,0);
//pose_pub.publish(stampOut);
}
else {
res.button_found = false;
}
cloud_mutex.unlock ();
return true;
}
/**
* extract handles from a pointcloud
* @param[pointCloudIn] input point cloud
* @param[pointCloudNormals] normals for the input cloud
* @param[handle_indices] vector of indices, each item being a set of indices representing a handle
* @param[handle_coefficients] vector of cofficients, each item being the coefficients of the line representing the handle
*/
void HandleExtractor::extractHandles(PointCloud::Ptr &cloudInput, PointCloudNormal::Ptr &pointCloudNormals,
std::vector< pcl::PointIndices> &handle_indices, std::vector< pcl::ModelCoefficients> &handle_coefficients
)
{
// setup handle cluster object
pcl::EuclideanClusterExtraction<Point> handle_cluster_;
KdTreePtr clusters_tree_(new KdTree);
handle_cluster_.setClusterTolerance(handle_cluster_tolerance);
handle_cluster_.setMinClusterSize(min_handle_cluster_size);
handle_cluster_.setSearchMethod(clusters_tree_);
handle_cluster_.setInputCloud(cloudInput);
pcl::PointCloud<Point>::Ptr cloud_filtered(new pcl::PointCloud<Point>());
pcl::VoxelGrid<Point> vg;
vg.setInputCloud(cloudInput);
vg.setLeafSize(0.01, 0.01, 0.01);
vg.filter(*cloud_filtered);
// setup line ransac object
pcl::SACSegmentation<Point> seg_line_;
seg_line_.setModelType(pcl::SACMODEL_LINE);
seg_line_.setMethodType(pcl::SAC_RANSAC);
seg_line_.setInputCloud(cloudInput);
seg_line_.setDistanceThreshold(line_ransac_distance);
seg_line_.setOptimizeCoefficients(true);
seg_line_.setMaxIterations(line_ransac_max_iter);
// setup Inlier projection object
pcl::ProjectInliers<Point> proj_;
proj_.setInputCloud(cloud_filtered);
proj_.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
// setup polygonal prism
pcl::ExtractPolygonalPrismData<Point> prism_;
prism_.setHeightLimits(min_handle_height, max_handle_height);
prism_.setInputCloud(cloudInput);
//setup Plane Segmentation
outInfo("Segmenting planes");
pcl::SACSegmentation<Point> seg_plane_;
seg_plane_.setOptimizeCoefficients(true);
seg_plane_.setModelType(pcl::SACMODEL_PLANE);
seg_plane_.setMethodType(pcl::SAC_RANSAC);
seg_plane_.setDistanceThreshold(0.03);
seg_plane_.setMaxIterations(500);
seg_plane_.setInputCloud(cloud_filtered);
pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients());
pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices());
seg_plane_.segment(*plane_inliers, *plane_coefficients);
if(plane_inliers->indices.size() != 0)
{
outDebug("Plane model: "
<< plane_coefficients->values[0] << ","
<< plane_coefficients->values[1] << ","
<< plane_coefficients->values[2] << ","
<< plane_coefficients->values[3] << " with "
<< (int)plane_inliers->indices.size() << "inliers ");
//Project inliers of the planes into a perfect plane
PointCloud::Ptr cloud_projected(new PointCloud());
proj_.setIndices(plane_inliers);
proj_.setModelCoefficients(plane_coefficients);
proj_.filter(*cloud_projected);
// Create a Convex Hull representation using the projected inliers
PointCloud::Ptr cloud_hull(new PointCloud());
pcl::ConvexHull<Point> chull_;
chull_.setDimension(2);
chull_.setInputCloud(cloud_projected);
chull_.reconstruct(*cloud_hull);
outInfo("Convex hull has: " << (int)cloud_hull->points.size() << " data points.");
if((int) cloud_hull->points.size() == 0)
{
outInfo("Convex hull has: no data points. Returning.");
return;
}
// Extract the handle clusters using a polygonal prism
pcl::PointIndices::Ptr handlesIndicesPtr(new pcl::PointIndices());
prism_.setInputPlanarHull(cloud_hull);
prism_.segment(*handlesIndicesPtr);
// cluster the points found in the prism
std::vector< pcl::PointIndices> handle_clusters;
handle_cluster_.setIndices(handlesIndicesPtr);
handle_cluster_.extract(handle_clusters);
for(size_t j = 0; j < handle_clusters.size(); j++)
{
// for each cluster in the prism, attempt to fit a line using ransac
pcl::PointIndices single_handle_indices;
pcl::ModelCoefficients handle_line_coefficients;
seg_line_.setIndices(getIndicesPointerFromObject(handle_clusters[j]));
seg_line_.segment(single_handle_indices, handle_line_coefficients);
if(single_handle_indices.indices.size() > 0)
{
outInfo("Found a handle with " << single_handle_indices.indices.size() << " inliers.");
outDebug("Handle Line coefficients: " << handle_line_coefficients);
handle_indices.push_back(single_handle_indices);
handle_coefficients.push_back(handle_line_coefficients);
}
}
}
else
{
outInfo("no planes found");
return;
}
}