template <typename PointT> inline void
pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
                  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
{
  min_pt.setConstant (FLT_MAX);
  max_pt.setConstant (-FLT_MAX);

  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    for (size_t i = 0; i < indices.size (); ++i)
    {
      pcl::Array4fMapConst pt = cloud.points[indices[i]].getArray4fMap ();
      min_pt = min_pt.array ().min (pt);
      max_pt = max_pt.array ().max (pt);
    }
  }
  // NaN or Inf values could exist => check for them
  else
  {
    for (size_t i = 0; i < indices.size (); ++i)
    {
      // Check if the point is invalid
      if (!pcl_isfinite (cloud.points[indices[i]].x) || 
          !pcl_isfinite (cloud.points[indices[i]].y) || 
          !pcl_isfinite (cloud.points[indices[i]].z))
        continue;
      pcl::Array4fMapConst pt = cloud.points[indices[i]].getArray4fMap ();
      min_pt = min_pt.array ().min (pt);
      max_pt = max_pt.array ().max (pt);
    }
  }
}

template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::getDistancesToModel (
      const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
  if (!normals_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::getDistancesToModel] No input dataset containing normals was given!\n");
    return;
  }

  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients))
  {
    distances.clear ();
    return;
  }

  // Obtain the plane normal
  Eigen::Vector4f coeff = model_coefficients;
  coeff[3] = 0;

  distances.resize (indices_->size ());

  // Iterate through the 3d points and calculate the distances from them to the plane
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the plane normal as the dot product
    // D = (P-A).N/|N|
    Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
    Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
    double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]);

    // Calculate the angular distance between the point normal and the plane normal
    double d_normal = fabs (getAngle3D (n, coeff));
    d_normal = (std::min) (d_normal, M_PI - d_normal);

    distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
  }
}

template <typename PointT> void
pcl::MaximumLikelihoodSampleConsensus<PointT>::getMinMax (
    const PointCloudConstPtr &cloud, 
    const boost::shared_ptr <std::vector<int> > &indices, 
    Eigen::Vector4f &min_p, 
    Eigen::Vector4f &max_p)
{
  min_p.setConstant (FLT_MAX);
  max_p.setConstant (-FLT_MAX);
  min_p[3] = max_p[3] = 0;

  for (size_t i = 0; i < indices->size (); ++i)
  {
    if (cloud->points[(*indices)[i]].x < min_p[0]) min_p[0] = cloud->points[(*indices)[i]].x;
    if (cloud->points[(*indices)[i]].y < min_p[1]) min_p[1] = cloud->points[(*indices)[i]].y;
    if (cloud->points[(*indices)[i]].z < min_p[2]) min_p[2] = cloud->points[(*indices)[i]].z;

    if (cloud->points[(*indices)[i]].x > max_p[0]) max_p[0] = cloud->points[(*indices)[i]].x;
    if (cloud->points[(*indices)[i]].y > max_p[1]) max_p[1] = cloud->points[(*indices)[i]].y;
    if (cloud->points[(*indices)[i]].z > max_p[2]) max_p[2] = cloud->points[(*indices)[i]].z;
  }
}

template <typename PointT, typename PointNT> int
pcl::SampleConsensusModelCylinder<PointT, PointNT>::countWithinDistance (
      const Eigen::VectorXf &model_coefficients, const double threshold)
{
  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients))
    return (0);

  int nr_p = 0;

  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  float ptdotdir = line_pt.dot (line_dir);
  float dirdotdir = 1.0f / line_dir.dot (line_dir);
  // Iterate through the 3d points and calculate the distances from them to the sphere
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Aproximate the distance from the point to the cylinder as the difference between
    // dist(point,cylinder_axis) and cylinder radius
    Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
    Eigen::Vector4f n  (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
    double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);

    // Calculate the point's projection on the cylinder axis
    float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
    Eigen::Vector4f pt_proj = line_pt + k * line_dir;
    Eigen::Vector4f dir = pt - pt_proj;
    dir.normalize ();

    // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
    double d_normal = fabs (getAngle3D (n, dir));
    d_normal = (std::min) (d_normal, M_PI - d_normal);

    if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
      nr_p++;
  }
  return (nr_p);
}

