示例#1
0
int
main(int argc, char** argv)
{
  // initialize PointClouds
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);

  // populate our PointCloud with points
  cloud->width    = 500;
  cloud->height   = 1;
  cloud->is_dense = false;
  cloud->points.resize (cloud->width * cloud->height);
  for (size_t i = 0; i < cloud->points.size (); ++i)
  {
    if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
    {
      cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
      cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
      if (i % 5 == 0)
        cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
      else if(i % 2 == 0)
        cloud->points[i].z =  sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
                                      - (cloud->points[i].y * cloud->points[i].y));
      else
        cloud->points[i].z =  - sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
                                        - (cloud->points[i].y * cloud->points[i].y));
    }
    else
    {
      cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
      cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
      if( i % 2 == 0)
        cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
      else
        cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
    }
  }

  std::vector<int> inliers;

  // created RandomSampleConsensus object and compute the appropriated model
  pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
    model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud));
  pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
    model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
  if(pcl::console::find_argument (argc, argv, "-f") >= 0)
  {
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
    ransac.setDistanceThreshold (.01);
    ransac.computeModel();
    ransac.getInliers(inliers);
  }
  else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 )
  {
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
    ransac.setDistanceThreshold (.01);
    ransac.computeModel();
    ransac.getInliers(inliers);
  }

  // copies all inliers of the model computed to another PointCloud
  pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);

  // creates the visualization object and adds either our orignial cloud or all of the inliers
  // depending on the command line arguments specified.
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
  if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
    viewer = simpleVis(final);
  else
