// TODO: This method is only used for planar segmentation, consider removing
Box3D
minAreaRect(pcl::PointCloud<PointT>::Ptr &input_cloud)
{
//assuming chull is already sequential, if not we'll need to sort it. It seems like it is.
Box3D box;
pcl::PointCloud<PointT>::Ptr cloud_hull (new pcl::PointCloud<PointT>);
pcl::ConvexHull<PointT> chull;
chull.setInputCloud (input_cloud);
chull.reconstruct (*cloud_hull);
int n = cloud_hull->size();
PointT out[3];
if( n > 2 )
{
rotatingCalipers( cloud_hull, n, CALIPERS_MINAREARECT, out );
box.center.x = out[0].x + (out[1].x + out[2].x)*0.5f;
box.center.y = out[0].y + (out[1].y + out[2].y)*0.5f;
/*box.size.xSize = (float)sqrt((double)out[1].x*out[1].x + (double)out[1].y*out[1].y);
box.size.ySize = (float)sqrt((double)out[2].x*out[2].x + (double)out[2].y*out[2].y);
box.angle = (float)atan2( (double)out[1].y, (double)out[1].x );*/
//xSize always the long!
float norm1 = (float)sqrt((double)out[1].x*out[1].x + (double)out[1].y*out[1].y);
float norm2 = (float)sqrt((double)out[2].x*out[2].x + (double)out[2].y*out[2].y);
if(norm1 > norm2)
{
box.size.xSize = norm1;
box.size.ySize = norm2;
//box.angle = (float)atan2( (double)out[1].y, (double)out[1].x );//(float)atan(out[1].y/out[1].x);//
}
else
{
box.size.xSize = norm2;
box.size.ySize = norm1;
//box.angle = (float)atan2( (double)out[2].y, (double)out[2].x ); //(float)atan(out[2].y/out[2].x);//
}
} //haven't tested the below cases!
else if( n == 2 )
{
box.center.x = ( cloud_hull->points[0].x + cloud_hull->points[1].x)*0.5f;
box.center.y = ( cloud_hull->points[0].y + cloud_hull->points[1].y)*0.5f;
double dx = cloud_hull->points[1].x - cloud_hull->points[0].x;
double dy = cloud_hull->points[1].y - cloud_hull->points[0].y;
box.size.xSize = (float)sqrt(dx*dx + dy*dy);
box.size.ySize = 0;
//box.angle = (float)atan2( dy, dx );
}
else
{
if( n == 1 )
box.center.x = cloud_hull->points[0].x;
box.center.y = cloud_hull->points[0].y;
//box.angle = 0;
}
//box.fillQuatGivenAxisAngle();
return box;
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>),
cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>),
cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader reader;
reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
// Build a filter to remove spurious NaNs
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 1.1);
pass.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering has: "
<< cloud_filtered->points.size () << " data points." << std::endl;
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
std::cerr << "PointCloud after segmentation has: "
<< inliers->indices.size () << " inliers." << std::endl;
// Project the model inliers
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setIndices (inliers);
proj.setInputCloud (cloud_filtered);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
std::cerr << "PointCloud after projection has: "
<< cloud_projected->points.size () << " data points." << std::endl;
// Create a Concave Hull representation of the projected inliers
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConcaveHull<pcl::PointXYZ> chull;
chull.setInputCloud (cloud_projected);
chull.setAlpha (0.1);
chull.reconstruct (*cloud_hull);
std::cerr << "Concave hull has: " << cloud_hull->points.size ()
<< " data points." << std::endl;
pcl::PCDWriter writer;
writer.write ("table_scene_mug_stereo_textured_hull.pcd", *cloud_hull, false);
return (0);
}
void DynModelExporter2::createMarkerForConvexHull(pcl::PointCloud<pcl::PointXYZ>& plane_cloud, pcl::ModelCoefficients::Ptr& plane_coefficients, visualization_msgs::Marker& marker)
{
// init marker
marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
marker.action = visualization_msgs::Marker::ADD;
// project the points of the plane on the plane
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setInputCloud (plane_cloud.makeShared());
proj.setModelCoefficients (plane_coefficients);
proj.filter(*cloud_projected);
// create the convex hull in the plane
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::ConvexHull<pcl::PointXYZ > chull;
chull.setInputCloud (cloud_projected);
chull.reconstruct(*cloud_hull);
// work around known bug in ROS Diamondback perception_pcl: convex hull is centered around centroid of input cloud (fixed in pcl svn revision 443)
// thus: we shift the mean of cloud_hull to the mean of cloud_projected (fill dx, dy, dz and apply when creating the marker points)
Eigen::Vector4f meanPointCH, meanPointCP;
pcl::compute3DCentroid(*cloud_projected, meanPointCP);
pcl::compute3DCentroid(*cloud_hull, meanPointCH);
//float dx = 0;//meanPointCP[0]-meanPointCH[0];
//float dy = 0;//meanPointCP[1]-meanPointCH[1];
//float dz = 0;//meanPointCP[2]-meanPointCH[2];
// create colored part of plane by creating marker for each triangle between neighbored points on contour of convex hull an midpoint
marker.points.clear();
for (unsigned int j = 0; j < cloud_hull->points.size(); ++j)
{
geometry_msgs::Point p;
p.x = cloud_hull->points[j].x; p.y = cloud_hull->points[j].y; p.z = cloud_hull->points[j].z;
marker.points.push_back( p );
p.x = cloud_hull->points[(j+1)%cloud_hull->points.size() ].x; p.y = cloud_hull->points[(j+1)%cloud_hull->points.size()].y; p.z = cloud_hull->points[(j+1)%cloud_hull->points.size()].z;
marker.points.push_back( p );
p.x = meanPointCP[0]; p.y = meanPointCP[1]; p.z = meanPointCP[2];
marker.points.push_back( p );
}
// scale of the marker
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 1;
}
visualization_msgs::Marker PlaneExt::AddPlanePoints(pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud)
{
// Add new plane hull
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>());
tVertices polygons = ComputeConcaveHull(plane_cloud, cloud_hull);
// Join with current
ConcaveHullJoinCurrent(cloud_hull, polygons);
// Triangulate
TriangulatePlanePolygon();
return planeTriangles;
}
visualization_msgs::Marker PlaneExt::NewPlanePoints(pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud)
{
// Init a new plane hull (previous will be deleted
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>());
tVertices polygons = ComputeConcaveHull(plane_cloud, cloud_hull);
// revrite hull
ConcaveHullRewrite(cloud_hull, polygons);
// triangulate
TriangulatePlanePolygon();
return planeTriangles;
}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr calc_convex_hull(pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud, pcl::PointIndices::Ptr inliers, pcl::ModelCoefficients::Ptr coefficients){
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGBA>),
cloud_projected (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::ConvexHull<pcl::PointXYZRGBA> chull;
// Project the model inliers
pcl::ProjectInliers<pcl::PointXYZRGBA> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setIndices (inliers);
proj.setInputCloud (cloud);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
// Create a Convex Hull representation of the projected inliers
chull.setInputCloud (cloud_projected);
//chull.setAlpha (0.1);
chull.reconstruct (*cloud_hull);
return cloud_hull;
}
//extract table clouds, convex hull, and convert to msg stored in DB
bool extract_table_msg(pcl_cloud::Ptr cloud_in, bool is_whole, bool store_cloud=false, bool store_convex=false)
{
pcl_cloud::Ptr cloud1(new pcl_cloud());
pcl_cloud::Ptr cloud_out(new pcl_cloud());
//filter out range that may not be a table plane
pcl::PassThrough<Point> pass;
pass.setFilterFieldName("z");
pass.setFilterLimits(0.45,1.2);
pass.setInputCloud(cloud_in);
pass.filter(*cloud1);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::SACSegmentation<Point> seg;
seg.setOptimizeCoefficients (true);
//plane model
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud1);
seg.segment (*inliers, *coefficients);
if(inliers->indices.size () == 0){
ROS_INFO("No plane detected!");
return false;
}
//form a new cloud from indices
pcl::ExtractIndices<Point> extract;
extract.setInputCloud (cloud1);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_out);
//filter out noise
pcl_cloud::Ptr cloud_nonoise(new pcl_cloud());
outlier_filter_radius(cloud_out,cloud_nonoise);
/*
//normal estimation and filter table plane
pcl::NormalEstimationOMP<Point, pcl::Normal> ne;
ne.setInputCloud (cloud_nonoise);
pcl::search::KdTree<Point>::Ptr tree (new pcl::search::KdTree<Point> ());
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
pcl::Normal nor;
float size = 0.0;
for(int i=0;i<cloud_normals->height*cloud_normals->width;i++){
if(isnan(cloud_normals->at(i).normal_x)){
//std::cout<<cloud_normals->at(i).normal_x<<std::endl;
}
else{
nor.normal_x += cloud_normals->at(i).normal_x;
nor.normal_y += cloud_normals->at(i).normal_y;
nor.normal_z += cloud_normals->at(i).normal_z;
size++;
}
}
//normalize instead of average
float normal_length = sqrt(nor.normal_x*nor.normal_x+nor.normal_y*nor.normal_y+nor.normal_z*nor.normal_z);
nor.normal_x = nor.normal_x/normal_length;
nor.normal_y = nor.normal_y/normal_length;
nor.normal_z = nor.normal_z/normal_length;
*/
//instead of computer normal again, using it from plane model
pcl::Normal nor;
nor.normal_x = coefficients->values[0];
nor.normal_y = coefficients->values[1];
nor.normal_z = coefficients->values[2];
if(Debug){
std::cout<<nor.normal_x<<std::endl;
std::cout<<nor.normal_y<<std::endl;
std::cout<<nor.normal_z<<std::endl;
}
//the default viewpoint is (0,0,0),thus using global cloud some plane and viewpoint are in a same plane,
//which may not helpful to flip normal direction
pcl::Normal standard_normal;
standard_normal.normal_x = 0.0;
standard_normal.normal_y = 0.0;
standard_normal.normal_z = 1.0;
float cosin_angle = (standard_normal.normal_z*nor.normal_z);
float tmp_angle = acos(cosin_angle) * (180.0/PI);
//if(tmp_angle>90.0?(tmp_angle-90.0)<normal_angle:tmp_angle<normal_angle){
if(tmp_angle-90.0>0.0){
if((180.0 - tmp_angle)<normal_angle){
ROS_INFO("Normal direction meet requirement %f, angle calculated is: 180.0-%f=%f. ", normal_angle, tmp_angle, 180.0-tmp_angle);
}
else{
ROS_INFO("Normal direction exceed threshold %f, angle calculated is: 180.0-%f=%f. skip.", normal_angle, tmp_angle, 180.0-tmp_angle);
return false;
}
}
else{
if(tmp_angle<normal_angle) {
ROS_INFO("Normal direction meet requirement angle %f, angle calculated is %f", normal_angle, tmp_angle);
}
else{
ROS_INFO("Normal direction exceed threshold %f, angle calculated is: %f. skip.", normal_angle, tmp_angle);
return false;
}
}
pcl_cloud::Ptr cloud_hull(new pcl_cloud());
pcl_cloud::Ptr cloud_cave(new pcl_cloud());
extract_convex(cloud1,inliers,coefficients,cloud_hull,cloud_cave);
//reject some size convex
//store the plane that pass the table filter
if(store_cloud) {
ROS_INFO("Storing table cloud (noise & filted)...");
if (is_whole) {
mongodb_store::MessageStoreProxy table_whole_cloud_noise(*nh, "whole_table_clouds_noise");
mongodb_store::MessageStoreProxy table_whole_cloud(*nh, "whole_table_clouds");
//conversion
sensor_msgs::PointCloud2 whole_table_cloud_noise;
pcl::toROSMsg(*cloud_out, whole_table_cloud_noise);
table_whole_cloud_noise.insert(whole_table_cloud_noise);
sensor_msgs::PointCloud2 whole_table_cloud;
pcl::toROSMsg(*cloud_nonoise, whole_table_cloud);
table_whole_cloud.insert(whole_table_cloud);
}
else {
mongodb_store::MessageStoreProxy dbtable_cloud_noise(*nh, "table_clouds_noise");
mongodb_store::MessageStoreProxy dbtable_cloud(*nh, "table_clouds");
//conversion
sensor_msgs::PointCloud2 table_cloud_noise;
pcl::toROSMsg(*cloud_out, table_cloud_noise);
dbtable_cloud_noise.insert(table_cloud_noise);
sensor_msgs::PointCloud2 table_cloud;
pcl::toROSMsg(*cloud_nonoise, table_cloud);
dbtable_cloud.insert(table_cloud);
//store table centre for each scan
ROS_INFO("Storing table centre...");
Table<Point> tb(nh);
tb.table_cloud_centre(table_cloud);
}
}
ROS_INFO("Convex cloud has been extracted contains %lu points", (*cloud_hull).points.size());
if(store_convex){
ROS_INFO("Storing convex cloud...");
if(is_whole){
mongodb_store::MessageStoreProxy dbtable_whole_convex_cloud(*nh, "whole_table_convex");
mongodb_store::MessageStoreProxy dbtable_whole_concave_cloud(*nh, "whole_table_concave");
//conversion
sensor_msgs::PointCloud2 whole_table_convex;
pcl::toROSMsg(*cloud_hull, whole_table_convex);
dbtable_whole_convex_cloud.insert(whole_table_convex);
sensor_msgs::PointCloud2 whole_table_concave;
pcl::toROSMsg(*cloud_cave, whole_table_concave);
dbtable_whole_concave_cloud.insert(whole_table_concave);
}
else{
mongodb_store::MessageStoreProxy dbtable_convex_cloud(*nh, "table_convex");
mongodb_store::MessageStoreProxy dbtable_concave_cloud(*nh, "table_concave");
//conversion
sensor_msgs::PointCloud2 table_convex;
pcl::toROSMsg(*cloud_hull, table_convex);
dbtable_convex_cloud.insert(table_convex);
sensor_msgs::PointCloud2 table_concave;
pcl::toROSMsg(*cloud_cave, table_concave);
dbtable_concave_cloud.insert(table_concave);
}
}
//construct a table msg
table_detection::Table table_msg;
table_msg.pose.pose.orientation.w = 1.0;
table_msg.header.frame_id = "/map";
table_msg.header.stamp = ros::Time();
for(int i=0;i<(*cloud_hull).points.size();i++){
geometry_msgs::Point32 pp;
pp.x = (*cloud_hull).at(i).x;
pp.y = (*cloud_hull).at(i).y;
pp.z = (*cloud_hull).at(i).z;
table_msg.tabletop.points.push_back(pp);
}
//insert to mongodb
if(is_whole){
mongodb_store::MessageStoreProxy whole_table_shape(*nh, "whole_table_shapes");
whole_table_shape.insert(table_msg);
ROS_INFO("Insert whole table shape to collection.");
}
else{
mongodb_store::MessageStoreProxy table_shape(*nh, "table_shapes");
table_shape.insert(table_msg);
ROS_INFO("Insert table shape to collection.");
}
return true;
}
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;
}
void FilterPlanes::filterPlanes(vector<BasicPlane> &plane_stack){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>);
if (verbose_text>0){
cout << "PointCloud before filtering: " << cloud->width * cloud->height << " data points." << std::endl;
}
#if DO_TIMING_PROFILE
vector<int64_t> tic_toc;
tic_toc.push_back(bot_timestamp_now());
#endif
/* Ptcoll_cfg ptcoll_cfg;
ptcoll_cfg.point_lists_id =pose_element_id; //bot_timestamp_now();
ptcoll_cfg.collection = pose_coll_id;
ptcoll_cfg.element_id = pose_element_id; */
//1. Downsample the dataset using a leaf size of 1cm
// this is 99% of the cpu time
pcl::VoxelGrid<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud);
// for table dataset: sor.setLeafSize (0.01, 0.01, 0.01);
sor.setLeafSize (0.05, 0.05, 0.05);
// sor.setLeafSize (0.1, 0.1, 0.1);
sor.filter (*cloud_filtered);
if (verbose_text>0){
cout << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
}
#if DO_TIMING_PROFILE
tic_toc.push_back(bot_timestamp_now());
#endif
if (verbose_lcm>2){
/* ptcoll_cfg.id = 200;
ptcoll_cfg.reset=true;
ptcoll_cfg.name ="cloud_downsampled";
ptcoll_cfg.npoints = cloud_filtered->points.size();
float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0};
ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
ptcoll_cfg.type =1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_filtered);*/
}
// 2. Set up the Ransac Plane Fitting Object:
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100); // was 4000
seg.setDistanceThreshold (distance_threshold_); // 0.01 for table data set
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
#if DO_TIMING_PROFILE
tic_toc.push_back(bot_timestamp_now());
#endif
int n_major = 0, nr_points = cloud_filtered->points.size ();
//vector<BasicPlaneX> plane_stack;
BasicPlane one_plane;
// Extract the primary planes:
// major planes are the coarse planes extracted via ransac. they might contain disconnected points
// these are broken up into minor planes which are portions of major planes
if(verbose_lcm > 2){
for (int i=0;i<7;i++){
char n_major_char[10];
sprintf(n_major_char,"%d",i);
/* ptcoll_cfg.id =210+ i+3;
ptcoll_cfg.reset=true;
ptcoll_cfg.name =(char*) "cloud_p ";
ptcoll_cfg.name.append(n_major_char);
ptcoll_cfg.npoints = 0;
float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0};
ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
ptcoll_cfg.type=1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_filtered);
*/
}
for (int i=0;i<7;i++){
/*
Ptcoll_cfg ptcoll_cfg2;
// the i below accounts for super planes in the same utime
ptcoll_cfg2.point_lists_id =pose_element_id; //bot_timestamp_now();
ptcoll_cfg2.collection = pose_coll_id;
ptcoll_cfg2.element_id = pose_element_id;
if (i==0){
ptcoll_cfg2.reset=true;
}else{
ptcoll_cfg2.reset=false;
}
ptcoll_cfg2.id =500;
ptcoll_cfg2.name.assign("Grown Stack Final");
ptcoll_cfg2.npoints = 0;
ptcoll_cfg2.type =1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, *cloud_filtered);
*/
}
// TODO: this will rest this object. need to add publish of reset
// at start of this block and set rese ttofal
for (int i=0;i<7;i++){
/*
Ptcoll_cfg ptcoll_cfg2;
// the i below accounts for super planes in the same utime
ptcoll_cfg2.point_lists_id =pose_element_id; //bot_timestamp_now();
ptcoll_cfg2.collection = pose_coll_id;
ptcoll_cfg2.element_id = pose_element_id;
if (i==0){
ptcoll_cfg2.reset=true;
}else{
ptcoll_cfg2.reset=false;
}
ptcoll_cfg2.id =501;
ptcoll_cfg2.name.assign("Grown Stack Final Hull");
ptcoll_cfg2.npoints = 0;
ptcoll_cfg2.type =3;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, *cloud_filtered);
*/
}
}
#if DO_TIMING_PROFILE
tic_toc.push_back(bot_timestamp_now());
#endif
while (cloud_filtered->points.size () > stop_proportion_ * nr_points) {
// While XX% of the original cloud is still there
char n_major_char[10];
sprintf(n_major_char,"%d",n_major);
if (n_major >6) {
if (verbose_text >0){
std::cout << n_major << " is the max number of planes to find, quitting" << std::endl;
}
break;
}
//a Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
if (inliers->indices.size () < stop_cloud_size_) // stop when the plane is only a few points
{
//std::cerr << "No remaining planes in this set" << std::endl;
break;
}
//b Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
if (verbose_text>0){
std::cout << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
}
// Create the filtering object
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud_p);
sor.setMeanK (30);
sor.setStddevMulThresh (0.5); //was 1.0
sor.filter (*cloud_p);
if(verbose_lcm > 2){
/*
ptcoll_cfg.id =210+ n_major+3;
ptcoll_cfg.reset=true;
ptcoll_cfg.name ="cloud_p ";
ptcoll_cfg.name.append(n_major_char);
ptcoll_cfg.npoints = cloud_p->points.size();
float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0};
ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
ptcoll_cfg.type=1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_p);
*/
}
vector<BasicPlane> grown_stack;
vector<BasicPlane> * grown_stack_ptr;
grown_stack_ptr = &grown_stack;
GrowCloud grow;
grow.setInputCloud(cloud_p);
grow.setMinCloudSize(stop_cloud_size_); // useing stop cloud size here too
grow.setLCM(publish_lcm);
grow.doGrowCloud(*grown_stack_ptr);
if (verbose_text>0){
cout << "grow_cloud new found " << grown_stack.size() << " seperate clouds\n";
}
// Spit raw clouds out to LCM:
if(verbose_lcm > 2){
for (int i=0;i<grown_stack.size();i++){
/*
Ptcoll_cfg ptcoll_cfg2;
ptcoll_cfg2.reset=false;
// the i below accounts for super planes in the same utime
ptcoll_cfg2.point_lists_id =10*n_major + i; //filterplanes->pose_element_id;
ptcoll_cfg2.collection = pose_coll_id;
ptcoll_cfg2.element_id = pose_element_id;
ptcoll_cfg2.id =500;
ptcoll_cfg2.name.assign("Grown Stack Final");
ptcoll_cfg2.npoints = grown_stack[i].cloud.points.size ();
ptcoll_cfg2.type =1;
int id = plane_stack.size() + i; // 10*n_major + i;
float colorm_temp0[] ={colors[3*(id%num_colors)],colors[3*(id%num_colors)+1],colors[3*(id%num_colors)+2] ,0.0};
ptcoll_cfg2.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, grown_stack[i].cloud);
*/
}
}
BasicPlane one_plane;
int n_minor=0;
BOOST_FOREACH( BasicPlane one_plane, grown_stack ){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGB>);
// c Project the model inliers (seems to be necessary to fitting convex hull
pcl::ProjectInliers<pcl::PointXYZRGB> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp (new pcl::PointCloud<pcl::PointXYZRGB> ());
*temp = (one_plane.cloud);
proj.setInputCloud (temp);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
std::vector <pcl::Vertices> vertices;
if (1==1){ // convex:
pcl::ConvexHull<pcl::PointXYZRGB> chull;
chull.setInputCloud (cloud_projected);
chull.setDimension(2);
chull.reconstruct (*cloud_hull,vertices);
}else { // concave
pcl::ConcaveHull<pcl::PointXYZRGB> chull;
chull.setInputCloud (cloud_projected);
chull.setKeepInformation(true);
chull.setAlpha(0.5);
// for arch way:
// 1.1 too few
// 0.7 a little to few but much better
chull.reconstruct (*cloud_hull,vertices);
}
//std::cout << "Hull has: " << cloud_hull->points.size () << " vertices." << std::endl;
if (cloud_hull->points.size () ==0){
cout <<"ERROR: CONVEX HULL HAS NO POINTS! - NEED TO RESOLVE THIS\n";
}
// d.2 Find the mean colour of the hull:
int rgba[]={0,0,0,0};
for(int i=0;i<temp->points.size();i++){
int rgba_one = *reinterpret_cast<int*>(&temp->points[i].rgba);
rgba[3] += ((rgba_one >> 24) & 0xff);
rgba[2] += ((rgba_one >> 16) & 0xff);
rgba[1] += ((rgba_one >> 8) & 0xff);
rgba[0] += (rgba_one & 0xff);
}
double scale = ((double) temp->points.size());
rgba[3] =(int) round(((double) rgba[3]) / scale);
rgba[2] =(int) round(((double) rgba[2]) / scale);
rgba[1] =(int) round(((double) rgba[1]) / scale);
rgba[0] =(int) round(((double) rgba[0]) / scale);
// Write the plane to file for experimentation:
//stringstream oss;
//oss << ptcoll_cfg.element_id <<"_" << ptcoll_cfg.name << ".pcd";
//writer.write (oss.str(), *this_hull, false);
for(int i=0;i<cloud_hull->points.size();i++){
unsigned char* rgba_ptr = (unsigned char*)&cloud_hull->points[i].rgba;
(*rgba_ptr) = rgba[0] ;
(*(rgba_ptr+1)) = rgba[1];
(*(rgba_ptr+2)) = rgba[2];
(*(rgba_ptr+3)) = rgba[3];
}
(one_plane.coeffs) = *coefficients;
one_plane.cloud = *cloud_hull;
one_plane.utime = pose_element_id;
one_plane.major = n_major;
one_plane.minor = n_minor;
one_plane.n_source_points = cloud_projected->points.size();
plane_stack.push_back(one_plane);
n_minor++;
// int stop;
// cout << "int to cont: ";
// cin >> stop;
}
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_filtered);
n_major++;
}
int
main (int argc, char** argv)
{
// ros::init(argc, argv, "extract_sec");
// ros::NodeHandle node_handle;
// pcl::visualization::PCLVisualizer result_viewer("planar_segmentation");
boost::shared_ptr<pcl::visualization::PCLVisualizer> result_viewer (new pcl::visualization::PCLVisualizer ("planar_segmentation"));
result_viewer->addCoordinateSystem(0.3, "reference", 0);
result_viewer->setCameraPosition(-0.499437, 0.111597, -0.758007, -0.443141, 0.0788583, -0.502855, -0.034703, -0.992209, -0.119654);
result_viewer->setCameraClipDistances(0.739005, 2.81526);
// result_viewer->setCameraPosition(Position, Focal point, View up);
// result_viewer->setCameraClipDistances(Clipping plane);
/***************************************
* parse arguments
***************************************/
if(argc<5)
{
pcl::console::print_info("Usage: extract_sec DATA_PATH/PCD_FILE_FORMAT START_INDEX END_INDEX DEMO_NAME (opt)STEP_SIZE(1)");
exit(1);
}
int view_id=0;
int step=1;
std::string basename_cloud=argv[1];
unsigned int index_start = std::atoi(argv[2]);
unsigned int index_end = std::atoi(argv[3]);
std::string demo_name=argv[4];
if(argc>5) step=std::atoi(argv[5]);
/***************************************
* set up result directory
***************************************/
mkdir("/home/zhen/Documents/Dataset/human_result", S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
char result_folder[50];
std::snprintf(result_folder, sizeof(result_folder), "/home/zhen/Documents/Dataset/human_result/%s", demo_name.c_str());
mkdir(result_folder, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
std::string basename_pcd = (basename_cloud.find(".pcd") == std::string::npos) ? (basename_cloud + ".pcd") : basename_cloud;
std::string filename_pcd;
std::string mainGraph_file;
mainGraph_file = std::string(result_folder) + "/mainGraph.txt";
// write video config
char video_file[100];
std::snprintf(video_file, sizeof(video_file), "%s/video.txt", result_folder);
std::ofstream video_config(video_file);
if (video_config.is_open())
{
video_config << index_start << " " << index_end << " " << demo_name << " " << step;
video_config.close();
}
/***************************************
* set up cloud, segmentation, tracker, detectors, graph, features
***************************************/
TableObject::Segmentation tableObjSeg;
TableObject::Segmentation initialSeg;
TableObject::track3D tracker(false);
TableObject::colorDetector finger1Detector(0,100,0,100,100,200); //right
TableObject::colorDetector finger2Detector(150,250,0,100,0,100); //left
TableObject::touchDetector touchDetector(0.01);
TableObject::bottleDetector bottleDetector;
TableObject::mainGraph mainGraph((int)index_start);
std::vector<manipulation_features> record_features;
manipulation_features cur_features;
TableObject::pcdCloud pcdSceneCloud;
CloudPtr sceneCloud;
CloudPtr planeCloud(new Cloud);
CloudPtr cloud_objects(new Cloud);
CloudPtr cloud_finger1(new Cloud);
CloudPtr cloud_finger2(new Cloud);
CloudPtr cloud_hull(new Cloud);
CloudPtr track_target(new Cloud);
CloudPtr tracked_cloud(new Cloud);
std::vector<pcl::PointIndices> clusters;
pcl::ModelCoefficients coefficients;
pcl::PointIndices f1_indices;
pcl::PointIndices f2_indices;
Eigen::Affine3f toBottleCoordinate;
Eigen::Affine3f transformation;
Eigen::Vector3f bottle_init_ori;
// set threshold of size of clustered cloud
tableObjSeg.setThreshold(30);
initialSeg.setThreshold(500);
// downsampler
pcl::ApproximateVoxelGrid<pcl::PointXYZRGBA> grid;
float leaf_size=0.005;
grid.setLeafSize (leaf_size, leaf_size, leaf_size);
/***************************************
* start processing
***************************************/
unsigned int idx = index_start;
int video_id=0;
bool change = false;
while( idx <= index_end && !result_viewer->wasStopped())
{
std::cout << std::endl;
std::cout << "frame id=" << idx << std::endl;
filename_pcd = cv::format(basename_cloud.c_str(), idx);
if(idx==index_start) {
/***************************************
* Intialization:
* -plane localization
* -object cluster extraction
* -bottle cluster localization
***************************************/
initialSeg.resetCloud(filename_pcd, false);
initialSeg.seg(false);
initialSeg.getObjects(cloud_objects, clusters);
initialSeg.getCloudHull(cloud_hull);
initialSeg.getPlaneCoefficients(coefficients);
initialSeg.getsceneCloud(pcdSceneCloud);
initialSeg.getTableTopCloud(planeCloud);
sceneCloud=pcdSceneCloud.getCloud();
/***************************************
* fingertip, hand_arm removal
***************************************/
//opencv color filtering for fingertip_1
{
pcl::ScopeTime t_finger1("Finger 1(blue) detection");
finger1Detector.setInputCloud(cloud_objects, clusters);
finger1Detector.filter(f1_indices,cloud_finger1);
}
finger1Detector.showDetectedCloud(result_viewer, "finger1");
//opencv color filtering for fingertip_2
{
pcl::ScopeTime t_finger2("Finger 2(orange) detection");
finger2Detector.setInputCloud(cloud_objects, clusters);
finger2Detector.filter(f2_indices,cloud_finger2);
}
finger2Detector.showDetectedCloud(result_viewer, "finger2");
// remove hand (include cluster that contains the detected fingertips and also the other clusters that are touching the cluster)
std::vector<int> hand_arm1=TableObject::findHand(cloud_objects, clusters, f1_indices);
for(int i=hand_arm1.size()-1; i>=0; i--)
{
clusters.erase(clusters.begin()+hand_arm1[i]);
std::cout << "removing hand_arm : cluster index = " << hand_arm1[i] << std::endl;
}
std::vector<int> hand_arm2=TableObject::findHand(cloud_objects, clusters, f2_indices);
for(int i=hand_arm2.size()-1; i>=0; i--)
{
clusters.erase(clusters.begin()+hand_arm2[i]);
std::cout << "removing hand_arm : cluster index = " << hand_arm2[i] << std::endl;
}
// DEBUG
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> plane(planeCloud);
// result_viewer->addPointCloud<RefPointType>(planeCloud, plane, "tabletop");
// CloudPtr debug(new Cloud);
// initialSeg.getOutPlaneCloud(debug);
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> out_plane(debug);
// result_viewer->addPointCloud<RefPointType>(debug, out_plane, "out_plane");
// choose bottle_id at 1st frame & confirm fitted model is correct
TableObject::view3D::drawClusters(result_viewer, cloud_objects, clusters, true);
while (!result_viewer->wasStopped ())
{
result_viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
std::cout << "cluster size = " << clusters.size() << std::endl;
/***************************************
* Localizing cylinder
***************************************/
CloudPtr cluster_bottle (new Cloud);
int bottle_id = 0;
pcl::copyPointCloud (*cloud_objects, clusters[bottle_id], *cluster_bottle);
bottleDetector.setInputCloud(cluster_bottle);
bottleDetector.fit();
bottleDetector.getTransformation(toBottleCoordinate);
bottle_init_ori= bottleDetector.getOrientation();
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(toBottleCoordinate.inverse(), x, y, z, roll, pitch, yaw);
result_viewer->removeCoordinateSystem("reference", 0);
result_viewer->addCoordinateSystem(0.3, toBottleCoordinate.inverse(), "reference", 0);
bottleDetector.drawOrientation(result_viewer);
/***************************************
* Record features
***************************************/
bottle_features cur_bottle_features;
cur_bottle_features.loc[0] = x;
cur_bottle_features.loc[1] = y;
cur_bottle_features.loc[2] = z;
cur_bottle_features.ori[0] = roll;
cur_bottle_features.ori[1] = pitch;
cur_bottle_features.ori[2] = yaw;
cur_bottle_features.color[0] = bottleDetector.getCenter().r;
cur_bottle_features.color[1] = bottleDetector.getCenter().g;
cur_bottle_features.color[2] = bottleDetector.getCenter().b;
cur_bottle_features.size[0] = bottleDetector.getHeight();
cur_bottle_features.size[1] = bottleDetector.getRadius();
cur_features.bottle = cur_bottle_features;
pcl::PointXYZ center_finger1 = TableObject::computeObjCentroid(cloud_finger1);
pcl::PointXYZ center_finger2 = TableObject::computeObjCentroid(cloud_finger2);
center_finger1 = pcl::transformPoint<pcl::PointXYZ>(center_finger1, toBottleCoordinate);
center_finger2 = pcl::transformPoint<pcl::PointXYZ>(center_finger2, toBottleCoordinate);
cur_features.gripper_1.loc[0] = center_finger1.x;
cur_features.gripper_1.loc[1] = center_finger1.y;
cur_features.gripper_1.loc[2] = center_finger1.z;
cur_features.gripper_2.loc[0] = center_finger2.x;
cur_features.gripper_2.loc[1] = center_finger2.y;
cur_features.gripper_2.loc[2] = center_finger2.z;
record_features.push_back(cur_features);
/***************************************
* Tracking initialization
***************************************/
{
pcl::ScopeTime t_track("Tracker initialization");
tracker.setTarget(cluster_bottle, bottleDetector.getCenter());
tracker.initialize();
}
/***************************************
* Touch detection
***************************************/
std::vector<CloudPtr> touch_clouds;
touch_clouds.push_back(cluster_bottle);
touch_clouds.push_back(cloud_finger1);
touch_clouds.push_back(cloud_finger2);
// touch detection between each pair of objects (including fingertips, tabletop objects and tabletop)
for(int i=0; i<touch_clouds.size(); i++)
{
int j;
bool touch;
for(j=i+1; j<touch_clouds.size(); j++)
{
// touch detection between object_i and object_j
char relation [50];
std::sprintf(relation, "object%d_object%d", i, j);
std::cout << relation << std::endl;
{
pcl::ScopeTime t("Touch detection");
touch=touchDetector.detect(touch_clouds[i], touch_clouds[j]);
}
// touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i);
// relational scene graph -> main graph
if(touch) {
mainGraph.addInitialRelationalGraph(2);
} else {
mainGraph.addInitialRelationalGraph(0);
}
}
// touch detection between each objects and tabletop
char relation [50];
std::sprintf (relation, "object%d_object%d", i, (int)touch_clouds.size());
std::cout << relation << std::endl;
{
pcl::ScopeTime t("Touch detection");
touch=touchDetector.detectTableTouch(touch_clouds[i], coefficients);
}
// touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i);
// relational scene graph -> main graph
if(touch) {
mainGraph.addInitialRelationalGraph(2);
} else {
mainGraph.addInitialRelationalGraph(0);
}
}
/***************************************
* Visualization
***************************************/
// draw extracted object clusters
// TableObject::view3D::drawClusters(result_viewer, cloud_objects, touch_clusters);
// draw extracted plane points
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> plane(planeCloud);
// result_viewer->addPointCloud<RefPointType>(planeCloud, plane, "tabletop");
// std::stringstream ss;
// ss << (int)touch_clusters.size();
// result_viewer->addText3D(ss.str(), planeCloud->points.at(334*640+78),0.1);
// draw extracted plane contour polygon
result_viewer->addPolygon<RefPointType>(cloud_hull, 0, 255, 0, "polygon");
change = true;
} else
{
/***************************************
* object cloud extraction
***************************************/
tableObjSeg.resetCloud(filename_pcd, false);
tableObjSeg.seg(cloud_hull,false);
tableObjSeg.getObjects(cloud_objects, clusters);
tableObjSeg.getsceneCloud(pcdSceneCloud);
sceneCloud=pcdSceneCloud.getCloud();
/***************************************
* fingertip extraction
***************************************/
//opencv color filtering for fingertip_1
{
pcl::ScopeTime t_finger1("Finger 1(blue) detection");
finger1Detector.setInputCloud(cloud_objects, clusters);
finger1Detector.filter(f1_indices,cloud_finger1);
}
finger1Detector.showDetectedCloud(result_viewer, "finger1");
//opencv color filtering for fingertip_2
{
pcl::ScopeTime t_finger1("Finger 2(orange) detection");
finger2Detector.setInputCloud(cloud_objects, clusters);
finger2Detector.filter(f2_indices,cloud_finger2);
}
finger2Detector.showDetectedCloud(result_viewer, "finger2");
/***************************************
* filter out black glove cluster & gray sleeve, also update cloud_objects with removed cluster indices
***************************************/
for(int i=0; i<clusters.size(); i++)
{
pcl::CentroidPoint<RefPointType> color_points;
for(int j=0; j<clusters[i].indices.size(); j++)
{
color_points.add(cloud_objects->at(clusters[i].indices[j]));
}
RefPointType mean_color;
color_points.get(mean_color);
if(mean_color.r>30 & mean_color.r<70 & mean_color.g>30 & mean_color.g<70 & mean_color.b>30 & mean_color.b<70)
{
clusters.erase(clusters.begin()+ i);
i=i-1;
}
}
/***************************************
* Tracking objects
***************************************/
{
pcl::ScopeTime t_track("Tracking");
grid.setInputCloud (sceneCloud);
grid.filter (*track_target);
tracker.track(track_target, transformation);
tracker.getTrackedCloud(tracked_cloud);
}
tracker.viewTrackedCloud(result_viewer);
// tracker.drawParticles(result_viewer);
/***************************************
* compute tracked <center, orientation>
***************************************/
pcl::PointXYZ bottle_loc_point(0,0,0);
bottle_loc_point = pcl::transformPoint<pcl::PointXYZ>(bottle_loc_point, transformation);
result_viewer->removeShape("bottle_center");
// result_viewer->addSphere<pcl::PointXYZ>(bottle_loc_point, 0.05, "bottle_center");
Eigen::Vector3f bottle_ori;
pcl::transformVector(bottle_init_ori,bottle_ori,transformation);
TableObject::view3D::drawArrow(result_viewer, bottle_loc_point, bottle_ori, "bottle_arrow");
/***************************************
* calculate toTrackedBottleCoordinate
***************************************/
Eigen::Affine3f toTrackedBottleCoordinate;
Eigen::Vector3f p( bottle_loc_point.x, bottle_loc_point.y, bottle_loc_point.z ); // position
// get a vector that is orthogonal to _orientation ( yc = _orientation x [1,0,0]' )
Eigen::Vector3f yc( 0, bottle_ori[2], -bottle_ori[1] );
yc.normalize();
// get a transform that rotates _orientation into z and moves cloud into origin.
pcl::getTransformationFromTwoUnitVectorsAndOrigin(yc, bottle_ori, p, toTrackedBottleCoordinate);
result_viewer->removeCoordinateSystem("reference");
result_viewer->addCoordinateSystem(0.3, toTrackedBottleCoordinate.inverse(), "reference", 0);
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(toTrackedBottleCoordinate.inverse(), x, y, z, roll, pitch, yaw);
/***************************************
* setup bottle feature
***************************************/
cur_features = record_features[video_id-1];
cur_features.bottle.loc[0] = x;
cur_features.bottle.loc[1] = y;
cur_features.bottle.loc[2] = z;
cur_features.bottle.ori[0] = roll;
cur_features.bottle.ori[1] = pitch;
cur_features.bottle.ori[2] = yaw;
pcl::PointXYZ center_finger1 = TableObject::computeObjCentroid(cloud_finger1);
pcl::PointXYZ center_finger2 = TableObject::computeObjCentroid(cloud_finger2);
center_finger1 = pcl::transformPoint<pcl::PointXYZ>(center_finger1, toTrackedBottleCoordinate);
center_finger2 = pcl::transformPoint<pcl::PointXYZ>(center_finger2, toTrackedBottleCoordinate);
cur_features.gripper_1.loc[0] = center_finger1.x;
cur_features.gripper_1.loc[1] = center_finger1.y;
cur_features.gripper_1.loc[2] = center_finger1.z;
cur_features.gripper_2.loc[0] = center_finger2.x;
cur_features.gripper_2.loc[1] = center_finger2.y;
cur_features.gripper_2.loc[2] = center_finger2.z;
record_features.push_back(cur_features);
/***************************************
* Touch detection
***************************************/
std::vector<CloudPtr> touch_clouds;
touch_clouds.push_back(tracked_cloud);
touch_clouds.push_back(cloud_finger1);
touch_clouds.push_back(cloud_finger2);
// touch detection between each pair of objects (including fingertips, tabletop objects and tabletop)
for(int i=0; i<touch_clouds.size(); i++)
{
int j;
bool touch;
for(j=i+1; j<touch_clouds.size(); j++)
{
// touch detection between object_i and object_j
char relation [50];
std::sprintf(relation, "object%d_object%d", i, j);
std::cout << relation << std::endl;
{
pcl::ScopeTime t("Touch detection");
touch=touchDetector.detect(touch_clouds[i], touch_clouds[j]);
}
// touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i);
// relational scene graph -> main graph
if(touch) {
mainGraph.addRelationalGraph(2);
} else {
mainGraph.addRelationalGraph(0);
}
}
// touch detection between each objects and tabletop
char relation [50];
std::sprintf (relation, "object%d_object%d", i, (int)touch_clouds.size());
std::cout << relation << std::endl;
{
pcl::ScopeTime t("Touch detection");
touch=touchDetector.detectTableTouch(touch_clouds[i], coefficients);
}
// touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i);
// relational scene graph -> main graph
if(touch) {
mainGraph.addRelationalGraph(2);
} else {
mainGraph.addRelationalGraph(0);
}
}
/***************************************
* Visualization
***************************************/
// draw extracted point clusters
// TableObject::view3D::drawText(result_viewer, cloud_objects, touch_clusters);
/***************************************
* Main Graph
***************************************/
change = mainGraph.compareRelationGraph((int)idx);
}
// darw original cloud
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> rgb(sceneCloud);
if(!result_viewer->updatePointCloud<RefPointType>(sceneCloud, rgb, "new frame"))
result_viewer->addPointCloud<RefPointType>(sceneCloud, rgb, "new frame");
result_viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
//debug
std::cout << cur_features.bottle.loc[0] << " " << cur_features.bottle.loc[1] << " " << cur_features.bottle.loc[2]
<< " " << cur_features.bottle.ori[0] << " " << cur_features.bottle.ori[1] << " " << cur_features.bottle.ori[2]
<< " " << cur_features.bottle.color[0] << " " << cur_features.bottle.color[1] << " " << cur_features.bottle.color[2]
<< " " << cur_features.bottle.size[0] << " " << cur_features.bottle.size[1]
<< " " << cur_features.gripper_1.loc[0] << " " << cur_features.gripper_1.loc[1] << " " << cur_features.gripper_1.loc[2]
<< " " << cur_features.gripper_2.loc[0] << " " << cur_features.gripper_2.loc[1] << " " << cur_features.gripper_2.loc[2]
<< std::endl;
if(change)
{
char screenshot[100]; // make sure it's big enough
std::snprintf(screenshot, sizeof(screenshot), "%s/sec_%d.png", result_folder, (int)idx);
std::cout << screenshot << std::endl;
result_viewer->saveScreenshot(screenshot);
//record features
char feature_file[100]; // make sure it's big enough
std::snprintf(feature_file, sizeof(feature_file), "%s/features_original.txt", result_folder);
std::ofstream feature_writer(feature_file, std::ofstream::out | std::ofstream::app);
feature_writer << cur_features.bottle.loc[0] << " " << cur_features.bottle.loc[1] << " " << cur_features.bottle.loc[2]
<< " " << cur_features.bottle.ori[0] << " " << cur_features.bottle.ori[1] << " " << cur_features.bottle.ori[2]
<< " " << cur_features.bottle.color[0] << " " << cur_features.bottle.color[1] << " " << cur_features.bottle.color[2]
<< " " << cur_features.bottle.size[0] << " " << cur_features.bottle.size[1]
<< " " << cur_features.gripper_1.loc[0] << " " << cur_features.gripper_1.loc[1] << " " << cur_features.gripper_1.loc[2]
<< " " << cur_features.gripper_2.loc[0] << " " << cur_features.gripper_2.loc[1] << " " << cur_features.gripper_2.loc[2]
<< std::endl;
feature_writer.close();
std::cout << "features saved at " << feature_file << std::endl;
}
char screenshot[200]; // make sure it's big enough
std::snprintf(screenshot, sizeof(screenshot), "%s/video/sec_%d.png", result_folder, (int)video_id);
std::cout << screenshot << std::endl;
result_viewer->saveScreenshot(screenshot);
idx=idx+step;
video_id=video_id+1;
}
mainGraph.displayMainGraph();
mainGraph.recordMainGraph(mainGraph_file);
while (!result_viewer->wasStopped ())
{
result_viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
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;
}
// input: cloudInput
// input: pointCloudNormals
// output: cloudOutput
// output: pointCloudNormalsFiltered
void extractHandles(PointCloud::Ptr& cloudInput, PointCloud::Ptr& cloudOutput, PointCloudNormal::Ptr& pointCloudNormals, std::vector<int>& handles) {
// PCL objects
//pcl::PassThrough<Point> vgrid_; // Filtering + downsampling object
pcl::VoxelGrid<Point> vgrid_; // Filtering + downsampling object
pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_; // Planar segmentation object
pcl::SACSegmentation<Point> seg_line_; // Planar segmentation object
pcl::ProjectInliers<Point> proj_; // Inlier projection object
pcl::ExtractIndices<Point> extract_; // Extract (too) big tables
pcl::ConvexHull<Point> chull_;
pcl::ExtractPolygonalPrismData<Point> prism_;
pcl::PointCloud<Point> cloud_objects_;
pcl::EuclideanClusterExtraction<Point> cluster_, handle_cluster_;
pcl::PCDWriter writer;
double sac_distance_, normal_distance_weight_;
double eps_angle_, seg_prob_;
int max_iter_;
sac_distance_ = 0.05; //0.02
normal_distance_weight_ = 0.05;
max_iter_ = 500;
eps_angle_ = 30.0; //20.0
seg_prob_ = 0.99;
btVector3 axis(0.0, 0.0, 1.0);
seg_.setModelType(pcl::SACMODEL_NORMAL_PARALLEL_PLANE);
seg_.setMethodType(pcl::SAC_RANSAC);
seg_.setDistanceThreshold(sac_distance_);
seg_.setNormalDistanceWeight(normal_distance_weight_);
seg_.setOptimizeCoefficients(true);
seg_.setAxis(Eigen::Vector3f(fabs(axis.getX()), fabs(axis.getY()), fabs(axis.getZ())));
seg_.setEpsAngle(pcl::deg2rad(eps_angle_));
seg_.setMaxIterations(max_iter_);
seg_.setProbability(seg_prob_);
cluster_.setClusterTolerance(0.03);
cluster_.setMinClusterSize(200);
KdTreePtr clusters_tree_(new KdTree);
clusters_tree_->setEpsilon(1);
cluster_.setSearchMethod(clusters_tree_);
seg_line_.setModelType(pcl::SACMODEL_LINE);
seg_line_.setMethodType(pcl::SAC_RANSAC);
seg_line_.setDistanceThreshold(0.05);
seg_line_.setOptimizeCoefficients(true);
seg_line_.setMaxIterations(max_iter_);
seg_line_.setProbability(seg_prob_);
//filter cloud
double leafSize = 0.001;
pcl::VoxelGrid<Point> sor;
sor.setInputCloud (cloudInput);
sor.setLeafSize (leafSize, leafSize, leafSize);
sor.filter (*cloudOutput);
//sor.setInputCloud (pointCloudNormals);
//sor.filter (*pointCloudNormalsFiltered);
//std::vector<int> tempIndices;
//pcl::removeNaNFromPointCloud(*cloudInput, *cloudOutput, tempIndices);
//pcl::removeNaNFromPointCloud(*pointCloudNormals, *pointCloudNormalsFiltered, tempIndices);
// Segment planes
pcl::OrganizedMultiPlaneSegmentation<Point, PointNormal, pcl::Label> mps;
ROS_INFO("Segmenting planes");
mps.setMinInliers (20000);
mps.setMaximumCurvature(0.02);
mps.setInputNormals (pointCloudNormals);
mps.setInputCloud (cloudInput);
std::vector<pcl::PlanarRegion<Point> > regions;
std::vector<pcl::PointIndices> regionPoints;
std::vector< pcl::ModelCoefficients > planes_coeff;
mps.segment(planes_coeff, regionPoints);
ROS_INFO_STREAM("Number of regions:" << regionPoints.size());
if ((int) regionPoints.size() < 1) {
ROS_ERROR("no planes found");
return;
}
std::stringstream filename;
for (size_t plane = 0; plane < regionPoints.size (); plane++)
{
filename.str("");
filename << "plane" << plane << ".pcd";
writer.write(filename.str(), *cloudInput, regionPoints[plane].indices, true);
ROS_INFO("Plane model: [%f, %f, %f, %f] with %d inliers.",
planes_coeff[plane].values[0], planes_coeff[plane].values[1],
planes_coeff[plane].values[2], planes_coeff[plane].values[3], (int)regionPoints[plane].indices.size ());
//Project Points into the Perfect plane
PointCloud::Ptr cloud_projected(new PointCloud());
pcl::PointIndicesPtr cloudPlaneIndicesPtr(new pcl::PointIndices(regionPoints[plane]));
pcl::ModelCoefficientsPtr coeff(new pcl::ModelCoefficients(planes_coeff[plane]));
proj_.setInputCloud(cloudInput);
proj_.setIndices(cloudPlaneIndicesPtr);
proj_.setModelCoefficients(coeff);
proj_.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
proj_.filter(*cloud_projected);
PointCloud::Ptr cloud_hull(new PointCloud());
// Create a Convex Hull representation of the projected inliers
chull_.setInputCloud(cloud_projected);
chull_.reconstruct(*cloud_hull);
ROS_INFO("Convex hull has: %d data points.", (int)cloud_hull->points.size ());
if ((int) cloud_hull->points.size() == 0)
{
ROS_WARN("Convex hull has: %d data points. Returning.", (int)cloud_hull->points.size ());
return;
}
// Extract the handle clusters using a polygonal prism
pcl::PointIndices::Ptr handlesIndicesPtr(new pcl::PointIndices());
prism_.setHeightLimits(0.05, 0.1);
prism_.setInputCloud(cloudInput);
prism_.setInputPlanarHull(cloud_hull);
prism_.segment(*handlesIndicesPtr);
ROS_INFO("Number of handle candidates: %d.", (int)handlesIndicesPtr->indices.size ());
if((int)handlesIndicesPtr->indices.size () < 1100)
continue;
/*######### handle clustering code
handle_clusters.clear();
handle_cluster_.setClusterTolerance(0.03);
handle_cluster_.setMinClusterSize(200);
handle_cluster_.setSearchMethod(clusters_tree_);
handle_cluster_.setInputCloud(handles);
handle_cluster_.setIndices(handlesIndicesPtr);
handle_cluster_.extract(handle_clusters);
for(size_t j = 0; j < handle_clusters.size(); j++)
{
filename.str("");
filename << "handle" << (int)i << "-" << (int)j << ".pcd";
writer.write(filename.str(), *handles, handle_clusters[j].indices, true);
}*/
pcl::StatisticalOutlierRemoval<Point> sor;
PointCloud::Ptr cloud_filtered (new pcl::PointCloud<Point>);
sor.setInputCloud (cloudInput);
sor.setIndices(handlesIndicesPtr);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered);
PointCloudNormal::Ptr cloud_filtered_hack (new PointCloudNormal);
pcl::copyPointCloud(*cloud_filtered, *cloud_filtered_hack);
// Our handle is in cloud_filtered. We want indices of a cloud (also filtered for NaNs)
pcl::KdTreeFLANN<PointNormal> kdtreeNN;
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
kdtreeNN.setInputCloud(pointCloudNormals);
for(size_t j = 0; j < cloud_filtered_hack->points.size(); j++)
{
kdtreeNN.nearestKSearch(cloud_filtered_hack->points[j], 1, pointIdxNKNSearch, pointNKNSquaredDistance);
handles.push_back(pointIdxNKNSearch[0]);
}
}
}
/**
* 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;
}
}
