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);
//.........这里部分代码省略.........

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;
//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    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 ();
          }
        }

    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;
//.........这里部分代码省略.........

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;
//.........这里部分代码省略.........

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);
}

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);
//.........这里部分代码省略.........

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_