C++ ConvexHull类代码示例

    void
    cloud_callback (const CloudConstPtr& cloud)
    {
      FPS_CALC ("cloud callback");
      boost::mutex::scoped_lock lock (cloud_mutex_);
      cloud_ = cloud;
      search_.setInputCloud (cloud);

      // Subsequent frames are segmented by "tracking" the parameters of the previous frame
      // We do this by using the estimated inliers from previous frames in the current frame, 
      // and refining the coefficients

      if (!first_frame_)
      {
        if (!plane_indices_ || plane_indices_->indices.empty () || !search_.getInputCloud ())
        {
          PCL_ERROR ("Lost tracking. Select the object again to continue.\n");
          first_frame_ = true;
          return;
        }
        SACSegmentation<PointT> seg;
        seg.setOptimizeCoefficients (true);
        seg.setModelType (SACMODEL_PLANE);
        seg.setMethodType (SAC_RANSAC);
        seg.setMaxIterations (1000);
        seg.setDistanceThreshold (0.01);
        seg.setInputCloud (search_.getInputCloud ());
        seg.setIndices (plane_indices_);
        ModelCoefficients coefficients;
        PointIndices inliers;
        seg.segment (inliers, coefficients);

        if (inliers.indices.empty ())
        {
          PCL_ERROR ("No planar model found. Select the object again to continue.\n");
          first_frame_ = true;
          return;
        }

        // Visualize the object in 3D...
        CloudPtr plane_inliers (new Cloud);
        pcl::copyPointCloud (*search_.getInputCloud (), inliers.indices, *plane_inliers);
        if (plane_inliers->points.empty ())
        {
          PCL_ERROR ("No planar model found. Select the object again to continue.\n");
          first_frame_ = true;
          return;
        }
        else
        {
          plane_.reset (new Cloud);

          // Compute the convex hull of the plane
          ConvexHull<PointT> chull;
          chull.setDimension (2);
          chull.setInputCloud (plane_inliers);
          chull.reconstruct (*plane_);
        }
      }
    }

static void DrawHull(const ConvexHull& hull)
{
	const Point* ConvexVerts = hull.GetVerts();
	udword NbPolys = hull.GetNbPolygons();
//	printf("NbPolys: %d\n", NbPolys);
	for(udword i=0;i<NbPolys;i++)
	{
		const HullPolygon& PolygonData = hull.GetPolygon(i);
		glNormal3f(PolygonData.mPlane.n.x, PolygonData.mPlane.n.y, PolygonData.mPlane.n.z);

		const udword NbVertsInPoly = PolygonData.mNbVerts;
		const udword NbTris = NbVertsInPoly - 2;
		const udword* Indices = PolygonData.mVRef;
		udword Offset = 1;
		for(udword i=0;i<NbTris;i++)
		{
			const udword VRef0 = Indices[0];
			const udword VRef1 = Indices[Offset];
			const udword VRef2 = Indices[Offset+1];
			Offset++;

			const Point av3LineEndpoints[] = {ConvexVerts[VRef0], ConvexVerts[VRef1], ConvexVerts[VRef2]};
			glEnableClientState(GL_VERTEX_ARRAY);
			glVertexPointer(3, GL_FLOAT, sizeof(Point), &av3LineEndpoints[0].x);
			glDrawArrays(GL_TRIANGLES, 0, 3);
			glDisableClientState(GL_VERTEX_ARRAY);
		}
	}
}

void reconstructMesh(const PointCloud<PointXYZ>::ConstPtr &cloud,
  PointCloud<PointXYZ> &output_cloud, std::vector<Vertices> &triangles)
{
  boost::shared_ptr<std::vector<int> > indices(new std::vector<int>);
  indices->resize(cloud->points.size ());
  for (size_t i = 0; i < indices->size (); ++i) { (*indices)[i] = i; }

  pcl::search::KdTree<PointXYZ>::Ptr tree(new pcl::search::KdTree<PointXYZ>);
  tree->setInputCloud(cloud);

  PointCloud<PointXYZ>::Ptr mls_points(new PointCloud<PointXYZ>);
  PointCloud<PointNormal>::Ptr mls_normals(new PointCloud<PointNormal>);
  MovingLeastSquares<PointXYZ, PointNormal> mls;

  mls.setInputCloud(cloud);
  mls.setIndices(indices);
  mls.setPolynomialFit(true);
  mls.setSearchMethod(tree);
  mls.setSearchRadius(0.03);
  
  mls.process(*mls_normals);
  
  ConvexHull<PointXYZ> ch;
  
  ch.setInputCloud(mls_points);
  ch.reconstruct(output_cloud, triangles);
}