//TODO: move to semantics
void
PlaneExtraction::findClosestTable(std::vector<pcl::PointCloud<Point>, Eigen::aligned_allocator<pcl::PointCloud<Point> > >& v_cloud_hull,
                                  std::vector<pcl::ModelCoefficients>& v_coefficients_plane,
                                  Eigen::Vector3f& robot_pose,
                                  unsigned int& idx)
{
  std::vector<unsigned int> table_candidates;
  for(unsigned int i=0; i<v_cloud_hull.size(); i++)
  {
    //if(fabs(v_coefficients_plane[i].values[3])>0.5 && fabs(v_coefficients_plane[i].values[3])<1.2)
    table_candidates.push_back(i);
  }
  if(table_candidates.size()>0)
  {
    for(unsigned int i=0; i<table_candidates.size(); i++)
    {
      double d_min = 1000;
      double d = d_min;
      Eigen::Vector4f centroid;
      pcl::compute3DCentroid(v_cloud_hull[i], centroid);
      //for(unsigned int j=0; j<v_cloud_hull[i].size(); j++)
      //{
      //  Eigen::Vector3f p = v_cloud_hull[i].points[j].getVector3fMap();
      //  d += fabs((p-robot_pose).norm());
      //}
      //d /= v_cloud_hull[i].size();
      Eigen::Vector3f centroid3 = centroid.head(3);
      d = fabs((centroid3-robot_pose).norm());
      ROS_INFO("d: %f", d);
      if(d<d_min)
      {
        d_min = d;
        idx = table_candidates[i];
      }
    }
  }
}

geometry_msgs::Quaternion calc_quaternion_average( std::vector<geometry_msgs::Pose> pose_vector ){
	
	Eigen::Matrix4f averaging_matrix;
	Eigen::Vector4f quaternion;
	
	averaging_matrix.setZero();
	for( unsigned int sample_ii=0; sample_ii<pose_vector.size(); sample_ii++){
		quaternion(0) = pose_vector[sample_ii].orientation.x;
		quaternion(1) = pose_vector[sample_ii].orientation.y;
		quaternion(2) = pose_vector[sample_ii].orientation.z;
		quaternion(3) = pose_vector[sample_ii].orientation.w;
		averaging_matrix =  averaging_matrix + quaternion*quaternion.transpose();
	  }// for all samples
	 
	 Eigen::EigenSolver<Eigen::Matrix4f> ev_solver;
	 ev_solver.compute( averaging_matrix);
	 Eigen::Vector4cf eigen_values = ev_solver.eigenvalues();
	 float max_ev=eigen_values(0).real();
	 unsigned int max_ii = 0;
	 for( unsigned int ev_ii=1; ev_ii<4; ev_ii++){
		 if( eigen_values(ev_ii).real()>max_ev ){
			 max_ev = eigen_values(ev_ii).real();
			 max_ii=ev_ii;
		 }
	 }
	 
	 Eigen::Vector4f eigen_vector = ev_solver.eigenvectors().col(max_ii).real();
	 eigen_vector.normalize();
	  
	 geometry_msgs::Quaternion quaternion_msg;
	 quaternion_msg.x = eigen_vector(0);
	 quaternion_msg.y = eigen_vector(1);
	 quaternion_msg.z = eigen_vector(2);
	 quaternion_msg.w = eigen_vector(3);
	 return quaternion_msg;
	 
}

template <typename PointT, typename PointNT> int
pcl::SampleConsensusModelCone<PointT, PointNT>::countWithinDistance (
    const Eigen::VectorXf &model_coefficients, const double threshold)
{

  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients))
    return (0);

  int nr_p = 0;

  Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  float opening_angle = model_coefficients[6];

  float apexdotdir = apex.dot (axis_dir);
  float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
  // Iterate through the 3d points and calculate the distances from them to the cone
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
    Eigen::Vector4f n  (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);

    // Calculate the point's projection on the cone axis
    float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
    Eigen::Vector4f pt_proj = apex + k * axis_dir;

    // Calculate the direction of the point from center
    Eigen::Vector4f pp_pt_dir = pt - pt_proj;
    pp_pt_dir.normalize ();

    // Calculate the actual radius of the cone at the level of the projected point
    Eigen::Vector4f height = apex - pt_proj;
    double actual_cone_radius = tan(opening_angle) * height.norm ();
    height.normalize ();

    // Calculate the cones perfect normals
    Eigen::Vector4f cone_normal = sinf (opening_angle) * height + cosf (opening_angle) * pp_pt_dir;

    // Aproximate the distance from the point to the cone as the difference between
    // dist(point,cone_axis) and actual cone radius
    double d_euclid = fabs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);

    // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
    double d_normal = fabs (getAngle3D (n, cone_normal));
    d_normal = (std::min) (d_normal, M_PI - d_normal);

    if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
      nr_p++;
  }
  return (nr_p);
}

