C++ eigen::Vector4f类代码示例

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

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