// Methods bound to input/inlets
t_jit_err jit_pcl_segeuclidean_matrix_calc(t_jit_pcl_segeuclidean *x, void *inputs, void *outputs)
{
    t_jit_err			err = JIT_ERR_NONE;
    long				in_savelock;
    long				out_savelock;
    t_jit_matrix_info	in_minfo;
    t_jit_matrix_info	out_minfo;
    char				*in_bp;
    char				*out_bp;
    long				i, j;
    long				dimcount;
    long				planecount;
    long				dim[JIT_MATRIX_MAX_DIMCOUNT];
    void				*in_matrix;
    void				*out_matrix;
    
    long rowstride;
    float *fip, *fop;
    
    
    in_matrix 	= jit_object_method(inputs,_jit_sym_getindex,0);
    out_matrix 	= jit_object_method(outputs,_jit_sym_getindex,0);
    
    if (x && in_matrix && out_matrix)
    {
        in_savelock = (long) jit_object_method(in_matrix, _jit_sym_lock, 1);
        out_savelock = (long) jit_object_method(out_matrix, _jit_sym_lock, 1);
        
        jit_object_method(in_matrix, _jit_sym_getinfo, &in_minfo);
        jit_object_method(in_matrix, _jit_sym_getdata, &in_bp);
        
        if (!in_bp) {
            err=JIT_ERR_INVALID_INPUT;
            goto out;
        }
        
        //get dimensions/planecount
        dimcount   = in_minfo.dimcount;
        planecount = in_minfo.planecount;
        
        if( planecount < 3 )
        {
            object_error((t_object *)x, "jit.pcl requires a 3 plane matrix (xyz)");
            goto out;
        }
        if( in_minfo.type != _jit_sym_float32)
        {
            object_error((t_object *)x, "received: %s jit.pcl uses only float32 matrixes", in_minfo.type->s_name );
            goto out;
        }
        
        //if dimsize is 1, treat as infinite domain across that dimension.
        //otherwise truncate if less than the output dimsize
        
        for (i=0; i<dimcount; i++) {
            dim[i] = in_minfo.dim[i];
            if ( in_minfo.dim[i]<dim[i] && in_minfo.dim[i]>1) {
                dim[i] = in_minfo.dim[i];
            }
        }
        
        //******
        // PCL stuff
        
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
        cloud->width    = (uint32_t)dim[0];
        cloud->height   = (uint32_t)dim[1];
        cloud->is_dense = false;
        cloud->points.resize (cloud->width * cloud->height);
        
        rowstride = in_minfo.dimstride[1];// >> 2L;
        size_t count = 0;
        
        for (j = 0; j < dim[0]; j++)
        {
            fip =  (float *)(in_bp + j * in_minfo.dimstride[0]);
            
            for( i = 0; i < dim[1]; i++)
            {
                cloud->points[count].x = ((float *)fip)[0];
                cloud->points[count].y = ((float *)fip)[1];
                cloud->points[count].z = ((float *)fip)[2];
        //        cloud->points[count].r = (uint8_t)(((float *)fip)[3] * 255.0);
        //        cloud->points[count].g = (uint8_t)(((float *)fip)[4] * 255.0);
        //        cloud->points[count].b = (uint8_t)(((float *)fip)[5] * 255.0);
                count++;
                fip += rowstride;
            }
        }
        
      
        {

            /*
            //filter
            pcl::VoxelGrid<pcl::PointXYZRGB> grid;
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_voxel_ (new pcl::PointCloud<pcl::PointXYZRGB>);
            
            grid.setLeafSize (x->leafsize, x->leafsize, x->leafsize);
            grid.setInputCloud (cloud);
            grid.filter (*cloud_voxel_);
            
            
            pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_sor_ (new pcl::PointCloud<pcl::PointXYZRGB>);
            
            sor.setInputCloud(cloud_voxel_);
            sor.setMeanK( (uint32_t)x->npoints );
            sor.setStddevMulThresh(x->stdthresh);
            sor.filter(*cloud_sor_);
            */
            
            pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
            kdtree->setInputCloud(cloud);
            
            // Euclidean clustering object.
            pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
            // Set cluster tolerance to 2cm (small values may cause objects to be divided
            // in several clusters, whereas big values may join objects in a same cluster).
            clustering.setClusterTolerance(x->cluster_tol);
            // Set the minimum and maximum number of points that a cluster can have.
            clustering.setMinClusterSize(10);
            clustering.setMaxClusterSize(25000);
            clustering.setSearchMethod(kdtree);
            clustering.setInputCloud(cloud);
            
            std::vector<pcl::PointIndices> clusters;
            clustering.extract(clusters);
            // For every cluster...
            
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
            cluster->points.resize(cloud->size());
            cluster->width = (uint32_t)cloud->points.size();
            cluster->height = 1;
            cluster->is_dense = true;


            if( clusters.size() > 0 )
            {
                double segment_inc = 255. / clusters.size();
          
                int count = 0;
                int clusterN = 0;
                for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
                {

                    for (std::vector<int>::const_iterator p = i->indices.begin(); p != i->indices.end(); ++p)
                    {

                        cluster->points[count].x = cloud->points[ *p ].x;
                        cluster->points[count].y = cloud->points[ *p ].y;
                        cluster->points[count].z = cloud->points[ *p ].z;

                        cluster->points[count].r = (uint8_t)(segment_inc * clusterN);
                        cluster->points[count].g = (uint8_t)(segment_inc * clusterN);
                        cluster->points[count].b = 255;

                        count++;
                    }
                    clusterN++;
                }
            }
            err = jit_xyzrgb2jit(x, cluster, &out_minfo, &out_matrix );
            if( err != JIT_ERR_NONE )
                goto out;
            
        }
      
        // unable to make use of jitter's parallel methods since we need all the data together
        //jit_parallel_ndim_simplecalc2((method)jit_pcl_segeuclidean_calculate_ndim,
        //	x, dimcount, dim, planecount, &in_minfo, in_bp, &out_minfo, out_bp,
        //	0 /* flags1 */, 0 /* flags2 */);
        
    }
    else
        return JIT_ERR_INVALID_PTR;
    
out:
    jit_object_method( out_matrix, _jit_sym_lock, out_savelock );
    jit_object_method( in_matrix, _jit_sym_lock, in_savelock );
    return err;
}
int 
main (int argc, char** argv)
{
  // Read in the cloud data
  pcl::PCDReader reader;
  pcl::PCDWriter writer;
  pcl::PointCloud<PointType>::Ptr cloud (new pcl::PointCloud<PointType>);
  std::vector<int> filenames;
  filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
  reader.read (argv[filenames[0]], *cloud);
  std::string pcd_filename;
  std::string png_filename = argv[filenames[0]];

  // Take the origional png image out
  png::image<png::rgb_pixel> origin_image(cloud->width, cloud->height);
  int origin_index = 0;
  for (size_t y = 0; y < origin_image.get_height (); ++y) {
    for (size_t x = 0; x < origin_image.get_width (); ++x) {
      const PointType & p = cloud->points[origin_index++];
      origin_image[y][x] = png::rgb_pixel(p.r, p.g, p.b);
    }
  }

  png_filename.replace(png_filename.length () - 4, 4, ".png");
  origin_image.write(png_filename);

  std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*  

  float min_depth = 0.1;
  pcl::console::parse_argument (argc, argv, "-min_depth", min_depth);

  float max_depth = 3.0;
  pcl::console::parse_argument (argc, argv, "-max_depth", max_depth);

  float max_x = 1.0;
  pcl::console::parse_argument (argc, argv, "-max_x", max_x);

  float min_x = -1.0;
  pcl::console::parse_argument (argc, argv, "-min_x", min_x);

  float max_y = 1.0;
  pcl::console::parse_argument (argc, argv, "-max_y", max_y);

  float min_y = -1.0;
  pcl::console::parse_argument (argc, argv, "-min_y", min_y);


  int plane_seg_times = 1;
  pcl::console::parse_argument (argc, argv, "-plane_seg_times", plane_seg_times);
  for (int i = 0; i < plane_seg_times; i++) {
    remove_plane(cloud, min_depth, max_depth, min_x, max_x, min_y, max_y);
  }

  pcd_filename = argv[filenames[0]];
  pcd_filename.replace(pcd_filename.length () - 4, 8, "plane.pcd");
  pcl::io::savePCDFile(pcd_filename, *cloud);

  // std::cout << "PointCloud after removing the plane has: " << cloud->points.size () << " data points." << std::endl; //*
  uint32_t xmin = 1000, xmax = 0, ymin = 1000, ymax = 0;

  pcl::PointCloud<PointXYZRGBUV>::Ptr cloud_uv (new pcl::PointCloud<PointXYZRGBUV>);
  for (size_t index = 0; index < cloud->points.size(); index++) {
    const pcl::PointXYZRGB & p = cloud->points[index];
    if (p.x != p.x || p.y != p.y || p.z != p.z) { // if current point is invalid
      continue;
    }

    PointXYZRGBUV cp = PointXYZRGBUV(p, index % cloud-> width, index / cloud->width);
    cloud_uv->points.push_back (cp); 
  }
  cloud_uv->width = cloud_uv->points.size ();
  cloud_uv->height = 1;
  
  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<PointXYZRGBUV>::Ptr tree (new pcl::search::KdTree<PointXYZRGBUV>);
  tree->setInputCloud (cloud_uv);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<PointXYZRGBUV> ec;
  ec.setClusterTolerance (0.02); // 2cm
  ec.setMinClusterSize (500);
  ec.setMaxClusterSize (25000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (cloud_uv);
  ec.extract (cluster_indices);
  

  int j = 0;
  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>);
    xmin = 1000;
    xmax = 0; 
    ymin = 1000;
    ymax = 0;

    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) {
        PointXYZRGBUV& p = cloud_uv->points[*pit];
        pcl::PointXYZRGB cp_rgb;
        cp_rgb.x = p.x; cp_rgb.y = p.y; cp_rgb.z = p.z;
        cp_rgb.rgb = p.rgb; 
        cloud_cluster->points.push_back(cp_rgb);

        xmin = std::min(xmin, p.u);
        xmax = std::max(xmax, p.u);
        ymin = std::min(ymin, p.v);
        ymax = std::max(ymax, p.v);
    }
    cloud_cluster->is_dense = true;
    cloud_cluster->width = cloud_cluster->points.size();
    cloud_cluster->height = 1;

    std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
    pcd_filename = argv[filenames[0]];
    std::stringstream ss;
    ss << "cluster_" << j++ << ".pcd";
    pcd_filename.replace(pcd_filename.length () - 4, ss.str().length(), ss.str());
    pcl::io::savePCDFile(pcd_filename, *cloud_cluster);

    png::image<png::rgb_pixel> image(cloud_cluster->width, cloud_cluster->height);
    int i = 0;
    for (size_t y = 0; y < image.get_height (); ++y) {
      for (size_t x = 0; x < image.get_width (); ++x) {
        const PointType & p = cloud_cluster->points[i++];
        image[y][x] = png::rgb_pixel(p.r, p.g, p.b);
      }
    }
    pcd_filename.replace(pcd_filename.length () - 4, 4, ".png");
    image.write(pcd_filename);

    //crop out image patch
    png::image<png::rgb_pixel> image_patch(xmax - xmin + 1, ymax - ymin + 1);
    for (size_t y = 0; y < image_patch.get_height (); ++y) {
      for (size_t x = 0; x < image_patch.get_width (); ++x) {
        image_patch[y][x] = origin_image[y+ymin][x+xmin];
      }
    }
    pcd_filename.replace(pcd_filename.length () - 4, 7, "box.png");
    image_patch.write(pcd_filename);

    // writer.write<pcl::PointXYZRGB> (ss.str (), *cloud_cluster, false); //*
  }

  return (0);
}
void callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
    std::string base_frame("base_link");
    
    geometry_msgs::PointStamped pout;
    geometry_msgs::PointStamped pin;
    pin.header.frame_id = msg->header.frame_id;
    pin.point.x = 0; pin.point.y = 0; pin.point.z = 0;
    geometry_msgs::Vector3Stamped vout;
    geometry_msgs::Vector3Stamped vin;
    vin.header.frame_id = base_frame;
    vin.vector.x = 0; vin.vector.y = 0; vin.vector.z = 1;
    
    float height;
    Eigen::Vector3f normal;
    try {
        listener->transformPoint(base_frame, ros::Time(0), pin, msg->header.frame_id, pout);
        height = pout.point.z;
        listener->transformVector(msg->header.frame_id, ros::Time(0), vin, base_frame, vout);
        normal = Eigen::Vector3f(vout.vector.x, vout.vector.y, vout.vector.z);
    }
    catch (tf::TransformException ex) {
        ROS_INFO("%s",ex.what());
        return;
    }
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(*msg, *cloud);
	
	Eigen::Vector3f p = -height*normal; // a point in the floor plane
	float d = -p.dot(normal); // = height, d in the plane equation
	
	obstacle_cloud->points.clear();
	obstacle_cloud->points.reserve(cloud->size());
	floor_cloud->points.clear();
	Eigen::Vector3f temp;
	float floor_dist;
	pcl::PointXYZ point;
	for (size_t i = 0; i < cloud->size(); ++i) {
	    
	    /*temp = cloud->points[i].getVector3fMap(); // DEBUG!
	    
	    if (i%640 < 300 && i%640 > 200 && i < 640*200 && i > 640*100) {
	        temp -= 0.06*normal;
	    }*/

        // check signed distance to floor
	    floor_dist = normal.dot(cloud->points[i].getVector3fMap()) + d;
	    //floor_dist = normal.dot(temp) + d; // DEBUG
	    
	    // if enough below, consider a stair point
	    if (floor_dist < below_threshold) {
	        temp = cloud->points[i].getVector3fMap(); // RELEASE
	        point.getVector3fMap() = -(d/normal.dot(temp))*temp + normal*0.11;
	        floor_cloud->points.push_back(point);
	    }
	    else { // add as a normal obstacle or clearing point
	        obstacle_cloud->points.push_back(cloud->points[i]);
	    }
	}

	sensor_msgs::PointCloud2 floor_msg;
    pcl::toROSMsg(*floor_cloud, floor_msg);
	floor_msg.header = msg->header;

	floor_pub.publish(floor_msg);
	
	sensor_msgs::PointCloud2 obstacle_msg;
    pcl::toROSMsg(*obstacle_cloud, obstacle_msg);
	obstacle_msg.header = msg->header;
	
	obstacle_pub.publish(obstacle_msg);
}
void ColorizeRandomForest::extract(const sensor_msgs::PointCloud2 pc)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());

    std::vector<int> indices;
    pcl::fromROSMsg(pc, *cloud);
    cloud->is_dense = false;
    pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);

    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
    tree->setInputCloud (cloud);
    pcl::PassThrough<pcl::PointXYZRGB> pass;
    pass.setInputCloud (cloud);
    pass.setFilterFieldName (std::string("z"));
    pass.setFilterLimits (0.0, 1.5);
    pass.filter (*cloud);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
    ec.setClusterTolerance (0.025);
    ec.setMinClusterSize (200);
    ec.setMaxClusterSize (100000);
    ec.setSearchMethod (tree);
    ec.setInputCloud (cloud);
    ec.extract (cluster_indices);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloth_cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr noncloth_cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
    int cluster_num = 0;
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
    {
        JSK_ROS_INFO("Calculate time %d / %ld", cluster_num  , cluster_indices.size());
        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 (cloud->points[*pit]);
        cloud_cluster->is_dense = true;
        cluster_num ++ ;

        pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> ne;
        pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
        pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
        ne.setInputCloud (cloud_cluster);
        ne.setSearchMethod (tree);
        ne.setRadiusSearch (0.02);
        ne.compute (*cloud_normals);

        for (int cloud_index = 0; cloud_index <  cloud_normals->points.size(); cloud_index++) {
            cloud_normals->points[cloud_index].x = cloud_cluster->points[cloud_index].x;
            cloud_normals->points[cloud_index].y = cloud_cluster->points[cloud_index].y;
            cloud_normals->points[cloud_index].z = cloud_cluster->points[cloud_index].z;
        }

        int result_counter=0, call_counter = 0;
        pcl::PointXYZRGBNormal max_pt,min_pt;
        pcl::getMinMax3D(*cloud_normals, min_pt, max_pt);

        for (int i = 0 ; i < 30; i++) {
            double lucky = 0, lucky2 = 0;
            std::string axis("x"), other_axis("y");
            int rand_xy = rand()%2;
            if (rand_xy == 0) {
                lucky = min_pt.x - pass_offset_ + (max_pt.x - min_pt.x - pass_offset_*2) * 1.0 * rand() / RAND_MAX;
                lucky2 = min_pt.y + (max_pt.y - min_pt.y) * 1.0 * rand() / RAND_MAX;
            } else {
                lucky = min_pt.y - pass_offset_ + (max_pt.y - min_pt.y - pass_offset_*2) * 1.0 * rand() / RAND_MAX;
                lucky2 = min_pt.x + (max_pt.x - min_pt.x) * 1.0 * rand() / RAND_MAX;
                axis = std::string("y");
                other_axis = std::string("x");
            }
            pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals_pass (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
            pcl::PassThrough<pcl::PointXYZRGBNormal> pass;
            pcl::IndicesPtr indices_x(new std::vector<int>());
            pass.setInputCloud (cloud_normals);
            pass.setFilterFieldName (axis);
            float small = std::min(lucky, lucky + pass_offset_);
            float large = std::max(lucky, lucky + pass_offset_);
            pass.setFilterLimits (small, large);
            pass.filter (*cloud_normals_pass);
            pass.setInputCloud (cloud_normals_pass);
            pass.setFilterFieldName (other_axis);
            float small2 = std::min(lucky2, lucky2 + pass_offset2_);
            float large2 = std::max(lucky2, lucky2 + pass_offset2_);
            pass.setFilterLimits (small2, large2);
            pass.filter (*cloud_normals_pass);

            std::vector<int> tmp_indices;
            pcl::removeNaNFromPointCloud(*cloud_normals_pass, *cloud_normals_pass, tmp_indices);

            if(cloud_normals_pass->points.size() == 0)
                continue;

            pcl::FPFHEstimationOMP<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal, pcl::FPFHSignature33> fpfh;
            pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
            pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33> ());
            fpfh.setNumberOfThreads(8);
            fpfh.setInputCloud (cloud_normals_pass);
            fpfh.setInputNormals (cloud_normals_pass);
            fpfh.setSearchMethod (tree);
            fpfh.setRadiusSearch (radius_search_);
            fpfh.compute (*fpfhs);

            if((int)fpfhs->points.size() == 0)
                continue;

            std::vector<double> result;
            int target_id, max_value;
            if ((int)fpfhs->points.size() - sum_num_ - 1 < 1) {
                target_id = 0;
                max_value = (int)fpfhs->points.size();
            } else {
                target_id = rand() % ((int)fpfhs->points.size() - sum_num_ - 1);
                max_value = target_id + sum_num_;
            }

            bool has_nan = false;
            for(int index = 0; index < 33; index++) {
                float sum_hist_points = 0;
                for(int kndex = target_id; kndex < max_value; kndex++)
                {
                    sum_hist_points+=fpfhs->points[kndex].histogram[index];
                }
                result.push_back( sum_hist_points/sum_num_ );
            }

            for(int x = 0; x < result.size(); x ++) {
                if(pcl_isnan(result[x]))
                    has_nan = true;
            }
            if(has_nan)
                break;

            call_counter++;
            ros::ServiceClient client = pnh_->serviceClient<ml_classifiers::ClassifyData>("/predict");
            ml_classifiers::ClassifyData srv;
            ml_classifiers::ClassDataPoint class_data_point;
            class_data_point.point = result;
            srv.request.data.push_back(class_data_point);
            if(client.call(srv))
                if (atoi(srv.response.classifications[0].c_str()) == 0)
                    result_counter += 1;
            JSK_ROS_INFO("response result : %s", srv.response.classifications[0].c_str());
        }

        if(result_counter >= call_counter / 2) {
            JSK_ROS_INFO("Cloth %d / %d", result_counter, call_counter);
        }
        else {
            JSK_ROS_INFO("Not Cloth %d / %d", result_counter, call_counter);
        }

        for (int i = 0; i < cloud_cluster->points.size(); i++) {
            if(result_counter >= call_counter / 2) {
                cloth_cloud_cluster->points.push_back (cloud_cluster->points[i]);
            }
            else {
                noncloth_cloud_cluster->points.push_back (cloud_cluster->points[i]);
            }
        }
    }
    sensor_msgs::PointCloud2 cloth_pointcloud2;
    pcl::toROSMsg(*cloth_cloud_cluster, cloth_pointcloud2);
    cloth_pointcloud2.header = pc.header;
    cloth_pointcloud2.is_dense = false;
    pub_.publish(cloth_pointcloud2);

    sensor_msgs::PointCloud2 noncloth_pointcloud2;
    pcl::toROSMsg(*noncloth_cloud_cluster, noncloth_pointcloud2);
    noncloth_pointcloud2.header = pc.header;
    noncloth_pointcloud2.is_dense = false;
    pub2_.publish(noncloth_pointcloud2);
}
void
pcl::apps::RenderViewsTesselatedSphere::generateViews() {
  //center object
  double CoM[3];
  vtkIdType npts_com = 0, *ptIds_com = nullptr;
  vtkSmartPointer<vtkCellArray> cells_com = polydata_->GetPolys ();

  double center[3], p1_com[3], p2_com[3], p3_com[3], totalArea_com = 0;
  double comx = 0, comy = 0, comz = 0;
  for (cells_com->InitTraversal (); cells_com->GetNextCell (npts_com, ptIds_com);)
  {
    polydata_->GetPoint (ptIds_com[0], p1_com);
    polydata_->GetPoint (ptIds_com[1], p2_com);
    polydata_->GetPoint (ptIds_com[2], p3_com);
    vtkTriangle::TriangleCenter (p1_com, p2_com, p3_com, center);
    double area_com = vtkTriangle::TriangleArea (p1_com, p2_com, p3_com);
    comx += center[0] * area_com;
    comy += center[1] * area_com;
    comz += center[2] * area_com;
    totalArea_com += area_com;
  }

  CoM[0] = comx / totalArea_com;
  CoM[1] = comy / totalArea_com;
  CoM[2] = comz / totalArea_com;

  vtkSmartPointer<vtkTransform> trans_center = vtkSmartPointer<vtkTransform>::New ();
  trans_center->Translate (-CoM[0], -CoM[1], -CoM[2]);
  vtkSmartPointer<vtkMatrix4x4> matrixCenter = trans_center->GetMatrix ();

  vtkSmartPointer<vtkTransformFilter> trans_filter_center = vtkSmartPointer<vtkTransformFilter>::New ();
  trans_filter_center->SetTransform (trans_center);
  trans_filter_center->SetInputData (polydata_);
  trans_filter_center->Update ();

  vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
  mapper->SetInputConnection (trans_filter_center->GetOutputPort ());
  mapper->Update ();

  //scale so it fits in the unit sphere!
  double bb[6];
  mapper->GetBounds (bb);
  double ms = (std::max) ((std::fabs) (bb[0] - bb[1]),
                          (std::max) ((std::fabs) (bb[2] - bb[3]), (std::fabs) (bb[4] - bb[5])));
  double max_side = radius_sphere_ / 2.0;
  double scale_factor = max_side / ms;

  vtkSmartPointer<vtkTransform> trans_scale = vtkSmartPointer<vtkTransform>::New ();
  trans_scale->Scale (scale_factor, scale_factor, scale_factor);
  vtkSmartPointer<vtkMatrix4x4> matrixScale = trans_scale->GetMatrix ();

  vtkSmartPointer<vtkTransformFilter> trans_filter_scale = vtkSmartPointer<vtkTransformFilter>::New ();
  trans_filter_scale->SetTransform (trans_scale);
  trans_filter_scale->SetInputConnection (trans_filter_center->GetOutputPort ());
  trans_filter_scale->Update ();

  mapper->SetInputConnection (trans_filter_scale->GetOutputPort ());
  mapper->Update ();

  //////////////////////////////
  // * Compute area of the mesh
  //////////////////////////////
  vtkSmartPointer<vtkCellArray> cells = mapper->GetInput ()->GetPolys ();
  vtkIdType npts = 0, *ptIds = nullptr;

  double p1[3], p2[3], p3[3], totalArea = 0;
  for (cells->InitTraversal (); cells->GetNextCell (npts, ptIds);)
  {
    polydata_->GetPoint (ptIds[0], p1);
    polydata_->GetPoint (ptIds[1], p2);
    polydata_->GetPoint (ptIds[2], p3);
    totalArea += vtkTriangle::TriangleArea (p1, p2, p3);
  }

  //create icosahedron
  vtkSmartPointer<vtkPlatonicSolidSource> ico = vtkSmartPointer<vtkPlatonicSolidSource>::New ();
  ico->SetSolidTypeToIcosahedron ();
  ico->Update ();

  //tessellate cells from icosahedron
  vtkSmartPointer<vtkLoopSubdivisionFilter> subdivide = vtkSmartPointer<vtkLoopSubdivisionFilter>::New ();
  subdivide->SetNumberOfSubdivisions (tesselation_level_);
  subdivide->SetInputConnection (ico->GetOutputPort ());
  subdivide->Update();

  // Get camera positions
  vtkPolyData *sphere = subdivide->GetOutput ();

  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > cam_positions;
  if (!use_vertices_)
  {
    vtkSmartPointer<vtkCellArray> cells_sphere = sphere->GetPolys ();
    cam_positions.resize (sphere->GetNumberOfPolys ());

    size_t i=0;
    for (cells_sphere->InitTraversal (); cells_sphere->GetNextCell (npts_com, ptIds_com);)
    {
      sphere->GetPoint (ptIds_com[0], p1_com);
      sphere->GetPoint (ptIds_com[1], p2_com);
      sphere->GetPoint (ptIds_com[2], p3_com);
      vtkTriangle::TriangleCenter (p1_com, p2_com, p3_com, center);
      cam_positions[i] = Eigen::Vector3f (float (center[0]), float (center[1]), float (center[2]));
      i++;
    }

  }
  else
  {
    cam_positions.resize (sphere->GetNumberOfPoints ());
    for (vtkIdType i = 0; i < sphere->GetNumberOfPoints (); i++)
    {
      double cam_pos[3];
      sphere->GetPoint (i, cam_pos);
      cam_positions[i] = Eigen::Vector3f (float (cam_pos[0]), float (cam_pos[1]), float (cam_pos[2]));
    }
  }

  /*int valid = 0;
  for (size_t i = 0; i < cam_positions.size (); i++)
  {
    if (campos_constraints_func_ (cam_positions[i]))
    {
      cam_positions[valid] = cam_positions[i];
      valid++;
    }
  }

  cam_positions.resize (valid);*/

  double camera_radius = radius_sphere_;
  std::array<double, 3> cam_pos;
  std::array<double, 3> first_cam_pos;

  first_cam_pos[0] = cam_positions[0][0] * radius_sphere_;
  first_cam_pos[1] = cam_positions[0][1] * radius_sphere_;
  first_cam_pos[2] = cam_positions[0][2] * radius_sphere_;

  //create renderer and window
  vtkSmartPointer<vtkRenderWindow> render_win = vtkSmartPointer<vtkRenderWindow>::New ();
  vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New ();
  render_win->AddRenderer (renderer);
  render_win->SetSize (resolution_, resolution_);
  renderer->SetBackground (1.0, 0, 0);

  //create picker
  vtkSmartPointer<vtkWorldPointPicker> worldPicker = vtkSmartPointer<vtkWorldPointPicker>::New ();

  vtkSmartPointer<vtkCamera> cam = vtkSmartPointer<vtkCamera>::New ();
  cam->SetFocalPoint (0, 0, 0);

  Eigen::Vector3f cam_pos_3f = cam_positions[0];
  Eigen::Vector3f perp = cam_pos_3f.cross (Eigen::Vector3f::UnitY ());
  cam->SetViewUp (perp[0], perp[1], perp[2]);

  cam->SetPosition (first_cam_pos.data());
  cam->SetViewAngle (view_angle_);
  cam->Modified ();

  //For each camera position, traposesnsform the object and render view
  for (const auto &cam_position : cam_positions)
  {
    cam_pos[0] = cam_position[0];
    cam_pos[1] = cam_position[1];
    cam_pos[2] = cam_position[2];

    //create temporal virtual camera
    vtkSmartPointer<vtkCamera> cam_tmp = vtkSmartPointer<vtkCamera>::New ();
    cam_tmp->SetViewAngle (view_angle_);

    Eigen::Vector3f cam_pos_3f (static_cast<float> (cam_pos[0]), static_cast<float> (cam_pos[1]), static_cast<float> (cam_pos[2]));
    cam_pos_3f = cam_pos_3f.normalized ();
    Eigen::Vector3f test = Eigen::Vector3f::UnitY ();

    //If the view up is parallel to ray cam_pos - focalPoint then the transformation
    //is singular and no points are rendered...
    //make sure it is perpendicular
    if (fabs (cam_pos_3f.dot (test)) == 1)
    {
      //parallel, create
      test = cam_pos_3f.cross (Eigen::Vector3f::UnitX ());
    }

    cam_tmp->SetViewUp (test[0], test[1], test[2]);

    for (double &cam_po : cam_pos)
    {
      cam_po *= camera_radius;
    }

    cam_tmp->SetPosition (cam_pos.data());
    cam_tmp->SetFocalPoint (0, 0, 0);
    cam_tmp->Modified ();

    //rotate model so it looks the same as if we would look from the new position
    vtkSmartPointer<vtkMatrix4x4> view_trans_inverted = vtkSmartPointer<vtkMatrix4x4>::New ();
    vtkMatrix4x4::Invert (cam->GetViewTransformMatrix (), view_trans_inverted);
    vtkSmartPointer<vtkTransform> trans_rot_pose = vtkSmartPointer<vtkTransform>::New ();
    trans_rot_pose->Identity ();
    trans_rot_pose->Concatenate (view_trans_inverted);
    trans_rot_pose->Concatenate (cam_tmp->GetViewTransformMatrix ());
    vtkSmartPointer<vtkTransformFilter> trans_rot_pose_filter = vtkSmartPointer<vtkTransformFilter>::New ();
    trans_rot_pose_filter->SetTransform (trans_rot_pose);
    trans_rot_pose_filter->SetInputConnection (trans_filter_scale->GetOutputPort ());

    //translate model so we can place camera at (0,0,0)
    vtkSmartPointer<vtkTransform> translation = vtkSmartPointer<vtkTransform>::New ();
    translation->Translate (first_cam_pos[0] * -1, first_cam_pos[1] * -1, first_cam_pos[2] * -1);
    vtkSmartPointer<vtkTransformFilter> translation_filter = vtkSmartPointer<vtkTransformFilter>::New ();
    translation_filter->SetTransform (translation);
    translation_filter->SetInputConnection (trans_rot_pose_filter->GetOutputPort ());

    //modify camera
    cam_tmp->SetPosition (0, 0, 0);
    cam_tmp->SetFocalPoint (first_cam_pos[0] * -1, first_cam_pos[1] * -1, first_cam_pos[2] * -1);
    cam_tmp->Modified ();

    //notice transformations for final pose
    vtkSmartPointer<vtkMatrix4x4> matrixRotModel = trans_rot_pose->GetMatrix ();
    vtkSmartPointer<vtkMatrix4x4> matrixTranslation = translation->GetMatrix ();

    mapper->SetInputConnection (translation_filter->GetOutputPort ());
    mapper->Update ();

    //render view
    vtkSmartPointer<vtkActor> actor_view = vtkSmartPointer<vtkActor>::New ();
    actor_view->SetMapper (mapper);
    renderer->SetActiveCamera (cam_tmp);
    renderer->AddActor (actor_view);
    renderer->Modified ();
    //renderer->ResetCameraClippingRange ();
    render_win->Render ();

    //back to real scale transform
    vtkSmartPointer<vtkTransform> backToRealScale = vtkSmartPointer<vtkTransform>::New ();
    backToRealScale->PostMultiply ();
    backToRealScale->Identity ();
    backToRealScale->Concatenate (matrixScale);
    backToRealScale->Concatenate (matrixTranslation);
    backToRealScale->Inverse ();
    backToRealScale->Modified ();
    backToRealScale->Concatenate (matrixTranslation);
    backToRealScale->Modified ();

    Eigen::Matrix4f backToRealScale_eigen;
    backToRealScale_eigen.setIdentity ();

    for (int x = 0; x < 4; x++)
      for (int y = 0; y < 4; y++)
        backToRealScale_eigen (x, y) = float (backToRealScale->GetMatrix ()->GetElement (x, y));

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    cloud->points.resize (resolution_ * resolution_);

    if (gen_organized_)
    {
      cloud->width = resolution_;
      cloud->height = resolution_;
      cloud->is_dense = false;

      double coords[3];
      float * depth = new float[resolution_ * resolution_];
      render_win->GetZbufferData (0, 0, resolution_ - 1, resolution_ - 1, &(depth[0]));

      for (int x = 0; x < resolution_; x++)
      {
        for (int y = 0; y < resolution_; y++)
        {
          float value = depth[y * resolution_ + x];
          if (value == 1.0)
          {
            cloud->at (y, x).x = cloud->at (y, x).y = cloud->at (y, x).z = std::numeric_limits<float>::quiet_NaN ();
          }
          else
          {
            worldPicker->Pick (x, y, value, renderer);
            worldPicker->GetPickPosition (coords);
            cloud->at (y, x).x = static_cast<float> (coords[0]);
            cloud->at (y, x).y = static_cast<float> (coords[1]);
            cloud->at (y, x).z = static_cast<float> (coords[2]);
            cloud->at (y, x).getVector4fMap () = backToRealScale_eigen
                                  * cloud->at (y, x).getVector4fMap ();
          }
        }
      }

      delete[] depth;

    }
    else
    {
      cloud->width = resolution_ * resolution_;
      cloud->height = 1;

      double coords[3];
      float * depth = new float[resolution_ * resolution_];
      render_win->GetZbufferData (0, 0, resolution_ - 1, resolution_ - 1, &(depth[0]));

      int count_valid_depth_pixels = 0;
      for (int x = 0; x < resolution_; x++)
      {
        for (int y = 0; y < resolution_; y++)
        {
          float value = depth[y * resolution_ + x];
          if (value == 1.0)
            continue;

          worldPicker->Pick (x, y, value, renderer);
          worldPicker->GetPickPosition (coords);
          cloud->points[count_valid_depth_pixels].x = static_cast<float> (coords[0]);
          cloud->points[count_valid_depth_pixels].y = static_cast<float> (coords[1]);
          cloud->points[count_valid_depth_pixels].z = static_cast<float> (coords[2]);
          cloud->points[count_valid_depth_pixels].getVector4fMap () = backToRealScale_eigen
                      * cloud->points[count_valid_depth_pixels].getVector4fMap ();
          count_valid_depth_pixels++;
        }
      }

      delete[] depth;

      cloud->points.resize (count_valid_depth_pixels);
      cloud->width = count_valid_depth_pixels;
    }

    if(compute_entropy_) {
      //////////////////////////////
      // * Compute area of the mesh
      //////////////////////////////

      vtkSmartPointer<vtkPolyData> polydata = mapper->GetInput ();
      polydata->BuildCells ();

      vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys ();
      vtkIdType npts = 0, *ptIds = nullptr;

      double p1[3], p2[3], p3[3], area, totalArea = 0;
      for (cells->InitTraversal (); cells->GetNextCell (npts, ptIds);)
      {
        polydata->GetPoint (ptIds[0], p1);
        polydata->GetPoint (ptIds[1], p2);
        polydata->GetPoint (ptIds[2], p3);
        area = vtkTriangle::TriangleArea (p1, p2, p3);
        totalArea += area;
      }

      /////////////////////////////////////
      // * Select visible cells (triangles)
      /////////////////////////////////////
      vtkSmartPointer<vtkHardwareSelector> hardware_selector = vtkSmartPointer<vtkHardwareSelector>::New ();
      hardware_selector->ClearBuffers();
      vtkSmartPointer<vtkSelection> hdw_selection = vtkSmartPointer<vtkSelection>::New ();
      hardware_selector->SetRenderer (renderer);
      hardware_selector->SetArea (0, 0, resolution_ - 1, resolution_ - 1);
      hardware_selector->SetFieldAssociation(vtkDataObject::FIELD_ASSOCIATION_CELLS);
      hdw_selection = hardware_selector->Select ();
      vtkSmartPointer<vtkIdTypeArray> ids = vtkSmartPointer<vtkIdTypeArray>::New ();
      ids = vtkIdTypeArray::SafeDownCast(hdw_selection->GetNode(0)->GetSelectionList());
      double visible_area = 0;
      for (vtkIdType sel_id = 0; sel_id < (ids->GetNumberOfTuples ()); sel_id++)
      {
        int id_mesh = int (ids->GetValue (sel_id));
        if(id_mesh >= polydata->GetNumberOfPolys())
          continue;

        vtkCell * cell = polydata->GetCell (id_mesh);
        vtkTriangle* triangle = dynamic_cast<vtkTriangle*> (cell);
        double p0[3];
        double p1[3];
        double p2[3];
        triangle->GetPoints ()->GetPoint (0, p0);
        triangle->GetPoints ()->GetPoint (1, p1);
        triangle->GetPoints ()->GetPoint (2, p2);
        area = vtkTriangle::TriangleArea (p0, p1, p2);
        visible_area += area;
      }

      entropies_.push_back (float (visible_area / totalArea));
    }

    //transform cloud to give camera coordinates instead of world coordinates!
    vtkSmartPointer<vtkMatrix4x4> view_transform = cam_tmp->GetViewTransformMatrix ();
    Eigen::Matrix4f trans_view;
    trans_view.setIdentity ();

    for (int x = 0; x < 4; x++)
      for (int y = 0; y < 4; y++)
        trans_view (x, y) = float (view_transform->GetElement (x, y));

    //NOTE: vtk view coordinate system is different than the standard camera coordinates (z forward, y down, x right)
    //thus, the fliping in y and z
    for (auto &point : cloud->points)
    {
      point.getVector4fMap () = trans_view * point.getVector4fMap ();
      point.y *= -1.0f;
      point.z *= -1.0f;
    }

    renderer->RemoveActor (actor_view);

    generated_views_.push_back (cloud);

    //create pose, from OBJECT coordinates to CAMERA coordinates!
    vtkSmartPointer<vtkTransform> transOCtoCC = vtkSmartPointer<vtkTransform>::New ();
    transOCtoCC->PostMultiply ();
    transOCtoCC->Identity ();
    transOCtoCC->Concatenate (matrixCenter);
    transOCtoCC->Concatenate (matrixRotModel);
    transOCtoCC->Concatenate (matrixTranslation);
    transOCtoCC->Concatenate (cam_tmp->GetViewTransformMatrix ());

    //NOTE: vtk view coordinate system is different than the standard camera coordinates (z forward, y down, x right)
    //thus, the fliping in y and z
    vtkSmartPointer<vtkMatrix4x4> cameraSTD = vtkSmartPointer<vtkMatrix4x4>::New ();
    cameraSTD->Identity ();
    cameraSTD->SetElement (0, 0, 1);
    cameraSTD->SetElement (1, 1, -1);
    cameraSTD->SetElement (2, 2, -1);

    transOCtoCC->Concatenate (cameraSTD);
    transOCtoCC->Modified ();

    Eigen::Matrix4f pose_view;
    pose_view.setIdentity ();

    for (int x = 0; x < 4; x++)
      for (int y = 0; y < 4; y++)
        pose_view (x, y) = float (transOCtoCC->GetMatrix ()->GetElement (x, y));

    poses_.push_back (pose_view);

  }
}
    typename pcl::PointCloud<PointT>::Ptr 
    get_point_cloud(int distance, bool colored) 
    {

        //get rgb and depth data
        while(!device -> getDepth(depth_map)){}
        while(!device -> getRGB(rgb)){}

        int depth_width = 640;
        int depth_height = 480;

        //create the empty Pointcloud
        boost::shared_ptr<pcl::PointCloud<PointT>> cloud (new pcl::PointCloud<PointT>);

        //initialize the PointCloud height and width
        //cloud->height = std::max (image_height, depth_height);
        //cloud->width = std::max (image_width, depth_width);

        //allow infinite values for points coordinates
        cloud->is_dense = false;

        //set camera parameters for kinect
        float focal_x_depth = 585.187492217609;//5.9421434211923247e+02;
        float focal_y_depth = 585.308616340665;//5.9104053696870778e+02;
        float center_x_depth = 322.714077555293;//3.3930780975300314e+02;
        float center_y_depth = 248.626108676666;//2.4273913761751615e+02;


        float bad_point = std::numeric_limits<float>::quiet_NaN ();
        #pragma omp parallel for
        for (unsigned int y = 0; y < depth_height; ++y)
            for ( unsigned int x = 0; x < depth_width; ++x){
                PointT ptout;
                uint16_t dz = depth_map[y*depth_width + x];
                if (abs(dz) < distance){
                    // project
                    Eigen::Vector3d ptd((x - center_x_depth) * dz / focal_x_depth, (y - center_y_depth) * dz/focal_y_depth, dz);
                    // assign output xyz
                    
                        ptout.x = ptd.x()*0.001f;
                        ptout.y = ptd.y()*0.001f;
                        ptout.z = ptd.z()*0.001f;
                    
                    if(colored){
                        uint8_t r = rgb[(y*depth_width + x)*3];
                        uint8_t g = rgb[(y*depth_width + x)*3 + 1];
                        uint8_t b = rgb[(y*depth_width + x)*3 + 2];

                        ptout.rgba = pcl::PointXYZRGB(r, g, b).rgba; //assign color 
                        //ptout.rgba = pcl::PointXYZRGB(0, 0, 0).rgba;

                    } else
                        ptout.rgba = pcl::PointXYZRGB(0, 0, 0).rgba;
                        #pragma omp critical
                        cloud->points.push_back(ptout); //assigns point to cloud   
                } 
                
            }
        cloud->height = 1;
        cloud->width = cloud->points.size();
        return (cloud);
    }
  void TorusFinder::segment(
    const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
  {
    if (!done_initialization_) {
      return;
    }
    boost::mutex::scoped_lock lock(mutex_);
    vital_checker_->poke();
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud
      (new pcl::PointCloud<pcl::PointNormal>);
    pcl::fromROSMsg(*cloud_msg, *cloud);
    jsk_recognition_utils::ScopedWallDurationReporter r
      = timer_.reporter(pub_latest_time_, pub_average_time_);
    if (voxel_grid_sampling_) {
      pcl::PointCloud<pcl::PointNormal>::Ptr downsampled_cloud
      (new pcl::PointCloud<pcl::PointNormal>);
      pcl::VoxelGrid<pcl::PointNormal> sor;
      sor.setInputCloud (cloud);
      sor.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
      sor.filter (*downsampled_cloud);
      cloud = downsampled_cloud;
    }
    
    pcl::SACSegmentation<pcl::PointNormal> seg;
    if (use_normal_) {
      pcl::SACSegmentationFromNormals<pcl::PointNormal, pcl::PointNormal> seg_normal;
      seg_normal.setInputNormals(cloud);
      seg = seg_normal;
    }

    
    seg.setOptimizeCoefficients (true);
    seg.setInputCloud(cloud);
    seg.setRadiusLimits(min_radius_, max_radius_);
    if (algorithm_ == "RANSAC") {
      seg.setMethodType(pcl::SAC_RANSAC);
    }
    else if (algorithm_ == "LMEDS") {
      seg.setMethodType(pcl::SAC_LMEDS);
    }
    else if (algorithm_ == "MSAC") {
      seg.setMethodType(pcl::SAC_MSAC);
    }
    else if (algorithm_ == "RRANSAC") {
      seg.setMethodType(pcl::SAC_RRANSAC);
    }
    else if (algorithm_ == "RMSAC") {
      seg.setMethodType(pcl::SAC_RMSAC);
    }
    else if (algorithm_ == "MLESAC") {
      seg.setMethodType(pcl::SAC_MLESAC);
    }
    else if (algorithm_ == "PROSAC") {
      seg.setMethodType(pcl::SAC_PROSAC);
    }
    
    seg.setDistanceThreshold (outlier_threshold_);
    seg.setMaxIterations (max_iterations_);
    seg.setModelType(pcl::SACMODEL_CIRCLE3D);
    if (use_hint_) {
      seg.setAxis(hint_axis_);
      seg.setEpsAngle(eps_hint_angle_);
    }
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    seg.segment(*inliers, *coefficients);
    JSK_NODELET_INFO("input points: %lu", cloud->points.size());
    if (inliers->indices.size() > min_size_) {
      // check direction. Torus should direct to origin of pointcloud
      // always.
      Eigen::Vector3f dir(coefficients->values[4],
                          coefficients->values[5],
                          coefficients->values[6]);
      if (dir.dot(Eigen::Vector3f::UnitZ()) < 0) {
        dir = - dir;
      }
      
      Eigen::Affine3f pose = Eigen::Affine3f::Identity();
      Eigen::Vector3f pos = Eigen::Vector3f(coefficients->values[0],
                                            coefficients->values[1],
                                            coefficients->values[2]);
      Eigen::Quaternionf rot;
      rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), dir);
      pose = pose * Eigen::Translation3f(pos) * Eigen::AngleAxisf(rot);
      PCLIndicesMsg ros_inliers;
      ros_inliers.indices = inliers->indices;
      ros_inliers.header = cloud_msg->header;
      pub_inliers_.publish(ros_inliers);
      PCLModelCoefficientMsg ros_coefficients;
      ros_coefficients.values = coefficients->values;
      ros_coefficients.header = cloud_msg->header;
      pub_coefficients_.publish(ros_coefficients);
      jsk_recognition_msgs::Torus torus_msg;
      torus_msg.header = cloud_msg->header;
      tf::poseEigenToMsg(pose, torus_msg.pose);
      torus_msg.small_radius = 0.01;
      torus_msg.large_radius = coefficients->values[3];
      pub_torus_.publish(torus_msg);
      jsk_recognition_msgs::TorusArray torus_array_msg;
      torus_array_msg.header = cloud_msg->header;
      torus_array_msg.toruses.push_back(torus_msg);
      pub_torus_array_.publish(torus_array_msg);
      // publish pose stamped
      geometry_msgs::PoseStamped pose_stamped;
      pose_stamped.header = torus_msg.header;
      pose_stamped.pose = torus_msg.pose;
      pub_pose_stamped_.publish(pose_stamped);
      pub_torus_array_with_failure_.publish(torus_array_msg);
      pub_torus_with_failure_.publish(torus_msg);
    }
    else {
      jsk_recognition_msgs::Torus torus_msg;
      torus_msg.header = cloud_msg->header;
      torus_msg.failure = true;
      pub_torus_with_failure_.publish(torus_msg);
      jsk_recognition_msgs::TorusArray torus_array_msg;
      torus_array_msg.header = cloud_msg->header;
      torus_array_msg.toruses.push_back(torus_msg);
      pub_torus_array_with_failure_.publish(torus_array_msg);
      JSK_NODELET_INFO("failed to find torus");
    }
  }