template <typename PointInT> double 
pcl::tracking::NormalCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target)
{
    Eigen::Vector4f n = source.getNormalVector4fMap ();
    Eigen::Vector4f n_dash = target.getNormalVector4fMap ();
    if ( n.norm () <= 1e-5 || n_dash.norm () <= 1e-5 )
    {
        PCL_ERROR("norm might be ZERO!\n");
        std::cout << "source: " << source << std::endl;
        std::cout << "target: " << target << std::endl;
        exit (1);
        return 0.0;
    }
    else
    {
        n.normalize ();
        n_dash.normalize ();
        double theta = pcl::getAngle3D (n, n_dash);
        if (!pcl_isnan (theta))
            return 1.0 / (1.0 + weight_ * theta * theta);
        else
            return 0.0;
    }
}

vtkSmartPointer<vtkDataSet>
pcl::visualization::createLine (const Eigen::Vector4f &pt1, const Eigen::Vector4f &pt2)
{
  vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
  line->SetPoint1 (pt1.x (), pt1.y (), pt1.z ());
  line->SetPoint2 (pt2.x (), pt2.y (), pt2.z ());
  line->Update ();

  return (line->GetOutput ());
}

bool
pcl::lineWithLineIntersection (const Eigen::VectorXf &line_a, 
                               const Eigen::VectorXf &line_b, 
                               Eigen::Vector4f &point, double sqr_eps)
{
  Eigen::Vector4f p1, p2;
  lineToLineSegment (line_a, line_b, p1, p2);

  // If the segment size is smaller than a pre-given epsilon...
  double sqr_dist = (p1 - p2).squaredNorm ();
  if (sqr_dist < sqr_eps)
  {
    point = p1;
    return (true);
  }
  point.setZero ();
  return (false);
}

template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::doSamplesVerifyModel (
      const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
{
  // Needs a valid model coefficients
  if (model_coefficients.size () != 7)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
    return (false);
  }

  Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  float openning_angle = model_coefficients[6];

  float apexdotdir = apex.dot (axis_dir);
  float dirdotdir = 1.0f / axis_dir.dot (axis_dir);

  // Iterate through the 3d points and calculate the distances from them to the cone
  for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
  {
    Eigen::Vector4f pt (input_->points[*it].x, input_->points[*it].y, input_->points[*it].z, 0);

    // Calculate the point's projection on the cone axis
    float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
    Eigen::Vector4f pt_proj = apex + k * axis_dir;
    Eigen::Vector4f dir = pt - pt_proj;
    dir.normalize ();

    // Calculate the actual radius of the cone at the level of the projected point
    Eigen::Vector4f height = apex - pt_proj;
    double actual_cone_radius = tan (openning_angle) * height.norm ();

    // Aproximate the distance from the point to the cone as the difference between
    // dist(point,cone_axis) and actual cone radius
    if (fabs (static_cast<double>(pointToAxisDistance (pt, model_coefficients) - actual_cone_radius)) > threshold)
      return (false);
  }

  return (true);
}

template <typename PointT> inline unsigned int
pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &centroid)
{
  if (cloud.points.empty ())
    return (0);

  // Initialize to 0
  centroid.setZero ();
  // For each point in the cloud
  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    for (size_t i = 0; i < cloud.points.size (); ++i)
      centroid += cloud.points[i].getVector4fMap ();
    centroid[3] = 0;
    centroid /= static_cast<float> (cloud.points.size ());

    return (static_cast<unsigned int> (cloud.points.size ()));
  }
  // NaN or Inf values could exist => check for them
  else
  {
    unsigned cp = 0;
    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
      // Check if the point is invalid
      if (!isFinite (cloud [i]))
        continue;

      centroid += cloud[i].getVector4fMap ();
      ++cp;
    }
    centroid[3] = 0;
    centroid /= static_cast<float> (cp);

    return (cp);
  }
}

