void RosPlaneSegmenter::segmentPlaneIfNotExist(const sensor_msgs::PointCloud2 &input_cloud)
{
if (!initialized_)
{
std::cerr << "ERROR, RosPlaneSegmenter is not initialized yet.\n";
return;
}
else if (!this->ready)
{
ROS_INFO("Performing plane segmentation");
// perform table segmentation
CloudXYZRGBA::Ptr cloud(new CloudXYZRGBA());
fromROSMsg(input_cloud,*cloud);
CloudXYZRGBA::Ptr cloud_filtered (new CloudXYZRGBA());
std::string table_tf_parent;
tf::StampedTransform transform;
if (!listener_->frameExists(table_tf_name_))
{
ROS_WARN("Fail to find table frame [%s]",table_tf_name_.c_str());
return;
}
listener_->getParent(table_tf_name_,ros::Time(0),table_tf_parent);
if (use_rosbag_ || listener_->waitForTransform(table_tf_parent,table_tf_name_,ros::Time::now(),ros::Duration(1.5)))
{
ROS_INFO("Found table frame [%s]",table_tf_name_.c_str());
listener_->lookupTransform(table_tf_parent,table_tf_name_,ros::Time(0),transform);
Eigen::Affine3d box_pose;
tf::transformTFToEigen(transform, box_pose);
Eigen::Affine3f box_pose_float(box_pose);
cloud_filtered = this->cropBox(*cloud, box_pose_float, Eigen::Vector3f(0.5,0.5,0.5));
}
else
{
ROS_WARN("Fail to find table frame [%s]",table_tf_name_.c_str());
return;
}
CloudXYZRGBA::Ptr table_hull;
if (use_tf_surface_)
{
Eigen::Affine3d plane_pose;
tf::transformTFToEigen(transform, plane_pose);
Eigen::Affine3f plane_pose_float(plane_pose);
table_hull = this->generatePlaneConvexHullFromPoseXYplane(plane_pose_float,0.5);
}
else
{
table_hull = this->generatePlaneConvexHull(*cloud_filtered);
}
if (table_hull->size() > 0)
{
this->setPlaneConvexHull(*table_hull);
if (update_table_) writer_.write<pcl::PointXYZRGBA> (load_table_path_, *table_hull, true);
}
}
}
void VoxelGridDownsampleManager::pointCB(const sensor_msgs::PointCloud2ConstPtr &input)
{
try {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
if (grid_.size() == 0) {
ROS_INFO("the number of registered grids is 0, skipping");
return;
}
fromROSMsg(*input, *cloud);
for (size_t i = 0; i < grid_.size(); i++)
{
visualization_msgs::Marker::ConstPtr target_grid = grid_[i];
// not yet tf is supported
int id = target_grid->id;
// solve tf with ros::Time 0.0
// transform pointcloud to the frame_id of target_grid
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl_ros::transformPointCloud(target_grid->header.frame_id,
*cloud,
*transformed_cloud,
tf_listener);
double center_x = target_grid->pose.position.x;
double center_y = target_grid->pose.position.y;
double center_z = target_grid->pose.position.z;
double range_x = target_grid->scale.x * 1.0; // for debug
double range_y = target_grid->scale.y * 1.0;
double range_z = target_grid->scale.z * 1.0;
double min_x = center_x - range_x / 2.0;
double max_x = center_x + range_x / 2.0;
double min_y = center_y - range_y / 2.0;
double max_y = center_y + range_y / 2.0;
double min_z = center_z - range_z / 2.0;
double max_z = center_z + range_z / 2.0;
double resolution = target_grid->color.r;
// filter order: x -> y -> z -> downsample
pcl::PassThrough<pcl::PointXYZRGB> pass_x;
pass_x.setFilterFieldName("x");
pass_x.setFilterLimits(min_x, max_x);
pcl::PassThrough<pcl::PointXYZRGB> pass_y;
pass_y.setFilterFieldName("y");
pass_y.setFilterLimits(min_y, max_y);
pcl::PassThrough<pcl::PointXYZRGB> pass_z;
pass_z.setFilterFieldName("z");
pass_z.setFilterLimits(min_z, max_z);
ROS_INFO_STREAM(id << " filter x: " << min_x << " - " << max_x);
ROS_INFO_STREAM(id << " filter y: " << min_y << " - " << max_y);
ROS_INFO_STREAM(id << " filter z: " << min_z << " - " << max_z);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_after_x (new pcl::PointCloud<pcl::PointXYZRGB>);
pass_x.setInputCloud (transformed_cloud);
pass_x.filter(*cloud_after_x);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_after_y (new pcl::PointCloud<pcl::PointXYZRGB>);
pass_y.setInputCloud (cloud_after_x);
pass_y.filter(*cloud_after_y);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_after_z (new pcl::PointCloud<pcl::PointXYZRGB>);
pass_z.setInputCloud (cloud_after_y);
pass_z.filter(*cloud_after_z);
// downsample
pcl::VoxelGrid<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud_after_z);
sor.setLeafSize (resolution, resolution, resolution);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
sor.filter (*cloud_filtered);
// reverse transform
pcl::PointCloud<pcl::PointXYZRGB>::Ptr reverse_transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl_ros::transformPointCloud(input->header.frame_id,
*cloud_filtered,
*reverse_transformed_cloud,
tf_listener);
// adding the output into *output_cloud
// tmp <- cloud_filtered + output_cloud
// output_cloud <- tmp
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp (new pcl::PointCloud<pcl::PointXYZRGB>);
//pcl::concatenatePointCloud (*cloud_filtered, *output_cloud, tmp);
//output_cloud = tmp;
ROS_INFO_STREAM(id << " includes " << reverse_transformed_cloud->points.size() << " points");
for (size_t i = 0; i < reverse_transformed_cloud->points.size(); i++) {
output_cloud->points.push_back(reverse_transformed_cloud->points[i]);
}
}
sensor_msgs::PointCloud2 out;
toROSMsg(*output_cloud, out);
out.header = input->header;
pub_.publish(out); // for debugging
// for concatenater
size_t cluster_num = output_cloud->points.size() / max_points_ + 1;
ROS_INFO_STREAM("encoding into " << cluster_num << " clusters");
for (size_t i = 0; i < cluster_num; i++) {
size_t start_index = max_points_ * i;
size_t end_index = max_points_ * (i + 1) > output_cloud->points.size() ?
output_cloud->points.size(): max_points_ * (i + 1);
sensor_msgs::PointCloud2 cluster_out_ros;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr
cluster_out_pcl(new pcl::PointCloud<pcl::PointXYZRGB>);
cluster_out_pcl->points.resize(end_index - start_index);
// build cluster_out_pcl
ROS_INFO_STREAM("make cluster from " << start_index << " to " << end_index);
for (size_t j = start_index; j < end_index; j++) {
cluster_out_pcl->points[j - start_index] = output_cloud->points[j];
}
// conevrt cluster_out_pcl into ros msg
toROSMsg(*cluster_out_pcl, cluster_out_ros);
jsk_pcl_ros::SlicedPointCloud publish_point_cloud;
cluster_out_ros.header = input->header;
publish_point_cloud.point_cloud = cluster_out_ros;
publish_point_cloud.slice_index = i;
publish_point_cloud.sequence_id = sequence_id_;
pub_encoded_.publish(publish_point_cloud);
ros::Duration(1.0 / rate_).sleep();
}
}
catch (std::runtime_error e) { // catch any error
NODELET_WARN_STREAM("error has occured in VoxelGridDownsampleManager but ignore it: " << e.what());
ros::Duration(1.0 / rate_).sleep();
}
}
template<class T> void filter (const sensor_msgs::PointCloud2::ConstPtr &input,
const PCLIndicesMsg::ConstPtr &indices) {
pcl::PointCloud<T> pcl_input_cloud, output;
fromROSMsg(*input, pcl_input_cloud);
boost::mutex::scoped_lock lock (mutex_);
std::vector<int> ex_indices;
ex_indices.resize(0);
int width = input->width;
int height = input->height;
int ox, oy, sx, sy;
sx = step_x_;
ox = sx/2;
if(height == 1) {
sy = 1;
oy = 0;
} else {
sy = step_y_;
oy = sy/2;
}
if (indices) {
std::vector<int> flags;
flags.resize(width*height);
//std::vector<int>::iterator it;
//for(it = indices->begin(); it != indices->end(); it++)
//flags[*it] = 1;
for(unsigned int i = 0; i < indices->indices.size(); i++) {
flags[indices->indices.at(i)] = 1;
}
for(int y = oy; y < height; y += sy) {
for(int x = ox; x < width; x += sx) {
if (flags[y*width + x] == 1) {
ex_indices.push_back(y*width + x); // just use points in indices
}
}
}
} else {
for(int y = oy; y < height; y += sy) {
for(int x = ox; x < width; x += sx) {
ex_indices.push_back(y*width + x);
}
}
}
pcl::ExtractIndices<T> extract;
extract.setInputCloud (pcl_input_cloud.makeShared());
extract.setIndices (boost::make_shared <std::vector<int> > (ex_indices));
extract.setNegative (false);
extract.filter (output);
if (output.points.size() > 0) {
sensor_msgs::PointCloud2 ros_out;
toROSMsg(output, ros_out);
ros_out.header = input->header;
ros_out.width = (width - ox)/sx;
if((width - ox)%sx) ros_out.width += 1;
ros_out.height = (height - oy)/sy;
if((height - oy)%sy) ros_out.height += 1;
ros_out.row_step = ros_out.point_step * ros_out.width;
ros_out.is_dense = input->is_dense;
#if DEBUG
JSK_NODELET_INFO("%dx%d (%d %d)(%d %d) -> %dx%d %d", width,height, ox, oy, sx, sy,
ros_out.width, ros_out.height, ex_indices.size());
#endif
pub_.publish(ros_out);
JSK_NODELET_INFO("%s:: input header stamp is [%f]", getName().c_str(),
input->header.stamp.toSec());
JSK_NODELET_INFO("%s:: output header stamp is [%f]", getName().c_str(),
ros_out.header.stamp.toSec());
}
}
// callback function when the object descriptors are received
// this will also trigger the recognition if all the other keypoints and descriptors have been received
void object_descriptor_shot352_cb (const object_recognition::Shot352_bundle::Ptr input)
{
ros::NodeHandle nh_param("~");
nh_param.param<double>("maximum_descriptor_distance" , max_descr_dist_ , 0.25 );
// check if world was already processed
if (world_descriptors_shot352 == NULL)
{
ROS_WARN("Received object descriptors before having a world pointcloud to compare");
return;
}
// check if the stored world descriptors can be assinged to the stored keypoints
if ((int)world_keypoints->size() != (int)world_descriptors_shot352->size())
{
ROS_WARN("Received %i descriptors and %i keypoints for the world. Number must be equal", (int)world_descriptors_shot352->size(), (int)world_keypoints->size());
return;
}
// check if the received object descriptors can be assigned to the stored keypoints
if ((int)object_keypoints->size() != (int)input->descriptors.size())
{
ROS_WARN("Received %i descriptors and %i keypoints for the object. Number must be equal", (int)input->descriptors.size(), (int)object_keypoints->size());
return;
}
// Debug output
ROS_INFO("Received %i descriptors for the world and %i for the object", (int)world_descriptors_shot352->size(), (int)input->descriptors.size());
object_descriptors_shot352 = DescriptorCloudShot352::Ptr (new DescriptorCloudShot352 ());
fromROSMsg(*input, *object_descriptors_shot352);
//
// Find Object-World Correspondences with KdTree
//
cout << "... finding correspondences ..." << endl;
pcl::CorrespondencesPtr object_world_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<SHOT352> match_search;
match_search.setInputCloud (object_descriptors_shot352);
// For each world keypoint descriptor
// find nearest neighbor into the object keypoints descriptor cloud
// and add it to the correspondences vector
for (size_t i = 0; i < world_descriptors_shot352->size (); ++i)
{
std::vector<int> neigh_indices (1);
std::vector<float> neigh_sqr_dists (1);
if (!pcl_isfinite (world_descriptors_shot352->at (i).descriptor[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (world_descriptors_shot352->at (i), 1, neigh_indices, neigh_sqr_dists);
// add match only if the squared descriptor distance is less than 0.25
// SHOT descriptor distances are between 0 and 1 by design
if(found_neighs == 1 && neigh_sqr_dists[0] < (float)max_descr_dist_)
{
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
object_world_corrs->push_back (corr);
}
}
std::cout << "Correspondences found: " << object_world_corrs->size () << std::endl;
//
// all keypoints and descriptors were found, no match the correspondences to the real object!
//
cluster(object_world_corrs);
}
// Callback function when the world descriptors are received
void world_descriptor_shot1344_cb (const object_recognition::Shot1344_bundle::Ptr input)
{
world_descriptors_shot1344 = DescriptorCloudShot1344::Ptr (new DescriptorCloudShot1344 ());
fromROSMsg(*input, *world_descriptors_shot1344);
}
// Callback function when the world descriptors are received
void world_descriptor_shot352_cb (const object_recognition::Shot352_bundle::Ptr input)
{
world_descriptors_shot352 = DescriptorCloudShot352::Ptr (new DescriptorCloudShot352 ());
fromROSMsg(*input, *world_descriptors_shot352);
}
// callback function when the object descriptors are received
// this will also trigger the recognition if all the other keypoints and descriptors have been received
void object_descriptor_shot1344_cb (const object_recognition::Shot1344_bundle::Ptr input)
{
ros::NodeHandle nh_param("~");
nh_param.param<double>("maximum_descriptor_distance" , max_descr_dist_ , 0.25 );
// check if world was already processed
if (world_descriptors_shot1344 == NULL)
{
ROS_WARN("Received object descriptors before having a world pointcloud to compare");
return;
}
// check if the stored world descriptors can be assinged to the stored keypoints
if ((int)world_keypoints->size() != (int)world_descriptors_shot1344->size())
{
ROS_WARN("Received %i descriptors and %i keypoints for the world. Number must be equal", (int)world_descriptors_shot1344->size(), (int)world_keypoints->size());
return;
}
// check if the received object descriptors can be assigned to the stored keypoints
if ((int)object_keypoints->size() != (int)input->descriptors.size())
{
ROS_WARN("Received %i descriptors and %i keypoints for the object. Number must be equal", (int)input->descriptors.size(), (int)object_keypoints->size());
return;
}
object_descriptors_shot1344 = DescriptorCloudShot1344::Ptr (new DescriptorCloudShot1344 ());
fromROSMsg(*input, *object_descriptors_shot1344);
// Debug output
ROS_INFO("Received %i descriptors for the world and %i for the object", (int)world_descriptors_shot1344->size(), (int)input->descriptors.size());
//
// Find Object-World Correspondences with KdTree
//
cout << "... finding correspondences ..." << endl;
pcl::CorrespondencesPtr object_world_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<SHOT1344> match_search;
match_search.setInputCloud (object_descriptors_shot1344);
// For each world keypoint descriptor
// find nearest neighbor into the object keypoints descriptor cloud
// and add it to the correspondences vector
for (size_t i = 0; i < world_descriptors_shot1344->size (); ++i)
{
std::vector<int> neigh_indices (1);
std::vector<float> neigh_sqr_dists (1);
if (!pcl_isfinite (world_descriptors_shot1344->at (i).descriptor[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (world_descriptors_shot1344->at (i), 1, neigh_indices, neigh_sqr_dists);
// add match only if the squared descriptor distance is less than max_descr_dist_
// SHOT descriptor distances are between 0 and 1 by design
if(found_neighs == 1 && neigh_sqr_dists[0] < (float)max_descr_dist_)
{
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
// check if new correspondence is better than any previous at this point
bool found_better_result = false;
for (int j = 0; j < object_world_corrs->size(); ++j)
{
// is the found neigbor the same one like in the correspondence j
if (object_world_corrs->at(j).index_query == neigh_indices[0])
{
// do not add a new correspondence later
found_better_result = true;
// is the new distance smaller? (that means better)
if (neigh_sqr_dists[0] < object_world_corrs->at(j).distance)
{
// replace correspondence with better one
object_world_corrs->at(j) = corr;
}
else
// break out of inner loop to save time and try next keypoint
break;
}
}
// if this is a new correspondence, add a new correspondence at the end
if (!found_better_result)
object_world_corrs->push_back (corr);
}
}
std::cout << "Correspondences found: " << object_world_corrs->size () << std::endl;
//
// all correspondences were found, now match the correspondences to the real object!
//
cluster(object_world_corrs);
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// std::cout << "\n\n----------------Received point cloud!-----------------\n";
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsampled (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_planar (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_objects (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_red (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_green (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_blue (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_yellow (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_concat_clusters (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_concat_hulls (new pcl::PointCloud<pcl::PointXYZRGB>);
sensor_msgs::PointCloud2 downsampled2, planar2, objects2, filtered2, red2, green2, blue2, yellow2, concat_clusters2, concat_hulls2;
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> color_clouds, cluster_clouds, hull_clouds;
std::vector<pcl::PointXYZRGB> labels;
// fromROSMsg(*input, *cloud);
// pub_input.publish(*input);
// Downsample the input PointCloud
pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
sor.setInputCloud (input);
// sor.setLeafSize (0.01, 0.01, 0.01); //play around with leafsize (more samples, better resolution?)
sor.setLeafSize (0.001, 0.001, 0.001);
sor.filter (downsampled2);
fromROSMsg(downsampled2,*downsampled);
pub_downsampled.publish (downsampled2);
// Segment the largest planar component from the downsampled cloud
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
pcl::ModelCoefficients::Ptr coeffs (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.0085);
seg.setInputCloud (downsampled);
seg.segment (*inliers, *coeffs);
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (downsampled);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_planar);
// toROSMsg(*cloud_planar,planar2);
// pub_planar.publish (planar2);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_objects);
// toROSMsg(*cloud_objects,objects2);
// pub_objects.publish (objects2);
// PassThrough Filter
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (cloud_objects);
pass.setFilterFieldName ("z"); //all duplos in pcd1
pass.setFilterLimits (0.8, 1.0);
pass.filter (*cloud_filtered);
toROSMsg(*cloud_filtered,filtered2);
pub_filtered.publish (filtered2);
//don't passthrough filter, does color filter work too? (cloud_red has many points in top right off the table)
// Segment filtered PointCloud by color (red, green, blue, yellow)
for (size_t i = 0 ; i < cloud_filtered->points.size () ; i++)
{
if ( (int(cloud_filtered->points[i].r) > 2*int(cloud_filtered->points[i].g)) && (cloud_filtered->points[i].r > cloud_filtered->points[i].b) )
cloud_red->points.push_back(cloud_filtered->points[i]);
if ( (cloud_filtered->points[i].g > cloud_filtered->points[i].r) && (cloud_filtered->points[i].g > cloud_filtered->points[i].b) )
cloud_green->points.push_back(cloud_filtered->points[i]);
if ( (cloud_filtered->points[i].b > cloud_filtered->points[i].r) && (cloud_filtered->points[i].b > cloud_filtered->points[i].g) )
cloud_blue->points.push_back(cloud_filtered->points[i]);
if ( (cloud_filtered->points[i].r > cloud_filtered->points[i].g) && (int(cloud_filtered->points[i].g) - int(cloud_filtered->points[i].b) > 30) )
cloud_yellow->points.push_back(cloud_filtered->points[i]);
}
cloud_red->header.frame_id = "base_link";
cloud_red->width = cloud_red->points.size ();
cloud_red->height = 1;
color_clouds.push_back(cloud_red);
toROSMsg(*cloud_red,red2);
pub_red.publish (red2);
cloud_green->header.frame_id = "base_link";
cloud_green->width = cloud_green->points.size ();
cloud_green->height = 1;
color_clouds.push_back(cloud_green);
toROSMsg(*cloud_green,green2);
pub_green.publish (green2);
cloud_blue->header.frame_id = "base_link";
cloud_blue->width = cloud_blue->points.size ();
cloud_blue->height = 1;
color_clouds.push_back(cloud_blue);
toROSMsg(*cloud_blue,blue2);
pub_blue.publish (blue2);
cloud_yellow->header.frame_id = "base_link";
cloud_yellow->width = cloud_yellow->points.size ();
cloud_yellow->height = 1;
color_clouds.push_back(cloud_yellow);
toROSMsg(*cloud_yellow,yellow2);
pub_yellow.publish (yellow2);
// Extract Euclidian clusters from color-segmented PointClouds
int j(0), num_red (0), num_green(0), num_blue(0), num_yellow(0);
for (size_t cit = 0 ; cit < color_clouds.size() ; cit++)
{
pcl::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
tree->setInputCloud (color_clouds[cit]);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (0.0075); // 0.01
// ec.setMinClusterSize (12);
// ec.setMaxClusterSize (75);
ec.setMinClusterSize (100);
ec.setMaxClusterSize (4000);
ec.setSearchMethod (tree);
ec.setInputCloud (color_clouds[cit]);
ec.extract (cluster_indices);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
for (std::vector<int>::const_iterator pit = it->indices.begin () ; pit != it->indices.end () ; pit++)
cloud_cluster->points.push_back (color_clouds[cit]->points[*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
cloud_cluster->header.frame_id = "base_link";
cluster_clouds.push_back(cloud_cluster);
labels.push_back(cloud_cluster->points[0]);
if (cit == 0) num_red++;
if (cit == 1) num_green++;
if (cit == 2) num_blue++;
if (cit == 3) num_yellow++;
// Create ConvexHull for cluster (keep points on perimeter of cluster)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::ConvexHull<pcl::PointXYZRGB> chull;
chull.setInputCloud (cloud_cluster);
chull.reconstruct (*cloud_hull);
cloud_hull->width = cloud_hull->points.size ();
cloud_hull->height = 1;
cloud_hull->header.frame_id = "base_link";
hull_clouds.push_back(cloud_hull);
j++;
}
}
std::cout << "Number of RED clusters: " << num_red << std::endl;
std::cout << "Number of GREEN clusters: " << num_green << std::endl;
std::cout << "Number of BLUE clusters: " << num_blue << std::endl;
std::cout << "Number of YELLOW clusters: " << num_yellow << std::endl;
std::cout << "TOTAL number of clusters: " << j << std::endl;
// Concatenate PointCloud clusters and convex hulls
for (size_t k = 0 ; k < cluster_clouds.size() ; k++)
{
for (size_t l = 0 ; l < cluster_clouds[k]->size() ; l++)
{
cloud_concat_clusters->points.push_back(cluster_clouds[k]->points[l]);
cloud_concat_hulls->points.push_back(hull_clouds[k]->points[l]);
}
}
cloud_concat_clusters->header.frame_id = "base_link";
cloud_concat_clusters->width = cloud_concat_clusters->points.size ();
cloud_concat_clusters->height = 1;
toROSMsg(*cloud_concat_clusters,concat_clusters2);
pub_concat_clusters.publish (concat_clusters2);
cloud_concat_hulls->header.frame_id = "base_link";
cloud_concat_hulls->width = cloud_concat_hulls->points.size ();
cloud_concat_hulls->height = 1;
toROSMsg(*cloud_concat_hulls,concat_hulls2);
pub_concat_hulls.publish (concat_hulls2);
// Estimate the volume of each cluster
double height, width;
std::vector <double> heights, widths;
std::vector <int> height_ids, width_ids;
for (size_t k = 0 ; k < cluster_clouds.size() ; k++)
{
// Calculate cluster height
double tallest(0), shortest(1000), widest(0) ;
for (size_t l = 0 ; l < cluster_clouds[k]->size() ; l++)
{
double point_to_plane_dist;
point_to_plane_dist = coeffs->values[0] * cluster_clouds[k]->points[l].x +
coeffs->values[1] * cluster_clouds[k]->points[l].y +
coeffs->values[2] * cluster_clouds[k]->points[l].z + coeffs->values[3] /
sqrt( pow(coeffs->values[0], 2) + pow(coeffs->values[1], 2)+ pow(coeffs->values[2], 2) );
if (point_to_plane_dist < 0) point_to_plane_dist = 0;
if (point_to_plane_dist > tallest) tallest = point_to_plane_dist;
if (point_to_plane_dist < shortest) shortest = point_to_plane_dist;
}
// Calculate cluster width
for (size_t m = 0 ; m < hull_clouds[k]->size() ; m++)
{
double parallel_vector_dist;
for (size_t n = m ; n < hull_clouds[k]->size()-1 ; n++)
{
parallel_vector_dist = sqrt( pow(hull_clouds[k]->points[m].x - hull_clouds[k]->points[n+1].x,2) +
pow(hull_clouds[k]->points[m].y - hull_clouds[k]->points[n+1].y,2) +
pow(hull_clouds[k]->points[m].z - hull_clouds[k]->points[n+1].z,2) );
if (parallel_vector_dist > widest) widest = parallel_vector_dist;
}
}
// Classify block heights (error +/- .005m)
height = (shortest < .01) ? tallest : tallest - shortest; //check for stacked blocks
heights.push_back(height);
if (height > .020 && height < .032) height_ids.push_back(0); //0: standing flat
else if (height > .036 && height < .043) height_ids.push_back(1); //1: standing side
else if (height > .064) height_ids.push_back(2); //2: standing long
else height_ids.push_back(-1); //height not classified
// Classify block widths (error +/- .005m)
width = widest;
widths.push_back(widest);
if (width > .032-.005 && width < .0515+.005) width_ids.push_back(1); //1: short
else if (width > .065-.005 && width < .0763+.005) width_ids.push_back(2); //2: medium
else if (width > .1275-.005 && width < .1554+.005) width_ids.push_back(4); //4: long
else width_ids.push_back(-1); //width not classified
}
// Classify block size using width information
std::vector<int> block_ids, idx_1x1, idx_1x2, idx_1x4, idx_unclassified;
int num_1x1(0), num_1x2(0), num_1x4(0), num_unclassified(0);
for (size_t p = 0 ; p < width_ids.size() ; p++)
{
if (width_ids[p] == 1)
{
block_ids.push_back(1); //block is 1x1
idx_1x1.push_back(p);
num_1x1++;
}
else if (width_ids[p] == 2)
{
block_ids.push_back(2); //block is 1x2
idx_1x2.push_back(p);
num_1x2++;
}
else if (width_ids[p] == 4)
{
block_ids.push_back(4); //block is 1x4
idx_1x4.push_back(p);
num_1x4++;
}
else
{
block_ids.push_back(-1); //block not classified
idx_unclassified.push_back(p);
num_unclassified++;
}
}
// Determine Duplos of the same size
std::cout << "\nThere are " << num_1x1 << " blocks of size 1x1 ";
if (num_1x1>0) std::cout << "(cluster index: ";
for (size_t q = 0 ; q < idx_1x1.size() ; q++)
std::cout << idx_1x1[q] << ", ";
if (num_1x1>0) std::cout << ")";
std::cout << "\nThere are " << num_1x2 << " blocks of size 1x2 ";
if (num_1x2>0) std::cout << "(cluster index: ";
for (size_t q = 0 ; q < idx_1x2.size() ; q++)
std::cout << idx_1x2[q] << ", ";
if (num_1x2>0) std::cout << ")";
std::cout << "\nThere are " << num_1x4 << " blocks of size 1x4 ";
if (num_1x4>0) std::cout << "(cluster index: ";
for (size_t q = 0 ; q < idx_1x4.size() ; q++)
std::cout << idx_1x4[q] << ", ";
if (num_1x4>0) std::cout << ")";
std::cout << "\nThere are " << num_unclassified << " unclassified blocks ";
if (num_unclassified>0) std::cout << "(cluster index: ";
for (size_t q = 0 ; q < idx_unclassified.size() ; q++)
std::cout << idx_unclassified[q] << ", ";
if (num_unclassified>0) std::cout << ")";
std::cout << "\n\n\n";
return;
}