ConvexHull inset_ch(ConvexHull ch, double dist) {
    ConvexHull ret;
    for(unsigned i = 0; i < ch.size(); i++) {
        Point bisect = (rot90(ch[i+1] - ch[i]));
        ret.merge(unit_vector(bisect)*dist+ch[i]);
        ret.merge(unit_vector(bisect)*dist+ch[i+1]);
    }
    return ret;
}

bool HeightmapSampling::calculateConvexHull()
{
  ConvexHull < PointXYZ > cv;
  cv.setInputCloud(point_cloud_);
  boost::shared_ptr < PointCloud<PointXYZ> > cv_points;
  cv_points.reset(new PointCloud<PointXYZ> ());
  cv.reconstruct(*cv_points, convex_hull_vertices_);

  convex_hull_points_ = cv_points;

  return true;
}

void COpenCVInterfaceDlg::OnMorphologyConvexhull()
{
	if(mainImage.cols)
	{
		ConvexHull c;
		Mat rez=mainImage.clone();
		mainImage=Filters::gaussianFilter(mainImage,1);
		Scalar meanVal,stdval;

		meanStdDev(mainImage,meanVal,stdval);
		Canny(mainImage,mainImage,meanVal.val[0]*1/3.,meanVal.val[0]);
		//imshow("contours",mainImage);

		vector<std::vector<cv::Point>> contours;
		findContours(mainImage,contours,CV_RETR_LIST,CV_CHAIN_APPROX_NONE,Point(2,2));

		vector<Point> Points;
		//Points.resize(contours.size()*contours[0].size());
		for(int i=0;i<contours.size();i++)
		{
			for(int j=0;j<contours[i].size();j++)
			{
				Points.push_back(contours[i][j]);
			}
		}

		//for(int i=2;i<mainImage.rows-2;i++)
		//{
		//	for(int j=2;j<mainImage.cols-2;j++)
		//	{
		//		if(mainImage.at<uchar>(i,j)==255)
		//			Points.push_back(Point(i,j));
		//	}
		//}	

		std::vector<Point> chull;
		c.graham(Points, chull);

		for(int i=0;i<chull.size()-1;i++)
		{
			line(rez,chull[i],chull[i+1],Scalar(255,255,255),2);
		}

		line(rez,chull[chull.size()-1],chull[0],Scalar(255,255,255),2);
		prelImage=rez.clone();
		ShowResult(prelImage);
	}
	else
		MessageBox("No image loaded");
}

ConvexHull findConvexHull(PointSet *pointSet) {

  int pointCount = pointSet->getSize();

  pointSet->sortPointsByAngle();

  if (pointCount <= 3) { // The points are already a convex hull
    ConvexHull hull;
    for (int i = 0; i < pointCount; ++i) {
      hull.addPoint(*pointSet->getPoint(i));
    }
    if (pointCount > 0) {
      hull.addPoint(*pointSet->getPoint(0));
    }
    return hull;
  }

  std::stack<const Point *> candiates;

  const Point *prev;
  const Point *now;

  candiates.push(pointSet->getLastPoint());
  candiates.push(pointSet->getReferencePoint()); // Is always the first (idx 0)
  // element in the point set

  for (int i = 1; i < pointCount;) {
    const Point *point = pointSet->getPoint(i);

    now = candiates.top();
    candiates.pop();
    prev = candiates.top();
    candiates.push(now);

    if (isCCW(prev->pos(), now->pos(), point->pos())) {
      if (point->pos() == now->pos() && USE_CUSTOM_ALGO_FIX) {
        std::cout << "I saved your algorithm" << std::endl;
      } else {
        candiates.push(point);
      }
      ++i;
    } else {
      candiates.pop();
    }
  }

  ConvexHull hull(candiates);
  return hull;
}

pointer PCL_CONVEX_HULL (register context *ctx, int n, pointer *argv) {
  pointer in_cloud = argv[0];

  int width = intval(get_from_pointcloud(ctx, in_cloud, K_EUSPCL_WIDTH));
  int height = intval(get_from_pointcloud(ctx, in_cloud, K_EUSPCL_HEIGHT));
  pointer points = get_from_pointcloud(ctx, in_cloud, K_EUSPCL_POINTS);

  PointCloud< Point >::Ptr ptr =
    make_pcl_pointcloud< Point > (ctx, points, NULL, NULL, NULL, width, height);

  PointCloud< Point >::Ptr cloud_hull (new PointCloud<Point>);
  ConvexHull< Point > chull;

  chull.setInputCloud (ptr);
  chull.reconstruct (*cloud_hull);

  return make_pointcloud_from_pcl (ctx, *cloud_hull);
}