template <typename PointT> inline void
pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
                        Eigen::Vector4f &centroid)
{
  // Initialize to 0
  centroid.setZero ();
  if (indices.empty ()) 
    return;
  // For each point in the cloud
  int cp = 0;

  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    for (size_t i = 0; i < indices.size (); ++i)
      centroid += cloud.points[indices[i]].getVector4fMap ();
    centroid[3] = 0;
    centroid /= indices.size ();
  }
  // NaN or Inf values could exist => check for them
  else
  {
    for (size_t i = 0; i < indices.size (); ++i)
    {
      // Check if the point is invalid
      if (!pcl_isfinite (cloud.points[indices[i]].x) || 
          !pcl_isfinite (cloud.points[indices[i]].y) || 
          !pcl_isfinite (cloud.points[indices[i]].z))
        continue;

      centroid += cloud.points[indices[i]].getVector4fMap ();
      cp++;
    }
    centroid[3] = 0;
    centroid /= cp;
  }
}

template<typename PointType> void
pcl::apps::DominantPlaneSegmentation<PointType>::compute_table_plane ()
{
  // Has the input dataset been set already?
  if (!input_)
  {
    PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
    return;
  }

  CloudConstPtr cloud_;
  CloudPtr cloud_filtered_ (new Cloud ());
  CloudPtr cloud_downsampled_ (new Cloud ());
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
  pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
  pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
  CloudPtr table_projected_ (new Cloud ());
  CloudPtr table_hull_ (new Cloud ());

  typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);

  // Normal estimation parameters
  n3d_.setKSearch (k_);
  n3d_.setSearchMethod (normals_tree_);

  // Table model fitting parameters
  seg_.setDistanceThreshold (sac_distance_threshold_);
  seg_.setMaxIterations (2000);
  seg_.setNormalDistanceWeight (0.1);
  seg_.setOptimizeCoefficients (true);
  seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  seg_.setMethodType (pcl::SAC_RANSAC);
  seg_.setProbability (0.99);

  proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);

  // ---[ PassThroughFilter
  pass_.setFilterLimits (min_z_bounds_, max_z_bounds_);
  pass_.setFilterFieldName ("z");
  pass_.setInputCloud (input_);
  pass_.filter (*cloud_filtered_);

  if (int (cloud_filtered_->points.size ()) < k_)
  {
    PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %zu points! Aborting.",
        cloud_filtered_->points.size ());
    return;
  }

  // Downsample the point cloud
  grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
  grid_.setDownsampleAllData (false);
  grid_.setInputCloud (cloud_filtered_);
  grid_.filter (*cloud_downsampled_);

  // ---[ Estimate the point normals
  n3d_.setInputCloud (cloud_downsampled_);
  n3d_.compute (*cloud_normals_);

  // ---[ Perform segmentation
  seg_.setInputCloud (cloud_downsampled_);
  seg_.setInputNormals (cloud_normals_);
  seg_.segment (*table_inliers_, *table_coefficients_);

  if (table_inliers_->indices.size () == 0)
  {
    PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
    return;
  }

  // ---[ Extract the plane
  proj_.setInputCloud (cloud_downsampled_);
  proj_.setIndices (table_inliers_);
  proj_.setModelCoefficients (table_coefficients_);
  proj_.filter (*table_projected_);

  // ---[ Estimate the convex hull
  std::vector<pcl::Vertices> polygons;
  CloudPtr table_hull (new Cloud ());
  hull_.setInputCloud (table_projected_);
  hull_.reconstruct (*table_hull, polygons);

  // Compute the plane coefficients
  Eigen::Vector4f model_coefficients;
  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;

  model_coefficients[0] = table_coefficients_->values[0];
  model_coefficients[1] = table_coefficients_->values[1];
  model_coefficients[2] = table_coefficients_->values[2];
  model_coefficients[3] = table_coefficients_->values[3];

  // Need to flip the plane normal towards the viewpoint
  Eigen::Vector4f vp (0, 0, 0, 0);
  // See if we need to flip any plane normals
  vp -= table_hull->points[0].getVector4fMap ();
  vp[3] = 0;
  // Dot product between the (viewpoint - point) and the plane normal
  float cos_theta = vp.dot (model_coefficients);
  // Flip the plane normal