int
main (int argc, char** argv)
{

    if (argc < 4)
    {
        std::cerr << "please specify the point cloud and the command line arg '-r' or '-c' or '-s' or '-v' or '-av' or '-rg' + param\n" << std::endl;
        std::cerr << "radius filter: param ==> ray + min_neighbours " << std::endl;
        std::cerr << "    example: " << argv[0] << " cloud.pcd -r 100 10 \n" << std::endl;
        std::cerr << "codnitional filter: param ==> min_dist + max_dist along the axis" << std::endl;
        std::cerr << "    example: " << argv[0] << " cloud.pcd -c 0 1000 x\n" << std::endl;
        std::cerr << "statistical filter: param ==> num_of_neigh + std_dev" << std::endl;
        std::cerr << "    example: " << argv[0] << " cloud.pcd -s 50 1\n" << std::endl;
        std::cerr << "voxel grid downsampling: param ==> leaf_size" << std::endl;
        std::cerr << "    example: " << argv[0] << " cloud.pcd -v 10\n" << std::endl;
        std::cerr << "voxel grid (approximated) downsampling filter: param ==> leafsize" << std::endl;
        std::cerr << "    example: " << argv[0] << " cloud.pcd -av 10\n" << std::endl;
        std::cerr << "region growing: param ==> point_dist" << std::endl;
        std::cerr << "    example: " << argv[0] << " cloud.pcd -rg 10\n" << std::endl;
        exit(0);
    }
    pcl::PointCloud<PointType>::Ptr cloud (new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType>::Ptr cloud_filtered (new pcl::PointCloud<PointType>);
    if (pcl::io::loadPCDFile (argv[1], *cloud))
        return 0;
    cloud->height = 1;
    cloud->width = cloud->size();
    cloud->is_dense=0;

    do {
        pcl::PointCloud<PointType>::Ptr cloud_filtered (new pcl::PointCloud<PointType>);

        std::stringstream string;
        size_t begin;
        std::string buff;
        string << "output/";
        //if (system("mkdir output")) int a=0;

        buff.assign(argv[1]);
        // Check if the filename is a path
        begin=buff.find_last_of("/\\");
        if ( begin!=std::string::npos )
            buff=buff.substr(begin+1);

        begin=buff.find(".pcd");
        if ( begin!=std::string::npos )
            buff.assign ( buff,0,begin );
        else {
            std::cout << "No valid .pcd loaded" << std::endl;
            return 0;
        }
        string << buff;

        buff.assign(argv[2]);
        begin=buff.find("-");
        if ( begin!=std::string::npos )
            buff.assign ( buff, begin+1, buff.length() );
        else {
            std::cout << "No valid method defined" << std::endl;
            return 0;
        }
        string << "_" << buff;
        buff.clear();

        if (argc > 3)
            buff.assign(argv[3]);
        if ( buff.length() > 0 )
            string << "_" << buff;
        buff.clear();

        if (argc > 4)
            buff.assign(argv[4]);
        if ( buff.length() > 0 )
            string << "_" << buff;
        buff.clear();

        if (argc > 5)
            buff.assign(argv[5]);
        if ( buff.length() > 0 )
            string << "_" << buff;

        string << ".pcd";

        if (!filterIt(argc, argv, cloud, cloud_filtered))
            return 0;

        //cloud_filtered->is_dense = 0;


        // If ordered, remove the NaN pointsd
        if (0) {
            std::cout << "Cloud is organized and NaN points are now removed" << std::endl;
            std::vector<int> unused;
            cloud_filtered->is_dense = 0;
            pcl::removeNaNFromPointCloud (*cloud_filtered, *cloud_filtered, unused);
        }

        //pcl::io::savePCDFile<PointType>(string.str(), cloud_filtered.operator*());
        std::cerr << "Cloud points before filtering: " << cloud->points.size() << std::endl;
        std::cerr << "Cloud points after filtering: " << cloud_filtered->points.size() << std::endl;
        pcl::io::savePCDFileBinary<PointType>(string.str(), *cloud_filtered);
        std::cout << "Resulting cloud saved in " << string.str() << std::endl;

        std::stringstream command;
        command << "pcd_viewer_int " << argv[1] << " " << string.str();
        std::cout << "Displaying the result... " << argv[2] << " "<<argv[3];
        if (!system(command.str().data()))
            std::cout << "stop" << std::endl << std::endl;
        std::cout << "repeat? [y/n]" << std::endl;
        char c;
        cloud->clear();
        cloud = cloud_filtered;
        c=getchar();
        getchar();
        if (c != 'y')
            break;
    } while (1);
    return (0);
}
void
pcl::RadiusOutlierRemoval<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output)
{
  output.is_dense = true;
  // If fields x/y/z are not present, we cannot filter
  if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1)
  {
    PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.data.clear ();
    return;
  }

  if (search_radius_ == 0.0)
  {
    PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.data.clear ();
    return;
  }
  // Send the input dataset to the spatial locator
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg (*input_, *cloud);

  // Initialize the spatial locator
  if (!tree_)
  {
    if (cloud->isOrganized ())
      tree_.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
    else
      tree_.reset (new pcl::search::KdTree<pcl::PointXYZ> (false));
  }
  tree_->setInputCloud (cloud);

  // Allocate enough space to hold the results
  std::vector<int> nn_indices (indices_->size ());
  std::vector<float> nn_dists (indices_->size ());

  // Copy the common fields
  output.is_bigendian = input_->is_bigendian;
  output.point_step = input_->point_step;
  output.height = 1;

  output.data.resize (input_->width * input_->point_step); // reserve enough space
  removed_indices_->resize (input_->data.size ());

  int nr_p = 0;
  int nr_removed_p = 0;
  // Go over all the points and check which doesn't have enough neighbors
  for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
  {
    int k = tree_->radiusSearch ((*indices_)[cp], search_radius_, nn_indices, nn_dists);
    // Check if the number of neighbors is larger than the user imposed limit
    if (k < min_pts_radius_)
    {
      if (extract_removed_indices_)
      {
        (*removed_indices_)[nr_removed_p] = cp;
        nr_removed_p++;
      }
      continue;
    }

    memcpy (&output.data[nr_p * output.point_step], &input_->data[(*indices_)[cp] * output.point_step],
            output.point_step);
    nr_p++;
  }

  output.width = nr_p;
  output.height = 1;
  output.data.resize (output.width * output.point_step);
  output.row_step = output.point_step * output.width;

  removed_indices_->resize (nr_removed_p);
}
示例#11
0
int
main (int argc, char** argv)
{
  srand (time (NULL));

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  // Generate pointcloud data
  cloud->width = 1000;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

  for (size_t i = 0; i < cloud->points.size (); ++i)
  {
    cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
  }

  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

  kdtree.setInputCloud (cloud);

  pcl::PointXYZ searchPoint;

  searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
  searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
  searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);

  // K nearest neighbor search

  int K = 10;

  std::vector<int> pointIdxNKNSearch(K);
  std::vector<float> pointNKNSquaredDistance(K);

  std::cout << "K nearest neighbor search at (" << searchPoint.x 
            << " " << searchPoint.y 
            << " " << searchPoint.z
            << ") with K=" << K << std::endl;

  if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
  {
    for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
      std::cout << "    "  <<   cloud->points[ pointIdxNKNSearch[i] ].x 
                << " " << cloud->points[ pointIdxNKNSearch[i] ].y 
                << " " << cloud->points[ pointIdxNKNSearch[i] ].z 
                << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
  }

  // Neighbors within radius search

  std::vector<int> pointIdxRadiusSearch;
  std::vector<float> pointRadiusSquaredDistance;

  float radius = 256.0f * rand () / (RAND_MAX + 1.0f);

  std::cout << "Neighbors within radius search at (" << searchPoint.x 
            << " " << searchPoint.y 
            << " " << searchPoint.z
            << ") with radius=" << radius << std::endl;


  if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
  {
    for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
      std::cout << "    "  <<   cloud->points[ pointIdxRadiusSearch[i] ].x 
                << " " << cloud->points[ pointIdxRadiusSearch[i] ].y 
                << " " << cloud->points[ pointIdxRadiusSearch[i] ].z 
                << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
  }

  return 0;
}
void ImageProcessing::segEuclideanClusters(const char *filename) {
    // Read in the cloud data
    pcl::PCDReader reader;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
    reader.read(filename, *cloud);
    std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl; //*

    // Create the filtering object: downsample the dataset using a leaf size of 1cm
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    vg.setInputCloud(cloud);
    vg.setLeafSize(0.01f, 0.01f, 0.01f);
    vg.filter(*cloud_filtered);
    std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //*

    // Create the segmentation object for the planar model and set all the parameters
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ> ());
    pcl::PCDWriter writer;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(100);
    seg.setDistanceThreshold(0.02);

    int i = 0, nr_points = (int) cloud_filtered->points.size();
    while (cloud_filtered->points.size() > 0.3 * nr_points) {
        // 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;
        }

        // Extract the planar inliers from the input cloud
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud_filtered);
        extract.setIndices(inliers);
        extract.setNegative(false);

        // Get the points associated with the planar surface
        extract.filter(*cloud_plane);
        std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;

        // Remove the planar inliers, extract the rest
        extract.setNegative(true);
        extract.filter(*cloud_f);
        *cloud_filtered = *cloud_f;
    }

    // Creating the KdTree object for the search method of the extraction
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(cloud_filtered);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(0.02); // 2cm
    ec.setMinClusterSize(100);
    ec.setMaxClusterSize(25000);
    ec.setSearchMethod(tree);
    ec.setInputCloud(cloud_filtered);
    ec.extract(cluster_indices);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr all_clusters(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointXYZRGB aPoint;
    int j = 0;
    Color myColor;
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
        myColor = Color::random(); //one color for each cluster.
        //adding all points of one cluster
        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++) {
            cloud_cluster->points.push_back(cloud_filtered->points[*pit]); //*
            aPoint.x = cloud_cluster->points.back().x;
            aPoint.y = cloud_cluster->points.back().y;
            aPoint.z = cloud_cluster->points.back().z;
            aPoint.r = myColor.red;
            aPoint.g = myColor.green;
            aPoint.b = myColor.blue;
            all_clusters->points.push_back(aPoint);

        }
        cloud_cluster->width = cloud_cluster->points.size();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

        std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points." << std::endl;
        std::stringstream ss;
        ss << "cloud_cluster_" << j << ".pcd";

        j++;
    }

    //writer.write<pcl::PointXYZRGB> ("clustered_cloud.pcd", *cloud_cluster, false); //*

    pcl::visualization::CloudViewer viewer("Cluster viewer");
    viewer.showCloud(all_clusters);
    while (!viewer.wasStopped()) {
    }
}
void ImageProcessing::segRegionGrowing(const char *filename) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile(filename, *cloud) == -1) {
        std::cout << "Cloud reading failed." << std::endl;
        //return (-1);
    } else {
        std::cout << "Point cloud is loaded from " <<filename<< std::endl;
    }

    //visualize input cloud
    //visualizePointCloud(cloud);

    pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
    normal_estimator.setSearchMethod(tree);
    normal_estimator.setInputCloud(cloud);
    normal_estimator.setKSearch(50);
    normal_estimator.compute(*normals);

    pcl::IndicesPtr indices(new std::vector <int>);
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.0, 1.0);
    pass.filter(*indices);

    pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
    reg.setMinClusterSize(50);
    reg.setMaxClusterSize(1000000);
    reg.setSearchMethod(tree);
    reg.setNumberOfNeighbours(30);
    reg.setInputCloud(cloud);
    //reg.setIndices (indices);
    reg.setInputNormals(normals);
    reg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);
    reg.setCurvatureThreshold(1.0);

    std::vector <pcl::PointIndices> clusters;
    reg.extract(clusters);

    std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;
    std::cout << "First cluster has " << clusters[0].indices.size() << " points." << endl;
    std::cout << "These are the indices of the points of the initial" <<
            std::endl << "cloud that belong to the first cluster:" << std::endl;
    int counter = 0;
    //  while (counter < clusters[0].indices.size ())
    //  {
    //    std::cout << clusters[0].indices[counter] << ", ";
    //    counter++;
    //    if (counter % 10 == 0)
    //      std::cout << std::endl;
    //  }
    std::cout << std::endl;

    pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud();
    pcl::visualization::CloudViewer viewer("Cluster viewer");
    viewer.showCloud(colored_cloud);
    while (!viewer.wasStopped()) {
    }

    //visualize segmented cloud
    //visualizePointCloud(colored_cloud);

}
示例#14
0
int main (int argc, char** argv)
{
    if (argc != 3)
        return (0);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
    if (pcl::io::loadPLYFile (argv[1], *cloud) == -1)
        return (-1);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ> ());
    pcl::io::loadPLYFile(argv[2], *cloud2);
    pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
    feature_extractor.setInputCloud (cloud);
    feature_extractor.compute ();

    std::vector <float> moment_of_inertia;
    std::vector <float> eccentricity;
    pcl::PointXYZ min_point_AABB;
    pcl::PointXYZ max_point_AABB;
    pcl::PointXYZ min_point_OBB;
    pcl::PointXYZ max_point_OBB;
    pcl::PointXYZ position_OBB;
    Eigen::Matrix3f rotational_matrix_OBB;
    float major_value, middle_value, minor_value;
    Eigen::Vector3f major_vector, middle_vector, minor_vector;
    Eigen::Vector3f mass_center;

    feature_extractor.getMomentOfInertia (moment_of_inertia);
    feature_extractor.getEccentricity (eccentricity);
    feature_extractor.getAABB (min_point_AABB, max_point_AABB);
    feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
    feature_extractor.getEigenValues (major_value, middle_value, minor_value);
    feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
    feature_extractor.getMassCenter (mass_center);

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
    viewer->addCoordinateSystem (2.0, 0);
    viewer->initCameraParameters ();
    viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
    viewer->addPointCloud<pcl::PointXYZ> (cloud2, "object cloud");
    viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");

    Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
    Eigen::Quaternionf quat (rotational_matrix_OBB);
    viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");

    pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
    pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
    pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
    pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
    viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
    viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
    viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");

    while(!viewer->wasStopped())
    {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }

    return (0);
}
int main (int argc, char** argv)
{
    std::string fileName = argv[1];
    std::cout << "Reading " << fileName << std::endl;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

    if (pcl::io::loadPCDFile<pcl::PointXYZ> (fileName, *cloud) == -1) //* load the file
    {
        PCL_ERROR ("Couldn't read file");
        return (-1);
    }

    std::cout << "Loaded " << cloud->points.size() << " points." << std::endl;

    // Compute the normals
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
    normalEstimation.setInputCloud (cloud);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    normalEstimation.setSearchMethod (tree);

    pcl::PointCloud<pcl::Normal>::Ptr cloudWithNormals (new pcl::PointCloud<pcl::Normal>);

    normalEstimation.setRadiusSearch (0.03);

    normalEstimation.compute (*cloudWithNormals);

    std::cout << "Computed " << cloudWithNormals->points.size() << " normals." << std::endl;

    // Setup the feature computation
    typedef pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> VFHEstimationType;
    VFHEstimationType vfhEstimation;

    // Provide the original point cloud (without normals)
    vfhEstimation.setInputCloud (cloud);

    // Provide the point cloud with normals
    vfhEstimation.setInputNormals(cloudWithNormals);

    // Use the same KdTree from the normal estimation
    vfhEstimation.setSearchMethod (tree);

    //vfhEstimation.setRadiusSearch (0.2); // With this, error: "Both radius (.2) and K (1) defined! Set one of them to zero first and then re-run compute()"

    // Actually compute the VFH features
    pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhFeatures(new pcl::PointCloud<pcl::VFHSignature308>);
    vfhEstimation.compute (*vfhFeatures);

    std::cout << "output points.size (): " << vfhFeatures->points.size () << std::endl; // This outputs 1 - should be 397!

    // Display and retrieve the shape context descriptor vector for the 0th point.
    pcl::VFHSignature308 descriptor = vfhFeatures->points[0];
    VFHEstimationType::PointCloudOut::PointType descriptor2 = vfhFeatures->points[0];
    std::cout << "VFH:" << descriptor << std::endl;
    std::cout << "Numero de Elementos del VFH = " << sizeof(descriptor.histogram)/sizeof(descriptor.histogram[0]) << std::endl;


    // Create *_vfh.pcd file
    std::stringstream vfh_file;
    vfh_file << "# .PCD v.6 - Point Cloud Data file format" << std::endl;
    vfh_file << "FIELDS vfh" << std::endl;
    vfh_file << "SIZE 4" << std::endl;
    vfh_file << "TYPE F" << std::endl;
    vfh_file << "COUNT 308" << std::endl;
    vfh_file << "WIDTH 1" << std::endl;
    vfh_file << "HEIGHT 1" << std::endl;
    vfh_file << "POINTS 1" << std::endl;
    vfh_file << "DATA ascii" << std::endl;
    int vfh_length = sizeof(descriptor.histogram)/sizeof(descriptor.histogram[0]);
    for (int i = 0; i < vfh_length; i++)
    {
        vfh_file << descriptor.histogram[i] << " ";
    }

    std::ofstream outFile;
    outFile.open("Prueba_vfh.pcd");
    outFile << vfh_file.str();
    outFile.close();

    return 0;
}
示例#16
0
int main(int argc, char *argv[]) {
    if (argc != 2)
        return 0;
    
    Frame3D frames[8];
    
    for (int i = 0; i < 8; ++i) {
        frames[i].load(boost::str(boost::format("%s/%05d.3df") % argv[1] % i));
    }

    std::cout << "Merging point cloud" << std::endl;
    pcl::PointCloud<pcl::PointNormal>::Ptr model_point_cloud_norm = merge(frames);
    
    std::cout << "Got: " << model_point_cloud_norm->size() << " points" << std::endl;
    std::cout << "Generating mesh" << std::endl;

    pcl::PointCloud<pcl::PointNormal>::Ptr reduced_point_cloud(new pcl::PointCloud<pcl::PointNormal>());
    pcl::PassThrough<pcl::PointNormal> filter;

    filter.setInputCloud(model_point_cloud_norm);
    filter.filter(*reduced_point_cloud);
    std::cout << "Got: " << reduced_point_cloud->size() << " points" << std::endl;
    
    pcl::Poisson<pcl::PointNormal> rec;
    rec.setDepth(10);
    rec.setInputCloud(reduced_point_cloud);

    pcl::PolygonMesh triangles;
    rec.reconstruct(triangles);
    
    std::cout << "Finished" << std::endl;
    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::fromPCLPointCloud2(triangles.cloud, *cloud);
    
    for (int i = 0; i < 8; ++i) {
        float focal_length = frames[i].focal_length_;
        
        // Camera width
        double sizeX = frames[i].depth_image_.cols;
        // Camera height
        double sizeY = frames[i].depth_image_.rows;
        
        // Centers
        double cx = sizeX / 2.0;
        double cy = sizeY / 2.0;
        
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud = transformPointCloud(cloud, frames[i].getEigenTransform().inverse());
        
        const cv::Mat& zbuffer = computeZbuffer(*transformed_cloud, frames[i]);
        
        int point_found = 0;
        for (const pcl::Vertices& polygon : triangles.polygons) {
            const pcl::PointXYZRGB& point = transformed_cloud->at(polygon.vertices[0]);
            
            int u_unscaled = std::round(focal_length * (point.x / point.z) + cx);
            int v_unscaled = std::round(focal_length * (point.y / point.z) + cy);
            
            const cv::Vec3f& zmap_point = zbuffer.at<cv::Vec3f>(v_unscaled, u_unscaled);
            
            const float eps = 0.000000001;
            // If not visible
            if (std::fabs(zmap_point[0] - point.x) > eps
                    || std::fabs(zmap_point[1] - point.y) > eps
                    || std::fabs(zmap_point[2] - point.z) > eps)
                continue;

            float u = static_cast<float>(u_unscaled / sizeX);
            float v = static_cast<float>(v_unscaled / sizeY);
            
            if (u < 0. || u >= 1 || v < 0. || v >= 1)
                continue;
            
            int x = std::floor(frames[i].rgb_image_.cols * u);
            int y = std::floor(frames[i].rgb_image_.rows * v);
            
            for (int h = 0; h < 3; ++h) {
                pcl::PointXYZRGB& original_point = cloud->at(polygon.vertices[h]);
                const cv::Vec3b& rgb = frames[i].rgb_image_.at<cv::Vec3b>(y, x);
                if (original_point.r != 0 && original_point.g != 0 && original_point.b != 0)
                    continue;
                original_point.b = rgb[0];
                original_point.g = rgb[1];
                original_point.r = rgb[2];
            }
            
            ++point_found;
        }
        
        std::cout << point_found << " points found" << std::endl;
    }
    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr textured_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    textured_cloud->reserve(cloud->size());
    
    for (const pcl::PointXYZRGB& point : *cloud) {
        if (point.r != 0 || point.g != 0 || point.b != 0) {
            textured_cloud->push_back(point);
        }
    }
    
    // Filling gaps
    pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr tree2(new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
    tree2->setInputCloud(textured_cloud);
    for (pcl::PointXYZRGB& point : *cloud) {
        if (point.r != 0 || point.g != 0 || point.b != 0)
            continue;
        
        std::vector<int> k_indices;
        std::vector<float> k_dist;
        tree2->nearestKSearch(point, 1, k_indices, k_dist);
        
        const pcl::PointXYZRGB& textured_point = textured_cloud->at(k_indices[0]);
        point.r = textured_point.r;
        point.g = textured_point.g;
        point.b = textured_point.b;
    }
   
   // Smoothing
   tree2->setInputCloud(cloud);
   for (pcl::PointXYZRGB& point : *cloud) {
        if (point.r != 0 || point.g != 0 || point.b != 0)
            continue;
        
        std::vector<int> k_indices;
        std::vector<float> k_dist;
        tree2->nearestKSearch(point, 5, k_indices, k_dist);
        
        int r = 0;
        int g = 0;
        int b = 0;
        for (int i = 0; i < 5; ++i) {
            const pcl::PointXYZRGB& textured_point = textured_cloud->at(k_indices[i]);
            r += textured_point.r;
            g += textured_point.g;
            b += textured_point.b;
        }
        
        r /= 5;
        g /= 5;
        b /= 5;
        point.r = (uint8_t) r;
        point.g = (uint8_t) g;
        point.b = (uint8_t) b;
    }
    
    pcl::toPCLPointCloud2(*cloud, triangles.cloud);
    
    
    std::cout << "Finished texturing" << std::endl;
    
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(1, 1, 1);
    
    viewer->addPolygonMesh(triangles, "meshes", 0);
    
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();

    while (!viewer->wasStopped()) {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    return 0;
}
示例#17
0
PointViewSet PoissonFilter::run(PointViewPtr input)
{
    PointViewPtr output = input->makeNew();
    PointViewSet viewSet;
    viewSet.insert(output);

    bool logOutput = log()->getLevel() > LogLevel::Debug1;
    if (logOutput)
        log()->floatPrecision(8);

    log()->get(LogLevel::Debug2) << "Process PoissonFilter..." << std::endl;

    BOX3D buffer_bounds;
    input->calculateBounds(buffer_bounds);

    // convert PointView to PointNormal
    typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
    Cloud::Ptr cloud(new Cloud);
    pclsupport::PDALtoPCD(input, *cloud, buffer_bounds);

    pclsupport::setLogLevel(log()->getLevel());

    // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
    pcl::search::KdTree<pcl::PointNormal>::Ptr tree2;

    // Create search tree
    tree.reset(new pcl::search::KdTree<pcl::PointXYZ> (false));
    tree->setInputCloud(cloud);

    // Normal estimation
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal> ());
    n.setInputCloud(cloud);
    n.setSearchMethod(tree);
    n.setKSearch(20);
    n.compute(*normals);

    // Concatenate XYZ and normal information
    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);

    // Create search tree
    tree2.reset(new pcl::search::KdTree<pcl::PointNormal>);
    tree2->setInputCloud(cloud_with_normals);

    // initial setup
    pcl::Poisson<pcl::PointNormal> p;
    p.setInputCloud(cloud_with_normals);
    p.setSearchMethod(tree2);
    p.setDepth(m_depth);
    p.setPointWeight(m_point_weight);

    // create PointCloud for results
    pcl::PolygonMesh grid;
    p.reconstruct(grid);

    Cloud::Ptr cloud_f(new Cloud);
    pcl::fromPCLPointCloud2(grid.cloud, *cloud_f);

    if (cloud_f->points.empty())
    {
        log()->get(LogLevel::Debug2) << "Filtered cloud has no points!" << std::endl;
        return viewSet;
    }

    pclsupport::PCDtoPDAL(*cloud_f, output, buffer_bounds);

    log()->get(LogLevel::Debug2) << cloud->points.size() << " before, " <<
                                 cloud_f->points.size() << " after" << std::endl;
    log()->get(LogLevel::Debug2) << output->size() << std::endl;

    return viewSet;
}
 void PoseWithCovarianceStampedToGaussianPointCloud::convert(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
 {
   boost::mutex::scoped_lock lock(mutex_);
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
   Eigen::Vector2f mean;
   Eigen::Matrix2f S;
   if (cut_plane_ == "xy" || cut_plane_ == "flipped_xy") {
     mean = Eigen::Vector2f(msg->pose.pose.position.x, msg->pose.pose.position.y);
     S << msg->pose.covariance[0], msg->pose.covariance[1], 
       msg->pose.covariance[6], msg->pose.covariance[7];
   }
   else if (cut_plane_ == "yz" || cut_plane_ == "flipped_yz") {
     mean = Eigen::Vector2f(msg->pose.pose.position.y, msg->pose.pose.position.z);
     S << msg->pose.covariance[7], msg->pose.covariance[8], 
       msg->pose.covariance[13], msg->pose.covariance[14];
   }
   else if (cut_plane_ == "zx" || cut_plane_ == "flipped_zx") {
     mean = Eigen::Vector2f(msg->pose.pose.position.z, msg->pose.pose.position.x);
     S << msg->pose.covariance[14], msg->pose.covariance[12], 
       msg->pose.covariance[0], msg->pose.covariance[2];
   }
   double max_sigma = 0;
   for (size_t i = 0; i < 2; i++) {
     for (size_t j = 0; j < 2; j++) {
       double sigma = sqrt(S(i, j));
       if (max_sigma < sigma) {
         max_sigma = sigma;
       }
     }
   }
   Eigen::Matrix2f S_inv = S.inverse();
   double dx = 6.0 * max_sigma / sampling_num_;
   double dy = 6.0 * max_sigma / sampling_num_;
   for (double x = - 3.0 * max_sigma; x <= 3.0 * max_sigma; x += dx) {
     for (double y = - 3.0 * max_sigma; y <= 3.0 * max_sigma; y += dy) {
       Eigen::Vector2f diff(x, y);
       Eigen::Vector2f input = diff + mean;
       float z = gaussian(input, mean, S, S_inv);
       pcl::PointXYZ p;
       if (cut_plane_ == "xy") {
         p.x = input[0];
         p.y = input[1];
         p.z = z + msg->pose.pose.position.z;
       }
       else if (cut_plane_ == "yz") {
         p.y = input[0];
         p.z = input[1];
         p.x = z + msg->pose.pose.position.x;
       }
       else if (cut_plane_ == "zx") {
         p.z = input[0];
         p.x = input[1];
         p.y = z + msg->pose.pose.position.y;
       }
       else if (cut_plane_ == "flipped_xy") {
         p.x = input[0];
         p.y = input[1];
         p.z = -z + msg->pose.pose.position.z;
       }
       else if (cut_plane_ == "flipped_yz") {
         p.y = input[0];
         p.z = input[1];
         p.x = -z + msg->pose.pose.position.x;
       }
       else if (cut_plane_ == "flipped_zx") {
         p.z = input[0];
         p.x = input[1];
         p.y = -z + msg->pose.pose.position.y;
       }
       cloud->points.push_back(p);
     }
   }
   sensor_msgs::PointCloud2 ros_cloud;
   pcl::toROSMsg(*cloud, ros_cloud);
   ros_cloud.header = msg->header;
   pub_.publish(ros_cloud);
 }
示例#19
0
void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
{
	processingData_ = true;
	int quality = odom.info().inliers;

	bool lost = false;
	bool lostStateChanged = false;

	if(odom.pose().isNull())
	{
		UDEBUG("odom lost"); // use last pose
		lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
		imageView_->setBackgroundColor(Qt::darkRed);
		cloudView_->setBackgroundColor(Qt::darkRed);

		lost = true;
	}
	else if(odom.info().inliers>0 &&
			qualityWarningThr_ &&
			odom.info().inliers < qualityWarningThr_)
	{
		UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, qualityWarningThr_);
		lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
		imageView_->setBackgroundColor(Qt::darkYellow);
		cloudView_->setBackgroundColor(Qt::darkYellow);
	}
	else
	{
		UDEBUG("odom ok");
		lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
		imageView_->setBackgroundColor(cloudView_->getDefaultBackgroundColor());
		cloudView_->setBackgroundColor(Qt::black);
	}

	timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation));

	if(!odom.data().imageRaw().empty() &&
		!odom.data().depthOrRightRaw().empty() &&
		(odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size()))
	{
		UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality);

		if(!odom.data().depthRaw().empty())
		{
			if(odom.data().imageRaw().cols % decimationSpin_->value() == 0 &&
			   odom.data().imageRaw().rows % decimationSpin_->value() == 0)
			{
				validDecimationValue_ = decimationSpin_->value();
			}
			else
			{
				UWARN("Decimation (%d) must be a denominator of the width and height of "
						"the image (%d/%d). Using last valid decimation value (%d).",
						decimationSpin_->value(),
						odom.data().imageRaw().cols,
						odom.data().imageRaw().rows,
						validDecimationValue_);
			}
		}
		else
		{
			validDecimationValue_ = decimationSpin_->value();
		}

		// visualization: buffering the clouds
		// Create the new cloud
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
		cloud = util3d::cloudRGBFromSensorData(
				odom.data(),
				validDecimationValue_,
				0.0f,
				voxelSpin_->value());

		if(cloud->size())
		{
			if(!odom.pose().isNull())
			{
				if(cloudView_->getAddedClouds().contains("cloudtmp"))
				{
					cloudView_->removeCloud("cloudtmp");
				}

				while(maxCloudsSpin_->value()>0 && (int)addedClouds_.size() > maxCloudsSpin_->value())
				{
					UASSERT(cloudView_->removeCloud(addedClouds_.first()));
					addedClouds_.pop_front();
				}

				odom.data().id()?id_=odom.data().id():++id_;
				std::string cloudName = uFormat("cloud%d", id_);
				addedClouds_.push_back(cloudName);
				UASSERT(cloudView_->addCloud(cloudName, cloud, odom.pose()));
			}
			else
			{
				cloudView_->addOrUpdateCloud("cloudtmp", cloud, lastOdomPose_);
			}
		}
	}

	if(!odom.pose().isNull())
	{
		lastOdomPose_ = odom.pose();
		cloudView_->updateCameraTargetPosition(odom.pose());
	}

	if(odom.info().localMap.size())
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
		cloud->resize(odom.info().localMap.size());
		int i=0;
		for(std::multimap<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
		{
			(*cloud)[i].x = iter->second.x;
			(*cloud)[i].y = iter->second.y;
			(*cloud)[i++].z = iter->second.z;
		}
		cloudView_->addOrUpdateCloud("localmap", cloud);
	}

	if(!odom.data().imageRaw().empty())
	{
		if(odom.info().type == 0)
		{
			imageView_->setFeatures(odom.info().words, odom.data().depthRaw(), Qt::yellow);
		}
		else if(odom.info().type == 1)
		{
			std::vector<cv::KeyPoint> kpts;
			cv::KeyPoint::convert(odom.info().refCorners, kpts);
			imageView_->setFeatures(kpts, odom.data().depthRaw(), Qt::red);
		}

		imageView_->clearLines();
		if(lost)
		{
			if(lostStateChanged)
			{
				// save state
				odomImageShow_ = imageView_->isImageShown();
				odomImageDepthShow_ = imageView_->isImageDepthShown();
			}
			imageView_->setImageDepth(uCvMat2QImage(odom.data().imageRaw()));
			imageView_->setImageShown(true);
			imageView_->setImageDepthShown(true);
		}
		else
		{
			if(lostStateChanged)
			{
				// restore state
				imageView_->setImageShown(odomImageShow_);
				imageView_->setImageDepthShown(odomImageDepthShow_);
			}

			imageView_->setImage(uCvMat2QImage(odom.data().imageRaw()));
			if(imageView_->isImageDepthShown())
			{
				imageView_->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
			}

			if(odom.info().type == 0)
			{
				if(imageView_->isFeaturesShown())
				{
					for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
					{
						imageView_->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
					}
					for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
					{
						imageView_->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
					}
				}
			}
		}
		if(odom.info().type == 1 && odom.info().cornerInliers.size())
		{
			if(imageView_->isFeaturesShown() || imageView_->isLinesShown())
			{
				//draw lines
				UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
				for(unsigned int i=0; i<odom.info().cornerInliers.size(); ++i)
				{
					if(imageView_->isFeaturesShown())
					{
						imageView_->setFeatureColor(odom.info().cornerInliers[i], Qt::green); // inliers
					}
					if(imageView_->isLinesShown())
					{
						imageView_->addLine(
								odom.info().refCorners[odom.info().cornerInliers[i]].x,
								odom.info().refCorners[odom.info().cornerInliers[i]].y,
								odom.info().newCorners[odom.info().cornerInliers[i]].x,
								odom.info().newCorners[odom.info().cornerInliers[i]].y,
								Qt::blue);
					}
				}
			}
		}

		if(!odom.data().imageRaw().empty())
		{
			imageView_->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
		}
	}

	imageView_->update();
	cloudView_->update();
	QApplication::processEvents();
	processingData_ = false;
}
示例#20
0
NormalEstimator::NormalEstimator(Mat* m)
{
	/*
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  MatToPcl(*m, cloud);


  // Create the normal estimation class, and pass the input dataset to it
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud (cloud);

  // Create an empty kdtree representation, and pass it to the normal estimation object.
  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  ne.setSearchMethod (tree);

  // Output datasets
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

  // Use all neighbors in a sphere of radius 3cm
  ne.setRadiusSearch (0.03);

  // Compute the features
  ne.compute (*cloud_normals);

  // cloud_normals->points.size () should have the same size as the input cloud->points.size ()*

  result = PclToMat(NormsToPcl(cloud_normals));
  */



	// Load input file into a PointCloud<T> with an appropriate type
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
  MatToPcl(*m, cloud);

  // Create a KD-Tree
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);

  // Output has the PointNormal type in order to store the normals calculated by MLS
  pcl::PointCloud<pcl::PointNormal> mls_points;

  // Init object (second point type is for the normals, even if unused)
  pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;
 
  mls.setComputeNormals (true);

  // Set parameters
  mls.setInputCloud (cloud);
  mls.setPolynomialFit (true);
  mls.setSearchMethod (tree);
  mls.setSearchRadius (0.03);

  // Reconstruct
  mls.process (mls_points);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ>);
	for(int i=0; i < mls_points.points.size(); i++){
		cloud2->push_back(pcl::PointXYZ(mls_points.points[i].normal_x, mls_points.points[i].normal_y, mls_points.points[i].normal_z));
	}

  result = PclToMat(cloud2);
}
void
DoSampleRun2 ()
{
  cob_3d_mapping_filters::SpeckleFilter<PointXYZ> filter;
  pcl::PointCloud<PointXYZ>::Ptr cloud (new pcl::PointCloud<PointXYZ> ());
  pcl::PointCloud<PointXYZ>::Ptr cloud_out (new pcl::PointCloud<PointXYZ> ());
  cloud->width = 640;
  cloud->height = 480;
  double x = 0, y = 0;
  for (unsigned int i = 0; i < cloud->width; i++, y += 0.001)
  {
    x = 0;
    for (unsigned int j = 0; j < cloud->height; j++, x += 0.001)
    {
      PointXYZ pt;
      pt.x = x;
      pt.y = y;
      pt.z = 1;
      cloud->points.push_back (pt);
    }
  }
  boost::mt19937 rng; // I don't seed it on purpouse (it's not relevant)
  boost::normal_distribution<> nd (0.0, 0.05);
  boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);

  for (unsigned int i = 0; i < 3000; i++)
    cloud->points[i * 100].z += var_nor ();

  //pcl::io::savePCDFileASCII("/tmp/spk_cloud.pcd", *cloud);
  filter.setInputCloud (cloud);
  for (unsigned int s = 10; s <= 100; s += 10)
  {
    std::stringstream ss;
    ss << "/tmp/spk_acc_" << s << ".txt";
    std::ofstream file;
    file.open (ss.str ().c_str ());
    file << "thr\ttp\tfn\tfp\n";
    for (double c = 0.01; c <= 0.1; c += 0.01)
    {
      filter.setFilterParam (s, c);
      filter.filter (*cloud_out);
      pcl::PointIndices::Ptr ind = filter.getRemovedIndices ();
      std::cout << "Cloud size " << cloud_out->size () << ", ind: " << ind->indices.size () << std::endl;
      int fn_ctr = 0, tp_ctr = 0;
      for (unsigned int i = 0; i < 3000; i++)
      {
        bool found = false;
        for (unsigned int j = 0; j < ind->indices.size (); j++)
        {
          if (ind->indices[j] == i * 100)
          {
            tp_ctr++;
            found = true;
            break;
          }
        }
        if (!found)
          fn_ctr++;
      }
      int fp_ctr = ind->indices.size () - tp_ctr;
      double fn_ratio = (double)fn_ctr / 3000;
      double fp_ratio = (double)fp_ctr / 3000;
      double tp_ratio = (double)tp_ctr / 3000;
      file << c << "\t" << tp_ratio << "\t" << fn_ratio << "\t" << fp_ratio << "\n";
      std::cout << "c: " << c << " fn: " << fn_ratio << ", tp: " << tp_ratio << " fp: " << fp_ratio << std::endl;
    }
    file.close ();
  }
}
示例#22
0
int 
main (int argc, char** argv)
{
  // Read in the cloud data
  pcl::PCDReader reader;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
  reader.read ("table_scene_lms400.pcd", *cloud);
  std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*

  // Create the filtering object: downsample the dataset using a leaf size of 1cm
  pcl::VoxelGrid<pcl::PointXYZ> vg;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
  vg.setInputCloud (cloud);
  vg.setLeafSize (0.01f, 0.01f, 0.01f);
  vg.filter (*cloud_filtered);
  std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size ()  << " data points." << std::endl; //*

  // Create the segmentation object for the planar model and set all the parameters
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
  pcl::PCDWriter writer;
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (100);
  seg.setDistanceThreshold (0.02);

  int i=0, nr_points = (int) cloud_filtered->points.size ();
  while (cloud_filtered->points.size () > 0.3 * nr_points)
  {
    // 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;
    }

    // Extract the planar inliers from the input cloud
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers);
    extract.setNegative (false);

    // Write the planar inliers to disk
    extract.filter (*cloud_plane);
    std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;

    // Remove the planar inliers, extract the rest
    extract.setNegative (true);
    extract.filter (*cloud_f);
    *cloud_filtered = *cloud_f;
  }

  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud_filtered);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.02); // 2cm
  ec.setMinClusterSize (100);
  ec.setMaxClusterSize (25000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (cloud_filtered);
  ec.extract (cluster_indices);

  int j = 0;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
      cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
    cloud_cluster->width = cloud_cluster->points.size ();
    cloud_cluster->height = 1;
    cloud_cluster->is_dense = true;

    std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
    std::stringstream ss;
    ss << "cloud_cluster_" << j << ".pcd";
    writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
    j++;
  }

  return (0);
}
示例#23
0
int main(int argc, char** argv)
{
  double taubin_radius = 0.03; // radius of curvature-estimation neighborhood

  std::string file = "/home/andreas/data/mlaffordance/round21l_reg.pcd";
  PointCloud::Ptr cloud(new PointCloud);
  if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(file, *cloud) == -1) //* load the file
  {
    PCL_ERROR("Couldn't read input PCD file\n");
    return (-1);
  }

  pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
      new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
  std::vector<int> nn_outer_indices;
  std::vector<float> nn_outer_dists;
  pcl::KdTreeFLANN<pcl::PointXYZRGBA>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZRGBA>());

  int sample_index = 0;
  if (cloud->isOrganized())
  {
    organized_neighbor->setInputCloud(cloud);
    organized_neighbor->radiusSearch(cloud->points[sample_index], taubin_radius, nn_outer_indices, nn_outer_dists);
  }
  else
  {
    tree->setInputCloud(cloud);
    tree->radiusSearch(cloud->points[sample_index], taubin_radius, nn_outer_indices, nn_outer_dists);
  }

  Eigen::Matrix4d T_base, T_sqrt;
  T_base << 0, 0.445417, 0.895323, 0.22, 1, 0, 0, -0.02, 0, 0.895323, -0.445417, 0.24, 0, 0, 0, 1;

  T_sqrt << 0.9366, -0.0162, 0.3500, -0.2863, 0.0151, 0.9999, 0.0058, 0.0058, -0.3501, -0.0002, 0.9367, 0.0554, 0, 0, 0, 1;

  std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > T_cams;
  T_cams.push_back(T_base * T_sqrt.inverse());

  Eigen::Vector3d sample = cloud->points[sample_index].getVector3fMap().cast<double>();
  Quadric quadric(T_cams, cloud, sample, true);
  quadric.fitQuadric(nn_outer_indices);

  Eigen::MatrixXi cam_source = Eigen::MatrixXi::Zero(1, cloud->points.size());
  quadric.findTaubinNormalAxis(nn_outer_indices, cam_source);

  quadric.print();

  std::set<Eigen::Vector3i, VectorComparator> s;
  Eigen::Matrix3i M;
  M << 1,1,1,2,3,4,1,1,1;
  for (int i=0; i < M.rows(); i++)
    s.insert(M.row(i));
  std::cout << "M:" << std::endl;
  std::cout << M << std::endl;
  Eigen::Matrix<int, Eigen::Dynamic, 3> N(s.size(), 3);
  int i = 0;
  for (std::set<Eigen::Vector3i, VectorComparator>::iterator it=s.begin(); it!=s.end(); ++it)
  {
    N.row(i) = *it;
    i++;
  }
  std::cout << "N:" << std::endl;
  std::cout << N << std::endl;

  return 0;
}
示例#24
0
void FSModel::convertPointCloudToSurfaceMesh()
{
    // Load input file into a PointCloud<T> with an appropriate type
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    //sensor_msgs::PointCloud2 cloud_blob;
    //pcl::io::loadPCDFile ("bearHigh.pcd", cloud_blob);
    //pcl::fromROSMsg (cloud_blob, *cloud);
    //* the data should be available in cloud

    cloud->points.resize(pointCloud->size());
    for (size_t i = 0; i < pointCloud->points.size(); i++) {
        cloud->points[i].x = pointCloud->points[i].x;
        cloud->points[i].y = pointCloud->points[i].y;
        cloud->points[i].z = pointCloud->points[i].z;
    }

    // Normal estimation*
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (cloud);
    n.setInputCloud (cloud);
    n.setSearchMethod (tree);
    //n.setRadiusSearch(15.0);
    n.setKSearch (20);
    n.compute (*normals);
    //* normals should not contain the point normals + surface curvatures

    // Concatenate the XYZ and normal fields*
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
    pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
    //* cloud_with_normals = cloud + normals

    // Create search tree*
    pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
    tree2->setInputCloud (cloud_with_normals);

    // Initialize objects
    pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;

    // Set the maximum distance between connected points (maximum edge length)
    gp3.setSearchRadius (15.00);

    // Set typical values for the parameters
    gp3.setMu (2.5);
    gp3.setMaximumNearestNeighbors (100);
    gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
    gp3.setMinimumAngle(M_PI/18); // 10 degrees
    gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
    gp3.setNormalConsistency(false);

    // Get result
    gp3.setInputCloud (cloud_with_normals);
    gp3.setSearchMethod (tree2);
    gp3.reconstruct (triangles);

    // Additional vertex information
    std::vector<int> parts = gp3.getPartIDs();
    std::vector<int> states = gp3.getPointStates();

    pcl::io::savePLYFile("mesh.ply", triangles);
}
示例#25
0
void
pcl::StatisticalOutlierRemoval<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output)
{
  output.is_dense = true;
  // If fields x/y/z are not present, we cannot filter
  if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1)
  {
    PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.data.clear ();
    return;
  }

  if (std_mul_ == 0.0)
  {
    PCL_ERROR ("[pcl::%s::applyFilter] Standard deviation multipler not set!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.data.clear ();
    return;
  }
  // Send the input dataset to the spatial locator
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg (*input_, *cloud);

  // Initialize the spatial locator
  if (!tree_)
  {
    if (cloud->isOrganized ())
      tree_.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
    else
      tree_.reset (new pcl::search::KdTree<pcl::PointXYZ> (false));
  }

  tree_->setInputCloud (cloud);

  // Allocate enough space to hold the results
  std::vector<int> nn_indices (mean_k_);
  std::vector<float> nn_dists (mean_k_);

  std::vector<float> distances (indices_->size ());
  // Go over all the points and calculate the mean or smallest distance
  for (size_t cp = 0; cp < indices_->size (); ++cp)
  {
    if (!pcl_isfinite (cloud->points[(*indices_)[cp]].x) || !pcl_isfinite (cloud->points[(*indices_)[cp]].y)
        ||
        !pcl_isfinite (cloud->points[(*indices_)[cp]].z))
    {
      distances[cp] = 0;
      continue;
    }

    if (tree_->nearestKSearch ((*indices_)[cp], mean_k_, nn_indices, nn_dists) == 0)
    {
      distances[cp] = 0;
      PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
      continue;
    }

    // Minimum distance (if mean_k_ == 2) or mean distance
    double dist_sum = 0;
    for (int j = 1; j < mean_k_; ++j)
      dist_sum += sqrt (nn_dists[j]);
    distances[cp] = static_cast<float> (dist_sum / (mean_k_ - 1));
  }

  // Estimate the mean and the standard deviation of the distance vector
  double mean, stddev;
  getMeanStd (distances, mean, stddev);
  double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier

  // Copy the common fields
  output.is_bigendian = input_->is_bigendian;
  output.point_step = input_->point_step;
  output.height = 1;

  output.data.resize (indices_->size () * input_->point_step); // reserve enough space
  removed_indices_->resize (input_->data.size ());

  // Build a new cloud by neglecting outliers
  int nr_p = 0;
  int nr_removed_p = 0;
  for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
  {
    if (negative_)
    {
      if (distances[cp] <= distance_threshold)
      {
        if (extract_removed_indices_)
        {
          (*removed_indices_)[nr_removed_p] = cp;
          nr_removed_p++;
        }
        continue;
      }
    }
    else
    {
      if (distances[cp] > distance_threshold)
      {
        if (extract_removed_indices_)
        {
          (*removed_indices_)[nr_removed_p] = cp;
          nr_removed_p++;
        }
        continue;
      }
    }

    memcpy (&output.data[nr_p * output.point_step], &input_->data[(*indices_)[cp] * output.point_step],
            output.point_step);
    nr_p++;
  }
  output.width = nr_p;
  output.data.resize (output.width * output.point_step);
  output.row_step = output.point_step * output.width;

  removed_indices_->resize (nr_removed_p);
}
示例#26
0
文件: scan.cpp 项目: vsu91/mapping
int main(int argc, char** argv)
{

  if (argc < 5)
  {
    PCL_INFO ("Usage %s -input_dir /dir/with/models -output_dir /output/dir [options]\n", argv[0]);
    PCL_INFO (" * where options are:\n"
        "         -height <X>            : simulate scans with sensor mounted on height X\n"
        "         -noise_std <X>         : std of gaussian noise added to pointcloud. Default value 0.0001.\n"
        "         -distance <X>          : simulate scans with object located on a distance X. Can be used multiple times. Default value 4.\n"
        "         -tilt <X>              : tilt sensor for X degrees. X == 0 - sensor looks strait. X < 0 Sensor looks down. X > 0 Sensor looks up . Can be used multiple times. Default value -15.\n"
        "         -shift <X>             : shift object from the straight line. Can be used multiple times. Default value 0.\n"
        "         -num_views <X>         : how many times rotate the object in for every distance, tilt and shift. Default value 6.\n"

        "");
    return -1;
  }

  std::string input_dir, output_dir;
  double height = 1.5;
  double num_views = 6;
  double noise_std = 0.0001;
  std::vector<double> distances;
  std::vector<double> tilt;
  std::vector<double> shift;
  int full_model_n_points = 30000;

  pcl::console::parse_argument(argc, argv, "-input_dir", input_dir);
  pcl::console::parse_argument(argc, argv, "-output_dir", output_dir);
  pcl::console::parse_argument(argc, argv, "-num_views", num_views);
  pcl::console::parse_argument(argc, argv, "-height", height);
  pcl::console::parse_argument(argc, argv, "-noise_std", noise_std);
  pcl::console::parse_multiple_arguments(argc, argv, "-distance", distances);
  pcl::console::parse_multiple_arguments(argc, argv, "-tilt", tilt);
  pcl::console::parse_multiple_arguments(argc, argv, "-shift", shift);

  PCL_INFO("distances size: %d\n", distances.size());
  for (size_t i = 0; i < distances.size(); i++)
  {
    PCL_INFO("distance: %f\n", distances[i]);
  }

  // Set default values if user didn't provide any
  if (distances.size() == 0)
    distances.push_back(4);
  if (tilt.size() == 0)
    tilt.push_back(-15);
  if (shift.size() == 0)
    shift.push_back(0);

  // Check if input path exists
  boost::filesystem::path input_path(input_dir);
  if (!boost::filesystem::exists(input_path))
  {
    PCL_ERROR("Input directory doesnt exists.");
    return -1;
  }

  // Check if input path is a directory
  if (!boost::filesystem::is_directory(input_path))
  {
    PCL_ERROR("%s is not directory.", input_path.c_str());
    return -1;
  }

  // Check if output directory exists
  boost::filesystem::path output_path(output_dir);
  if (!boost::filesystem::exists(output_path) || !boost::filesystem::is_directory(output_path))
  {
    if (!boost::filesystem::create_directories(output_path))
    {
      PCL_ERROR ("Error creating directory %s.\n", output_path.c_str ());
      return -1;
    }
  }

  // Find all .vtk files in the input directory
  std::vector<std::string> files_to_process;
  PCL_INFO("Processing following files:\n");
  boost::filesystem::directory_iterator end_iter;
  for (boost::filesystem::directory_iterator iter(input_path); iter != end_iter; iter++)
  {
    boost::filesystem::path class_dir_path(*iter);
    for (boost::filesystem::directory_iterator iter2(class_dir_path); iter2 != end_iter; iter2++)
    {
      boost::filesystem::path file(*iter2);
      if (file.extension() == ".vtk")
      {
        files_to_process.push_back(file.c_str());
        PCL_INFO("\t%s\n", file.c_str());
      }
    }

  }

  // Check if there are any .vtk files to process
  if (files_to_process.size() == 0)
  {
    PCL_ERROR("Directory %s has no .vtk files.", input_path.c_str());
    return -1;
  }

  // Iterate over all files
  for (size_t i = 0; i < files_to_process.size(); i++)
  {
    vtkSmartPointer<vtkPolyData> model;
    vtkSmartPointer<vtkPolyDataReader> reader = vtkPolyDataReader::New();
    vtkSmartPointer<vtkTransform> transform = vtkTransform::New();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());

    // Compute output directory for this model
    std::vector<std::string> st;
    boost::split(st, files_to_process.at(i), boost::is_any_of("/"), boost::token_compress_on);
    std::string class_dirname = st.at(st.size() - 2);
    std::string dirname = st.at(st.size() - 1);
    dirname = dirname.substr(0, dirname.size() - 4);
    dirname = output_dir + class_dirname + "/" + dirname;

    // Check if output directory for this model exists. If not create
    boost::filesystem::path dirpath(dirname);
    if (!boost::filesystem::exists(dirpath))
    {
      if (!boost::filesystem::create_directories(dirpath))
      {
        PCL_ERROR ("Error creating directory %s.\n", dirpath.c_str ());
        return -1;
      }
    }

    // Load model from file
    reader->SetFileName(files_to_process.at(i).c_str());
    reader->Update();
    model = reader->GetOutput();
    PCL_INFO("Number of points %d\n",model->GetNumberOfPoints());

    // Coumpute bounds and center of the model
    double bounds[6];
    model->GetBounds(bounds);
    double min_z_value = bounds[4];

    double center[3];
    model->GetCenter(center);

    createFullModelPointcloud(model, full_model_n_points, *cloud);
    pcl::io::savePCDFile(dirname + "/full.pcd", *cloud);

    // Initialize PCLVisualizer. Add model to scene.
    pcl::visualization::PCLVisualizer viz;
    viz.initCameraParameters();
    viz.updateCamera();
    viz.setCameraPosition(0, 0, 0, 1, 0, 0, 0, 0, 1);
    viz.addModelFromPolyData(model, transform);
    viz.setRepresentationToSurfaceForAllActors();

    // Iterate over all shifts
    for (size_t shift_index = 0; shift_index < shift.size(); shift_index++)
    {

      // Iterate over all tilts
      for (size_t tilt_index = 0; tilt_index < tilt.size(); tilt_index++)
      {

        // Iterate over all distances
        for (size_t distance_index = 0; distance_index < distances.size(); distance_index++)
        {

          // Iterate over all angles
          double angle = 0;
          double angle_step = 360.0 / num_views;
          for (int angle_index = 0; angle_index < num_views; angle_index++)
          {

            // Set transformation with distance, shift, tilt and angle parameters.
            transform->Identity();
            transform->RotateY(tilt[tilt_index]);
            transform->Translate(distances[distance_index], shift[shift_index], -(height + min_z_value));
            transform->RotateZ(angle);

		/*		

            // Render pointcloud
            viz.renderView(640, 480, cloud);

            //Add noise
            addNoise(cloud, noise_std);

            // Compute new coordinates of the model center
            double new_center[3];
            transform->TransformPoint(center, new_center);

            // Shift origin of the poincloud to the model center and align with initial coordinate system.
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>());
            moveToNewCenterAndAlign(cloud, cloud_transformed, new_center, tilt[tilt_index]);

            // Compute file name for this pointcloud and save it
            std::stringstream ss;
            ss << dirname << "/rotation" << angle << "_distance" << distances[distance_index] << "_tilt"
                << tilt[tilt_index] << "_shift" << shift[shift_index] << ".pcd";
            pcl_INFO("Writing %d points to file %s\n", cloud->points.size(), ss.str().c_str());
            pcl::io::savePCDFile(ss.str(), *cloud_transformed);

		*/

            // increment angle by step
            angle += angle_step;
	
	

          }

        }
      }
    }

  }

  return 0;
}
int
 main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

  // Fill in the cloud data
  cloud->width  = 15;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

  // Generate the data
  for (size_t i = 0; i < cloud->points.size (); ++i)
  {
    cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].z = 1.0;
  }

  // Set a few outliers
  cloud->points[0].z = 2.0;
  cloud->points[3].z = -2.0;
  cloud->points[6].z = 4.0;

  std::cerr << "Point cloud data: " << cloud->points.size () << " points" << std::endl;
  for (size_t i = 0; i < cloud->points.size (); ++i)
    std::cerr << "    " << cloud->points[i].x << " "
                        << cloud->points[i].y << " "
                        << cloud->points[i].z << 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);
  seg.segment (*inliers, *coefficients);

  if (inliers->indices.size () == 0)
  {
    PCL_ERROR ("Could not estimate a planar model for the given dataset.");
    return (-1);
  }

  std::cerr << "Model coefficients: " << coefficients->values[0] << " " 
                                      << coefficients->values[1] << " "
                                      << coefficients->values[2] << " " 
                                      << coefficients->values[3] << std::endl;

  std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
  for (size_t i = 0; i < inliers->indices.size (); ++i)
    std::cerr << inliers->indices[i] << "    " << cloud->points[inliers->indices[i]].x << " "
                                               << cloud->points[inliers->indices[i]].y << " "
                                               << cloud->points[inliers->indices[i]].z << std::endl;

  return (0);
}
int main( int argc, char** argv )
{
  ros::init(argc, argv, "points_and_lines");
  ros::NodeHandle n;
  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);

  ros::Rate r(30);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);


  float f = 0.0;
  while (ros::ok())
  {
// %Tag(MARKER_INIT)%
    visualization_msgs::Marker points, line_strip, line_list;
    points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/my_frame";
    points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
    points.ns = line_strip.ns = line_list.ns = "points_and_lines";
    points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
    points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;
// %EndTag(MARKER_INIT)%

// %Tag(ID)%
    points.id = 0;
    line_strip.id = 1;
    line_list.id = 2;
// %EndTag(ID)%

// %Tag(TYPE)%
    points.type = visualization_msgs::Marker::POINTS;
    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
    line_list.type = visualization_msgs::Marker::LINE_LIST;
// %EndTag(TYPE)%

// %Tag(SCALE)%
    // POINTS markers use x and y scale for width/height respectively
    points.scale.x = 0.2;
    points.scale.y = 0.2;

    // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
    line_strip.scale.x = 0.1;
    line_list.scale.x = 0.1;
// %EndTag(SCALE)%

// %Tag(COLOR)%
    // Points are green
    points.color.g = 1.0f;
    points.color.a = 1.0;

    // Line strip is blue
    line_strip.color.b = 1.0;
    line_strip.color.a = 1.0;

    // Line list is red
    line_list.color.r = 1.0;
    line_list.color.a = 1.0;
// %EndTag(COLOR)%

// %Tag(HELIX)%
    // Create the vertices for the points and lines
    for (uint32_t i = 0; i < 100; ++i)
    {
      float y = 5 * sin(f + i / 100.0f * 2 * M_PI);
      float z = 5 * cos(f + i / 100.0f * 2 * M_PI);

      geometry_msgs::Point p;
      p.x = (int32_t)i - 50;
      p.y = y;
      p.z = z;

      points.points.push_back(p);
      line_strip.points.push_back(p);

      // The line list needs two points for each line
      line_list.points.push_back(p);
      p.z += 1.0;
      line_list.points.push_back(p);
    }
// %EndTag(HELIX)%

    marker_pub.publish(points);
    marker_pub.publish(line_strip);
    marker_pub.publish(line_list);

    r.sleep();

    f += 0.04;
  }
}
  void HintedHandleEstimator::estimate(
    const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
    const geometry_msgs::PointStampedConstPtr &point_msg)
  {
    boost::mutex::scoped_lock lock(mutex_);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
    pcl::PassThrough<pcl::PointXYZ> pass;
    int K = 1;
    std::vector<int> pointIdxNKNSearch(K);
    std::vector<float> pointNKNSquaredDistance(K);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZ>);

    pcl::fromROSMsg(*cloud_msg, *cloud);
    geometry_msgs::PointStamped transed_point;
    ros::Time now = ros::Time::now();
    try
      {
      listener_.waitForTransform(cloud->header.frame_id, point_msg->header.frame_id, now, ros::Duration(1.0));
      listener_.transformPoint(cloud->header.frame_id, now, *point_msg, point_msg->header.frame_id, transed_point);
    }
    catch(tf::TransformException ex)
      {
      JSK_ROS_ERROR("%s", ex.what());
      return;
    }
    pcl::PointXYZ searchPoint;
    searchPoint.x = transed_point.point.x;
    searchPoint.y = transed_point.point.y;
    searchPoint.z = transed_point.point.z;

    //remove too far cloud
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("x");
    pass.setFilterLimits(searchPoint.x - 3*handle.arm_w, searchPoint.x + 3*handle.arm_w);
    pass.filter(*cloud);
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("y");
    pass.setFilterLimits(searchPoint.y - 3*handle.arm_w, searchPoint.y + 3*handle.arm_w);
    pass.filter(*cloud);
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(searchPoint.z - 3*handle.arm_w, searchPoint.z + 3*handle.arm_w);
    pass.filter(*cloud);

    if(cloud->points.size() < 10){
      JSK_ROS_INFO("points are too small");
      return;
    }
    if(1){ //estimate_normal
      pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
      ne.setInputCloud(cloud);
      ne.setSearchMethod(kd_tree);
      ne.setRadiusSearch(0.02);
      ne.setViewPoint(0, 0, 0);
      ne.compute(*cloud_normals);
    }
    else{ //use normal of msg
      
    }
    if(! (kd_tree->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)){
      JSK_ROS_INFO("kdtree failed");
      return;
    }
    float x = cloud->points[pointIdxNKNSearch[0]].x;
    float y = cloud->points[pointIdxNKNSearch[0]].y;
    float z = cloud->points[pointIdxNKNSearch[0]].z;
    float v_x = cloud_normals->points[pointIdxNKNSearch[0]].normal_x;
    float v_y = cloud_normals->points[pointIdxNKNSearch[0]].normal_y;
    float v_z = cloud_normals->points[pointIdxNKNSearch[0]].normal_z;
    double theta = acos(v_x);
    // use normal for estimating handle direction
    tf::Quaternion normal(0, v_z/NORM(0, v_y, v_z) * cos(theta/2), -v_y/NORM(0, v_y, v_z) * cos(theta/2), sin(theta/2));
    tf::Quaternion final_quaternion = normal;
    double min_theta_index = 0;
    double min_width = 100;
    tf::Quaternion min_qua(0, 0, 0, 1);
    visualization_msgs::Marker debug_hand_marker;
    debug_hand_marker.header = cloud_msg->header;
    debug_hand_marker.ns = string("debug_grasp");
    debug_hand_marker.id = 0;
    debug_hand_marker.type = visualization_msgs::Marker::LINE_LIST;
    debug_hand_marker.pose.orientation.w = 1;
    debug_hand_marker.scale.x=0.003;
    tf::Matrix3x3 best_mat;
    //search 180 degree and calc the shortest direction
    for(double theta_=0; theta_<3.14/2; 
        theta_+=3.14/2/30){
      tf::Quaternion rotate_(sin(theta_), 0, 0, cos(theta_));
      tf::Quaternion temp_qua = normal * rotate_;
      tf::Matrix3x3 temp_mat(temp_qua);
      geometry_msgs::Pose pose_respected_to_tf;
      pose_respected_to_tf.position.x = x;
      pose_respected_to_tf.position.y = y;
      pose_respected_to_tf.position.z = z;
      pose_respected_to_tf.orientation.x = temp_qua.getX();
      pose_respected_to_tf.orientation.y = temp_qua.getY();
      pose_respected_to_tf.orientation.z = temp_qua.getZ();
      pose_respected_to_tf.orientation.w = temp_qua.getW();
      Eigen::Affine3d box_pose_respected_to_cloud_eigend;
      tf::poseMsgToEigen(pose_respected_to_tf, box_pose_respected_to_cloud_eigend);
      Eigen::Affine3d box_pose_respected_to_cloud_eigend_inversed
        = box_pose_respected_to_cloud_eigend.inverse();
      Eigen::Matrix4f box_pose_respected_to_cloud_eigen_inversed_matrixf;
      Eigen::Matrix4d box_pose_respected_to_cloud_eigen_inversed_matrixd
        = box_pose_respected_to_cloud_eigend_inversed.matrix();
      jsk_pcl_ros::convertMatrix4<Eigen::Matrix4d, Eigen::Matrix4f>(
                                                                    box_pose_respected_to_cloud_eigen_inversed_matrixd,
                                                                    box_pose_respected_to_cloud_eigen_inversed_matrixf);
      Eigen::Affine3f offset = Eigen::Affine3f(box_pose_respected_to_cloud_eigen_inversed_matrixf);
      pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
      pcl::transformPointCloud(*cloud, *output_cloud, offset);

      pcl::PassThrough<pcl::PointXYZ> pass;
      pcl::PointCloud<pcl::PointXYZ>::Ptr points_z(new pcl::PointCloud<pcl::PointXYZ>), points_yz(new pcl::PointCloud<pcl::PointXYZ>), points_xyz(new pcl::PointCloud<pcl::PointXYZ>);
      pass.setInputCloud(output_cloud);
      pass.setFilterFieldName("y");
      pass.setFilterLimits(-handle.arm_w*2, handle.arm_w*2);
      pass.filter(*points_z);
      pass.setInputCloud(points_z);
      pass.setFilterFieldName("z");
      pass.setFilterLimits(-handle.finger_d, handle.finger_d);
      pass.filter(*points_yz);
      pass.setInputCloud(points_yz);
      pass.setFilterFieldName("x");
      pass.setFilterLimits(-(handle.arm_l-handle.finger_l), handle.finger_l);
      pass.filter(*points_xyz);
      pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
      for(size_t index=0; index<points_xyz->size(); index++){
        points_xyz->points[index].x = points_xyz->points[index].z = 0;
      }
      if(points_xyz->points.size() == 0){JSK_ROS_INFO("points are empty");return;}
      kdtree.setInputCloud(points_xyz);
      std::vector<int> pointIdxRadiusSearch;
      std::vector<float> pointRadiusSquaredDistance;
      pcl::PointXYZ search_point_tree;
      search_point_tree.x=search_point_tree.y=search_point_tree.z=0;
      if( kdtree.radiusSearch(search_point_tree, 10, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
        double before_w=10, temp_w;
        for(size_t index = 0; index < pointIdxRadiusSearch.size(); ++index){
          temp_w =sqrt(pointRadiusSquaredDistance[index]);
          if(temp_w - before_w > handle.finger_w*2){
            break; // there are small space for finger
          }
          before_w=temp_w;
        }
        if(before_w < min_width){
          min_theta_index = theta_;
          min_width = before_w;
          min_qua = temp_qua;
          best_mat = temp_mat;
        }
        //for debug view
        geometry_msgs::Point temp_point;
        std_msgs::ColorRGBA temp_color;
        temp_color.r=0; temp_color.g=0; temp_color.b=1; temp_color.a=1;
        temp_point.x=x-temp_mat.getColumn(1)[0] * before_w;
        temp_point.y=y-temp_mat.getColumn(1)[1] * before_w;
        temp_point.z=z-temp_mat.getColumn(1)[2] * before_w;
        debug_hand_marker.points.push_back(temp_point);
        debug_hand_marker.colors.push_back(temp_color);
        temp_point.x+=2*temp_mat.getColumn(1)[0] * before_w;
        temp_point.y+=2*temp_mat.getColumn(1)[1] * before_w;
        temp_point.z+=2*temp_mat.getColumn(1)[2] * before_w;
        debug_hand_marker.points.push_back(temp_point);
        debug_hand_marker.colors.push_back(temp_color);
      }
    }
    geometry_msgs::PoseStamped handle_pose_stamped;
    handle_pose_stamped.header = cloud_msg->header;
    handle_pose_stamped.pose.position.x = x;
    handle_pose_stamped.pose.position.y = y;
    handle_pose_stamped.pose.position.z = z;
    handle_pose_stamped.pose.orientation.x = min_qua.getX();
    handle_pose_stamped.pose.orientation.y = min_qua.getY();
    handle_pose_stamped.pose.orientation.z = min_qua.getZ();
    handle_pose_stamped.pose.orientation.w = min_qua.getW();
    std_msgs::Float64 min_width_msg;
    min_width_msg.data = min_width;
    pub_pose_.publish(handle_pose_stamped);
    pub_debug_marker_.publish(debug_hand_marker);
    pub_debug_marker_array_.publish(make_handle_array(handle_pose_stamped, handle));
    jsk_recognition_msgs::SimpleHandle simple_handle;
    simple_handle.header = handle_pose_stamped.header;
    simple_handle.pose = handle_pose_stamped.pose;
    simple_handle.handle_width = min_width;
    pub_handle_.publish(simple_handle);
  }