::testing::AssertionResult HullContainsPoint(ConvexHull const &h, Point const &p) {
    if (h.contains(p)) {
        return ::testing::AssertionSuccess();
    } else {
        return ::testing::AssertionFailure()
            << "Convex hull:\n"
            << h << "\ndoes not contain " << p;
    }
}

ConvexHull graham_merge(ConvexHull a, ConvexHull b) {
    ConvexHull result;

    // we can avoid the find pivot step because of top_point_first
    if(b.boundary[0] <= a.boundary[0])
        std::swap(a, b);

    result.boundary = a.boundary;
    result.boundary.insert(result.boundary.end(),
                           b.boundary.begin(), b.boundary.end());

/** if we modified graham scan to work top to bottom as proposed in lect754.pdf we could replace the
 angle sort with a simple merge sort type algorithm. furthermore, we could do the graham scan
 online, avoiding a bunch of memory copies.  That would probably be linear. -- njh*/
    result.angle_sort();
    result.graham_scan();

    return result;
}

static ConvexHull* CreateConvexHull(udword nb_verts, const Point* verts)
{
	ConvexHull* CH = ICE_NEW(ConvexHull);
	ASSERT(CH);

	CONVEXHULLCREATE Create;
	Create.NbVerts		= nb_verts;
	Create.Vertices		= verts;
	Create.UnifyNormals	= true;
	Create.PolygonData	= true;

	bool status = CH->Compute(Create);
	ASSERT(status);
	if(!status)
	{
		DELETESINGLE(CH);
	}

	return CH;
}

void rot_cal(cairo_t* cr, ConvexHull ch) {
    Point tb = ch.boundary.back();
    for(unsigned i = 0; i < ch.boundary.size(); i++) {
        Point tc = ch.boundary[i];
        Point n = -rot90(tb-tc);
        Point ta = *ch.furthest(n);
        cairo_move_to(cr, tc);
        cairo_line_to(cr, ta);
        tb = tc;
    }
}

int convex_plane(eusfloat_t *src, int ssize,
                 eusfloat_t *coeff, eusfloat_t *ret) {

  typedef PointXYZ Point;
  PointCloud<Point>::Ptr cloud_projected (new PointCloud<Point>);
  PointCloud<Point>::Ptr cloud_filtered (new PointCloud<Point>);
  floatvector2pointcloud(src, ssize, 1, *cloud_filtered);

  ModelCoefficients::Ptr coefficients (new ModelCoefficients);
  coefficients->values.resize(4);
  coefficients->values[0] = coeff[0];
  coefficients->values[1] = coeff[1];
  coefficients->values[2] = coeff[2];
  coefficients->values[3] = coeff[3] / 1000.0;

  // Project the model inliers
  ProjectInliers<Point> proj;
  proj.setModelType (SACMODEL_PLANE);
  proj.setInputCloud (cloud_filtered);
  proj.setModelCoefficients (coefficients);
  proj.filter (*cloud_projected);

  // Create a Concave Hull representation of the projected inliers
  PointCloud<Point>::Ptr cloud_hull (new PointCloud<Point>);
  ConvexHull<Point> chull;

  chull.setInputCloud (cloud_projected);
  //chull.setAlpha (alpha);
  chull.reconstruct (*cloud_hull);

  for(int i = 0; i < cloud_hull->points.size(); i++) {
    *ret++ = cloud_hull->points[i].x * 1000.0;
    *ret++ = cloud_hull->points[i].y * 1000.0;
    *ret++ = cloud_hull->points[i].z * 1000.0;
  }

  return cloud_hull->points.size();
}