//.........这里部分代码省略.........

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // ... do data processing
  sensor_msgs::PointCloud2 output;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg (*input, *cloud);

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

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.10); // 2cm
  ec.setMinClusterSize (20);
  ec.setMaxClusterSize (2500);
  ec.setSearchMethod (tree);
  ec.setInputCloud(cloud);
  ec.extract (cluster_indices);
  
  ROS_INFO("cluster_indices has %d data points.", (int) cluster_indices.size());
  ROS_INFO("cloud has %d data points.", (int) cloud->points.size());

  visualization_msgs::Marker marker;
  marker.header   = cloud->header;
  marker.id       = 1;
  marker.type     = visualization_msgs::Marker::CUBE_LIST;
  marker.action   = visualization_msgs::Marker::ADD;
  marker.color.r  = 1;
  marker.color.g  = 0;
  marker.color.b  = 0;
  marker.color.a  = 0.7;
  marker.scale.x  = 0.2;
  marker.scale.y  = 0.2;
  marker.scale.z  = 0.2;
  marker.lifetime = ros::Duration(60.0);
  Eigen::Vector4f minPoint;
  Eigen::Vector4f maxPoint;
//  pcl::getMinMax3D(*cloud, minPoint, maxPoint);

  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->points[*pit]); 

    std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
    // Merge current clusters to whole point cloud
    *clustered_cloud += *cloud_cluster;
 //   for(size_t j = 0; j < cloud_cluster->points.size() - 1; j++)
 //   {
      /*
      geometry_msgs::Point pt1;
      pt1.x = cloud_cluster->points[j].x;
      pt1.y = cloud_cluster->points[j].y;
      pt1.z = cloud_cluster->points[j].z;
      geometry_msgs::Point pt2;
      pt2.x = cloud_cluster->points[j+1].x;
      pt2.y = cloud_cluster->points[j+1].y;
      pt2.z = cloud_cluster->points[j+1].z;

      marker.points.push_back(pt1);
      marker.points.push_back(pt2);
      */
      //Seg for top of prism
      geometry_msgs::Point pt3;      
      pt3.x = 0.0;
      pt3.y = 0.0;
      pt3.z = 0.0;
      std_msgs::ColorRGBA colors;
      colors.r = 0.0;
      colors.g = 0.0;
      colors.b = 0.0;
      for(size_t i=0; i<cloud_cluster->points.size(); i++)
      {
        pt3.x += cloud_cluster->points[i].x;
        pt3.y += cloud_cluster->points[i].y;
        pt3.z += cloud_cluster->points[i].z;
      }
      pt3.x /= cloud_cluster->points.size();
      pt3.y /= cloud_cluster->points.size();
      pt3.z /= cloud_cluster->points.size();
      pcl::getMinMax3D(*cloud_cluster, minPoint, maxPoint);
      marker.scale.y= maxPoint.y();
      //marker.scale.x= maxPoint.x();
      //marker.scale.z= maxPoint.z();
      marker.scale.x= maxPoint.x()-minPoint.x();
      colors.r = marker.scale.x;
//      colors.g = marker.scale.y;
      //marker.scale.z= maxPoint.z()-minPoint.z();
      //pt3.z = maxPoint.z();

      //geometry_msgs::Point pt4;
      //pt4.x = cloud_cluster->points[j+1].x;
      //pt4.y = cloud_cluster->points[j+1].y;
      //pt4.z = cloud_cluster->points[j+1].z;
      //pt4.z = maxPoint.z();

//.........这里部分代码省略.........