int main(int argc, char** argv) {
	//check if number of arguments is proper
	if(argc!=3){
		help();
		exit(0);
	}

	// initialize PointClouds
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);

	if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) == -1) //* load the file
			{
		cerr << "Couldn't read file :" << argv[1] << "\n";
		return (-1);
	}

	std::vector<int> inliers;

	// created RandomSampleConsensus object and compute the appropriated model
	pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));

	//RANSAC for Line model
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<PointT>());
	if (pcl::console::find_argument(argc, argv, "-l") >= 0) {
		pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
		pcl::PointIndices::Ptr inliers_l(new pcl::PointIndices);
		// Create the segmentation object
		pcl::SACSegmentation<pcl::PointXYZ> seg;
		// Optional
		seg.setOptimizeCoefficients(true);
		// Mandatory
		seg.setModelType(pcl::SACMODEL_LINE);
		seg.setMethodType(pcl::SAC_RANSAC);
		seg.setDistanceThreshold(0.05);

		seg.setInputCloud(cloud);
		seg.segment(*inliers_l, *coefficients);
		// Write the line inliers to disk
		pcl::ExtractIndices<PointT> extract;
		extract.setInputCloud(cloud);
		extract.setIndices(inliers_l);
		extract.setNegative(false);

		extract.filter(*cloud_line);
	}


	if (pcl::console::find_argument(argc, argv, "-f") >= 0) {
		//RANSAC for Plane model
		pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
		ransac.setDistanceThreshold(.01);
		ransac.computeModel();
		ransac.getInliers(inliers);
	} else if (pcl::console::find_argument(argc, argv, "-sf") >= 0) {
		//RANSAC for Sphere model
		pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
		ransac.setDistanceThreshold(.01);
		ransac.computeModel();
		ransac.getInliers(inliers);
	}

	// copies all inliers of the model computed to another PointCloud
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);

	// creates the visualization object and adds either our orignial cloud or all of the inliers
	// depending on the command line arguments specified.
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	if (pcl::console::find_argument(argc, argv, "-f") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0) {
		viewer = simpleVis(final);
	} else if (pcl::console::find_argument(argc, argv, "-l") >= 0) {