HullState findConvexHullStep(PointSet *pointSet, int simulateUntilStep) {

  int step = 0;

  int pointCount = pointSet->getSize();

  pointSet->sortPointsByAngle();

  //++step;
  if (step++ == simulateUntilStep) // sort done -> update numbers
  {
    return HullState::SortDone(step);
  }

  if (pointCount <= 3) { // The points are already a convex hull
    ConvexHull hull;
    for (int i = 0; i < pointCount; ++i) {
      hull.addPoint(*pointSet->getPoint(i));
    }
    if (pointCount > 0) {
      hull.addPoint(*pointSet->getPoint(0));
    }
    return HullState::HullFound(hull);
  }

  std::stack<const Point *> candiates;

  const Point *prev;
  const Point *now;

  candiates.push(pointSet->getLastPoint());
  candiates.push(pointSet->getReferencePoint()); // Is always the first (idx 0)
                                                 // element in the point set

  //++step;
  if (step++ == simulateUntilStep) // break print candidates
  {
    return HullState::CandidateAdded(candiates, step);
  }

  for (int i = 1; i < pointCount;) {
    const Point *point = pointSet->getPoint(i);

    now = candiates.top();
    candiates.pop();
    prev = candiates.top();
    candiates.push(now);

    if (isCCW(prev->pos(), now->pos(), point->pos())) {
      candiates.push(point);
      // std::cout << "adding " << i << std::endl;
      ++i;

      //++step;
      if (step++ == simulateUntilStep) // break print candidates
      {
        return HullState::CandidateAdded(candiates, step);
      }
    } else {
      //++step;
      if (step++ == simulateUntilStep) // break print candidates
      {
        return HullState::CandidatePoped(candiates, point, step);
      }
      // std::cout << "pop" << std::endl;
      candiates.pop();
    }
  }
  ConvexHull hull(candiates);
  return HullState::HullFound(hull);
}

    /** \brief Given a plane, and the set of inlier indices representing it,
      * segment out the object of intererest supported by it. 
      *
      * \param[in] picked_idx the index of a point on the object
      * \param[in] cloud the full point cloud dataset
      * \param[in] plane_indices a set of indices representing the plane supporting the object of interest
      * \param[in] plane_boundary_indices a set of indices representing the boundary of the plane
      * \param[out] object the segmented resultant object 
      */
    void
    segmentObject (int picked_idx, 
                   const CloudConstPtr &cloud, 
                   const PointIndices::Ptr &plane_indices, 
                   const PointIndices::Ptr &plane_boundary_indices, 
                   Cloud &object)
    {
      CloudPtr plane_hull (new Cloud);

      // Compute the convex hull of the plane
      ConvexHull<PointT> chull;
      chull.setDimension (2);
      chull.setInputCloud (cloud);
      chull.setIndices (plane_boundary_indices);
      chull.reconstruct (*plane_hull);

      // Remove the plane indices from the data
      PointIndices::Ptr everything_but_the_plane (new PointIndices);
      if (indices_fullset_.size () != cloud->points.size ())
      {
        indices_fullset_.resize (cloud->points.size ());
        for (int p_it = 0; p_it < static_cast<int> (indices_fullset_.size ()); ++p_it)
          indices_fullset_[p_it] = p_it;
      }
      std::vector<int> indices_subset = plane_indices->indices;
      std::sort (indices_subset.begin (), indices_subset.end ());
      set_difference (indices_fullset_.begin (), indices_fullset_.end (), 
                      indices_subset.begin (), indices_subset.end (), 
                      inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ()));

      // Extract all clusters above the hull
      PointIndices::Ptr points_above_plane (new PointIndices);
      ExtractPolygonalPrismData<PointT> exppd;
      exppd.setInputCloud (cloud);
      exppd.setInputPlanarHull (plane_hull);
      exppd.setIndices (everything_but_the_plane);
      exppd.setHeightLimits (0.0, 0.5);           // up to half a meter
      exppd.segment (*points_above_plane);

      // Use an organized clustering segmentation to extract the individual clusters
      EuclideanClusterComparator<PointT, Normal, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Normal, Label>);
      euclidean_cluster_comparator->setInputCloud (cloud);
      euclidean_cluster_comparator->setDistanceThreshold (0.03f, false);
      // Set the entire scene to false, and the inliers of the objects located on top of the plane to true
      Label l; l.label = 0;
      PointCloud<Label>::Ptr scene (new PointCloud<Label> (cloud->width, cloud->height, l));
      // Mask the objects that we want to split into clusters
      for (int i = 0; i < static_cast<int> (points_above_plane->indices.size ()); ++i)
        scene->points[points_above_plane->indices[i]].label = 1;
      euclidean_cluster_comparator->setLabels (scene);

      vector<bool> exclude_labels (2);  exclude_labels[0] = true; exclude_labels[1] = false;
      euclidean_cluster_comparator->setExcludeLabels (exclude_labels);

      OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation (euclidean_cluster_comparator);
      euclidean_segmentation.setInputCloud (cloud);

      PointCloud<Label> euclidean_labels;
      vector<PointIndices> euclidean_label_indices;
      euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);

      // For each cluster found
      bool cluster_found = false;
      for (size_t i = 0; i < euclidean_label_indices.size (); i++)
      {
        if (cluster_found)
          break;
        // Check if the point that we picked belongs to it
        for (size_t j = 0; j < euclidean_label_indices[i].indices.size (); ++j)
        {
          if (picked_idx != euclidean_label_indices[i].indices[j])
            continue;
          //pcl::PointCloud<PointT> cluster;
          pcl::copyPointCloud (*cloud, euclidean_label_indices[i].indices, object);
          cluster_found = true;
          break;
          //object_indices = euclidean_label_indices[i].indices;
          //clusters.push_back (cluster);
        }
      }
    }