template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
  // ---[ Step 1a : compute the centroid in XYZ space
  Eigen::Vector4f xyz_centroid;

  if (use_given_centroid_) 
    xyz_centroid = centroid_to_use_;
  else
    compute3DCentroid (*surface_, *indices_, xyz_centroid);          // Estimate the XYZ centroid

  // ---[ Step 1b : compute the centroid in normal space
  Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
  int cp = 0;

  // If the data is dense, we don't need to check for NaN

  if (use_given_normal_)
    normal_centroid = normal_to_use_;
  else
  {
    if (normals_->is_dense)
    {
      for (size_t i = 0; i < indices_->size (); ++i)
      {
        normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
        cp++;
      }
    }
    // NaN or Inf values could exist => check for them
    else
    {
      for (size_t i = 0; i < indices_->size (); ++i)
      {
        if (!pcl_isfinite (normals_->points[(*indices_)[i]].normal[0])
            ||
            !pcl_isfinite (normals_->points[(*indices_)[i]].normal[1])
            ||
            !pcl_isfinite (normals_->points[(*indices_)[i]].normal[2]))
          continue;
        normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
        cp++;
      }
    }
    normal_centroid /= cp;
  }

  // Compute the direction of view from the viewpoint to the centroid
  Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0);
  Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid;
  d_vp_p.normalize ();

  // Estimate the SPFH at nn_indices[0] using the entire cloud
  computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_);

  // We only output _1_ signature
  output.points.resize (1);
  output.width = 1;
  output.height = 1;

  // Estimate the FPFH at nn_indices[0] using the entire cloud and copy the resultant signature
  for (int d = 0; d < hist_f1_.size (); ++d)
    output.points[0].histogram[d + 0] = hist_f1_[d];

  size_t data_size = hist_f1_.size ();
  for (int d = 0; d < hist_f2_.size (); ++d)
    output.points[0].histogram[d + data_size] = hist_f2_[d];

  data_size += hist_f2_.size ();
  for (int d = 0; d < hist_f3_.size (); ++d)
    output.points[0].histogram[d + data_size] = hist_f3_[d];

  data_size += hist_f3_.size ();
  for (int d = 0; d < hist_f4_.size (); ++d)
    output.points[0].histogram[d + data_size] = hist_f4_[d];

  // ---[ Step 2 : obtain the viewpoint component
  hist_vp_.setZero (nr_bins_vp_);

  double hist_incr;
  if (normalize_bins_)
    hist_incr = 100.0 / (double)(indices_->size ());
  else
    hist_incr = 1.0;

  for (size_t i = 0; i < indices_->size (); ++i)
  {
    Eigen::Vector4f normal (normals_->points[(*indices_)[i]].normal[0],
                            normals_->points[(*indices_)[i]].normal[1],
                            normals_->points[(*indices_)[i]].normal[2], 0);
    // Normalize
    double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
    int fi = floor (alpha * hist_vp_.size ());
    if (fi < 0)
      fi = 0;
    if (fi > ((int)hist_vp_.size () - 1))
      fi = hist_vp_.size () - 1;
    // Bin into the histogram
    hist_vp_ [fi] += hist_incr;
  }
//.........这里部分代码省略.........

template<typename PointT, typename PointNT, typename PointLT> void
pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients, 
                                                                         std::vector<PointIndices>& inlier_indices,
                                                                         std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
                                                                         std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
                                                                         pcl::PointCloud<PointLT>& labels,
                                                                         std::vector<pcl::PointIndices>& label_indices)
{
  if (!initCompute ())
    return;

  // Check that we got the same number of points and normals
  if (static_cast<int> (normals_->points.size ()) != static_cast<int> (input_->points.size ()))
  {
    PCL_ERROR ("[pcl::%s::segment] Number of points in input cloud (%zu) and normal cloud (%zu) do not match!\n",
               getClassName ().c_str (), input_->points.size (),
               normals_->points.size ());
    return;
  }

  // Check that the cloud is organized
  if (!input_->isOrganized ())
  {
    PCL_ERROR ("[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n",
               getClassName ().c_str ());
    return;
  }

  // Calculate range part of planes' hessian normal form
  std::vector<float> plane_d (input_->points.size ());
  
  for (unsigned int i = 0; i < input_->size (); ++i)
    plane_d[i] = input_->points[i].getVector3fMap ().dot (normals_->points[i].getNormalVector3fMap ());
  
  // Make a comparator
  //PlaneCoefficientComparator<PointT,PointNT> plane_comparator (plane_d);
  compare_->setPlaneCoeffD (plane_d);
  compare_->setInputCloud (input_);
  compare_->setInputNormals (normals_);
  compare_->setAngularThreshold (static_cast<float> (angular_threshold_));
  compare_->setDistanceThreshold (static_cast<float> (distance_threshold_), true);

  // Set up the output
  OrganizedConnectedComponentSegmentation<PointT,pcl::Label> connected_component (compare_);
  connected_component.setInputCloud (input_);
  connected_component.segment (labels, label_indices);

  Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero ();
  Eigen::Vector4f vp = Eigen::Vector4f::Zero ();
  Eigen::Matrix3f clust_cov;
  pcl::ModelCoefficients model;
  model.values.resize (4);

  // Fit Planes to each cluster
  for (size_t i = 0; i < label_indices.size (); i++)
  {
    if (static_cast<unsigned> (label_indices[i].indices.size ()) > min_inliers_)
    {
      pcl::computeMeanAndCovarianceMatrix (*input_, label_indices[i].indices, clust_cov, clust_centroid);
      Eigen::Vector4f plane_params;
      
      EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
      EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
      pcl::eigen33 (clust_cov, eigen_value, eigen_vector);
      plane_params[0] = eigen_vector[0];
      plane_params[1] = eigen_vector[1];
      plane_params[2] = eigen_vector[2];
      plane_params[3] = 0;
      plane_params[3] = -1 * plane_params.dot (clust_centroid);

      vp -= clust_centroid;
      float cos_theta = vp.dot (plane_params);
      if (cos_theta < 0)
      {
        plane_params *= -1;
        plane_params[3] = 0;
        plane_params[3] = -1 * plane_params.dot (clust_centroid);
      }
      
      // Compute the curvature surface change
      float curvature;
      float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8);
      if (eig_sum != 0)
        curvature = fabsf (eigen_value / eig_sum);
      else
        curvature = 0;

      if (curvature < maximum_curvature_)
      {
        model.values[0] = plane_params[0];
        model.values[1] = plane_params[1];
        model.values[2] = plane_params[2];
        model.values[3] = plane_params[3];
        model_coefficients.push_back (model);
        inlier_indices.push_back (label_indices[i]);
        centroids.push_back (clust_centroid);
        covariances.push_back (clust_cov);
      }
    }
  }
//.........这里部分代码省略.........

bool checkCloud(const sensor_msgs::PointCloud2& cloud_msg,
                typename pcl::PointCloud<T>::Ptr hand_cloud,
                //typename pcl::PointCloud<T>::Ptr finger_cloud,
                const std::string& frame_id,
                tf::Vector3& hand_position,
                tf::Vector3& arm_position,
                tf::Vector3& arm_direction)
{  
  typename pcl::PointCloud<T>::Ptr cloud(new pcl::PointCloud<T>);
  pcl::fromROSMsg(cloud_msg, *cloud);

  if((cloud->points.size() < g_config.min_cluster_size) ||
     (cloud->points.size() > g_config.max_cluster_size))
    return false;

  pcl::PCA<T> pca;
  pca.setInputCloud(cloud);
  Eigen::Vector4f mean = pca.getMean();

  if((mean.coeff(0) < g_config.min_x) || (mean.coeff(0) > g_config.max_x)) return false;
  if((mean.coeff(1) < g_config.min_y) || (mean.coeff(1) > g_config.max_y)) return false;
  if((mean.coeff(2) < g_config.min_z) || (mean.coeff(2) > g_config.max_z)) return false;

  Eigen::Vector3f eigen_value = pca.getEigenValues();
  double ratio = eigen_value.coeff(0) / eigen_value.coeff(1);

  if((ratio < g_config.min_eigen_value_ratio) || (ratio > g_config.max_eigen_value_ratio)) return false;

  T search_point;
  Eigen::Matrix3f ev = pca.getEigenVectors();
  Eigen::Vector3f main_axis(ev.coeff(0, 0), ev.coeff(1, 0), ev.coeff(2, 0));
  main_axis.normalize();
  arm_direction.setX(main_axis.coeff(0));
  arm_direction.setY(main_axis.coeff(1));
  arm_direction.setZ(main_axis.coeff(2));
  arm_position.setX(mean.coeff(0));
  arm_position.setY(mean.coeff(1));
  arm_position.setZ(mean.coeff(2));

  main_axis = (-main_axis * 0.3) + Eigen::Vector3f(mean.coeff(0), mean.coeff(1), mean.coeff(2));
  search_point.x = main_axis.coeff(0);
  search_point.y = main_axis.coeff(1);
  search_point.z = main_axis.coeff(2);



  //find hand
  pcl::KdTreeFLANN<T> kdtree;
  kdtree.setInputCloud(cloud);

  //find the closet point from the serach_point
  std::vector<int> point_indeices(1);
  std::vector<float> point_distances(1);
  if ( kdtree.nearestKSearch (search_point, 1, point_indeices, point_distances) > 0 )
  {
    //update search point
    search_point = cloud->points[point_indeices[0]];

    //show seach point
    if(g_marker_array_pub.getNumSubscribers() != 0)
    {
      pushSimpleMarker(search_point.x, search_point.y, search_point.z,
                       1.0, 0, 0,
                       0.02,
                       g_marker_id, g_marker_array, frame_id);
    }

    //hand
    point_indeices.clear();
    point_distances.clear();
    kdtree.radiusSearch(search_point, g_config.hand_lenght, point_indeices, point_distances);
    for (size_t i = 0; i < point_indeices.size (); ++i)
    {
      hand_cloud->points.push_back(cloud->points[point_indeices[i]]);
      hand_cloud->points.back().r = 255;
      hand_cloud->points.back().g = 0;
      hand_cloud->points.back().b = 0;
    }

    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*hand_cloud, centroid);

    if(g_marker_array_pub.getNumSubscribers() != 0)
    {
      pushSimpleMarker(centroid.coeff(0), centroid.coeff(1), centroid.coeff(2),
                       0.0, 1.0, 0,
                       0.02,
                       g_marker_id, g_marker_array, frame_id);
    }

    hand_position.setX(centroid.coeff(0));
    hand_position.setY(centroid.coeff(1));
    hand_position.setZ(centroid.coeff(2));

#if 0
    //fingers
    search_point.x = centroid.coeff(0);
    search_point.y = centroid.coeff(1);
    search_point.z = centroid.coeff(2);
    std::vector<int> point_indeices_inner;
//.........这里部分代码省略.........

int LoadPCD::compute()
{
	//for each selected filename
	for (int k = 0; k < m_filenames.size(); ++k)
	{
		Eigen::Vector4f origin;
		Eigen::Quaternionf orientation;

		QString filename = m_filenames[k];

		boost::shared_ptr<PCLCloud> cloud_ptr_in = loadSensorMessage(filename, origin, orientation);

		if (!cloud_ptr_in) //loading failed?
			return 0;

		PCLCloud::Ptr cloud_ptr;
		if (!cloud_ptr_in->is_dense) //data may contain nans. Remove them
		{
			//now we need to remove nans
			pcl::PassThrough<PCLCloud> passFilter;
			passFilter.setInputCloud(cloud_ptr_in);

			cloud_ptr = PCLCloud::Ptr(new PCLCloud);
			passFilter.filter(*cloud_ptr);
		}
		else
		{
			cloud_ptr = cloud_ptr_in;
		}

		//now we construct a ccGBLSensor with these characteristics
		ccGBLSensor * sensor = new ccGBLSensor;

		// get orientation as rot matrix
		Eigen::Matrix3f eigrot = orientation.toRotationMatrix();

		// and copy it into a ccGLMatrix
		ccGLMatrix ccRot;
		for (int i = 0; i < 3; ++i)
		{
			for (int j = 0; j < 3; ++j)
			{
				ccRot.getColumn(j)[i] = eigrot(i,j);
			}
		}

		ccRot.getColumn(3)[3] = 1.0;

		// now in a format good for CloudComapre
		//ccGLMatrix ccRot = ccGLMatrix::FromQuaternion(orientation.coeffs().data());

		//ccRot = ccRot.transposed();
		ccRot.setTranslation(origin.data());

		sensor->setRigidTransformation(ccRot);
		sensor->setDeltaPhi(static_cast<PointCoordinateType>(0.05));
		sensor->setDeltaTheta(static_cast<PointCoordinateType>(0.05));
		sensor->setVisible(true);

		//uncertainty to some default
		sensor->setUncertainty(static_cast<PointCoordinateType>(0.01));

		ccPointCloud* out_cloud = sm2ccConverter(cloud_ptr).getCloud();
		if (!out_cloud)
			return -31;

		sensor->setGraphicScale(out_cloud->getBB().getDiagNorm() / 10);

		//do the projection on sensor
		ccGenericPointCloud* cloud = ccHObjectCaster::ToGenericPointCloud(out_cloud);
		int errorCode;
		CCLib::SimpleCloud* projectedCloud = sensor->project(cloud,errorCode,true);
		if (projectedCloud)
		{
			//DGM: we don't use it but we still have to delete it!
			delete projectedCloud;
			projectedCloud = 0;
		}

		QString cloud_name = QFileInfo(filename).baseName();
		out_cloud->setName(cloud_name);

		QFileInfo fi(filename);
		QString containerName = Q