点云delaunay三角化PCL and C++

本文使用了PCL点云库来作可视化。结果如下图：

算法改编自一个JS程序。好像是github上的项目，但是时间比较长，不记得是哪个了。

最终结果和源程序基本一样。因为是出于兴趣写的，没有很严谨，无法保证算法的百分百正确。

感兴趣的可以将源码进行规范和优化。

下边是调用：

pcl::visualization::PCLVisualizer viewer("rgb");
	std::vector<int> res = delaunayTransform(ptr);
	viewer.addPointCloud(ptr, "cloud");
	cout << res.size();
	for (int i =0; i < res.size(); i+=2) {
		viewer.addLine(ptr->points[res[i]], ptr->points[res[i+1]], 1, 0, 0, to_string(i));
}

 源码：

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

static const float EPSILON = std::numeric_limits<float>::epsilon();
static const float PI = 3.1415926;
static const float cos30 = cos(PI / 6);
static const float cos60 = cos(PI / 3);

/*
 * Triangle
 */
struct Triangle
{
	PointT pa, pb, pc;
};

/*
 * index of triangle
 */
struct TriangleIndex
{
	int pa, pb, pc;
};

/*
 * Triangle
 */
struct Circumcircle
{
	float r = 0;
	TriangleIndex index;
	PointT center;
};

/*
 * t_p1,t_p2 are the indexes of edges corresponding to vertex p1,p2
 */
struct Edges
{
	int p1 = -1, p2 = -1;
	int amount = 1;
	int index = -1;
	int t_p1 = -1;
	int t_p2 = -1;
	std::list<int> top;
};

struct Line
{
	int p0 = -1, p1 = -1;
	bool isGroup = false;
};

/*
 * Get the  super triangle of point cloud
 */
Triangle getSuperTranigle(PointCloudT::Ptr ptr);

/*
 * Get circumcircle of each triangle
 * If three points are in a straight line,it will  return {}
 */
Circumcircle getCircumcircle(PointCloudT::Ptr ptr, TriangleIndex &index);

/*
 * Remove those identical edges
 * |it = edge.erase(it)| is necessary
 */
void removeEdge(std::list<int> &edge);

/*
 * Delanay transform
 * return sets of index of triangle vertices
 */
std::vector<int> delaunayTransform(PointCloudT::Ptr ptr);

float getDistance(PointT &a, PointT &b);

std::vector<PointT> getCircleCenter(PointT &a, PointT &b, float r);

bool equals(Edges &e1, Edges &e2);

void removeOutside(int &i, std::vector<Edges> &total, std::queue<Edges> &outside, int &top);

/*
 * Alpha Shape
 * 1. Construction of the Delaunay triangular network
 * 2. Remove triangles that do not satisfy the alpha shape algorithm:
 *	 (1) One side of the current triangle is longer than 2*alpha
 *	 (2) The current side length is AB, constructs a circle with an alpha radius (usually has two circles), and one circle
 *	     contains other points (excluding A, B)
 * 3. Count the number of edges of the remaining triangular network
 * 4. If the edge is unique, that is, the quantity is one, the edge is the boundary and the endpoint is the boundary point
 *
 *  y
 *   ↑   ·  ·
 *   |  · ·   ·
 *   | ·  · ·   ·
 *	 |  ·  ·  
 * —|——————→ x
 */

std::vector<int> delaunayAlphaShape(std::vector<int> &triangle, PointCloudT::Ptr ptr, double alpha);






Triangle getSuperTranigle(PointCloudT::Ptr ptr) {
	PointT p_max, p_min, mid;
	pcl::getMinMax3D(*ptr, p_min, p_max);
	float dx = p_max.x - p_min.x;
	float dy = p_max.y - p_min.y;
	float dmax = dx > dy ? dx : dy;
	Triangle super_triangle;
	mid.x = p_min.x + dx * 0.5;
	mid.y = p_min.y + dy * 0.5;
	super_triangle.pa = { mid.x - 20 * dmax,mid.y - dmax,0 };
	super_triangle.pb = { mid.x,mid.y + 20 * dmax,0 };
	super_triangle.pc = { mid.x + 20 * dmax,mid.y - dmax ,0 };
	return super_triangle;
}

Circumcircle getCircumcircle(PointCloudT::Ptr ptr, TriangleIndex& index) {
	using namespace std;
	Circumcircle circumcircle;
	circumcircle.index = index;
	PointT p1 = ptr->points[index.pa], p2 = ptr->points[index.pb], p3 = ptr->points[index.pc];
	float xc, yc, m1, m2, mx1, mx2, my1, my2, dx, dy;
	float delta_y1 = abs(p1.y - p2.y);
	float delta_y2 = abs(p2.y - p3.y);
	if (delta_y1 < EPSILON && delta_y2 < EPSILON) {
		return circumcircle;
	}
	if (delta_y1 < EPSILON) {
		m2 = -((p3.x - p2.x) / (p3.y - p2.y));
		mx2 = (p2.x + p3.x) * 0.5;
		my2 = (p2.y + p3.y) * 0.5;
		xc = (p1.x + p2.x) * 0.5;
		yc = m2 * (xc - mx2) + my2;
	}

	else if (delta_y2 < EPSILON) {
		m1 = -((p2.x - p1.x) / (p2.y - p1.y));
		mx1 = (p1.x + p2.x) * 0.5;
		my1 = (p1.y + p2.y) * 0.5;
		xc = (p3.x + p2.x) * 0.5;
		yc = m1 * (xc - mx1) + my1;
	}

	else {
		m1 = -((p2.x - p1.x) / (p2.y - p1.y));
		m2 = -((p3.x - p2.x) / (p3.y - p2.y));
		mx1 = (p1.x + p2.x) * 0.5;
		mx2 = (p2.x + p3.x) * 0.5;
		my1 = (p1.y + p2.y) * 0.5;
		my2 = (p2.y + p3.y) * 0.5;
		xc = (m1 * mx1 - m2 * mx2 + my2 - my1) / (m1 - m2);
		yc = (delta_y1 > delta_y2) ? m1 * (xc - mx1) + my1 : m2 * (xc - mx2) + my2;
	}
	dx = p2.x - xc;
	dy = p2.y - yc;
	circumcircle.center.x = xc;
	circumcircle.center.y = yc;
	circumcircle.center.z = 0;
	circumcircle.r = dx * dx + dy * dy;
	return circumcircle;
}

void removeEdge(std::list<int>& edge) {
	int a = -1, b = -1, c = -1, d = -1;
	for (auto it = edge.end(); it != edge.begin(); )
	{
		a = *(--it);
		b = *(--it);
		for (auto it2 = it; it2 != edge.begin();) {
			c = *(--it2);
			d = *(--it2);
			if ((a == c && b == d) || (a == d && b == c)) {
				it = edge.erase(it); it = edge.erase(it);
				it2 = edge.erase(it2); it2 = edge.erase(it2);
				break;
			}
		}
	}
	return;
}


bool equals(Edges& e1, Edges& e2) {
	if (e1.p1 == e2.p1 && e1.p2 == e2.p2) return true;
	if (e1.p1 == e2.p2 && e1.p2 == e2.p1)return true;
	return false;
}


std::vector<int> delaunayTransform(PointCloudT::Ptr ptr) {
	std::vector<int> res;
	if (ptr->size() < 3) return  res;
	int amount = ptr->size();
	std::vector<int> indices(amount);
	for (int i = 0; i < amount; ++i) indices[i] = i;
	sort(indices.begin(), indices.end(), [ptr](int i, int j)->int {
		return  ptr->points[j].x != ptr->points[i].x ? (ptr->points[j].x < ptr->points[i].x) : (i - j > 0 ? 0 : 1);
		});
	Triangle triangle = getSuperTranigle(ptr);
	TriangleIndex ti = { amount ,amount + 1,amount + 2 };
	ptr->points.emplace_back(triangle.pa); ptr->points.emplace_back(triangle.pb); ptr->points.emplace_back(triangle.pc);
	Circumcircle cir = getCircumcircle(ptr, ti);
	std::list<Circumcircle> open = { cir };
	std::list<Circumcircle>closed = {};
	std::list<int> edge = {};
	int m = 0;
	float dx = 0, dy = 0;
	for (int i = indices.size(); i;) {
		m = indices[--i];
		edge.clear();
		for (auto j = open.end(); j != open.begin();) {
			--j;
			dx = ptr->points[m].x - j->center.x;
			if (dx > 0.0 && dx * dx > j->r) {
				closed.emplace_back(*j);
				j = open.erase(j);
				continue;
			}
			dy = ptr->points[m].y - j->center.y;
			if (dx * dx + dy * dy - j->r > EPSILON) continue;
			edge.emplace_back(j->index.pa);
			edge.emplace_back(j->index.pb);
			edge.emplace_back(j->index.pb);
			edge.emplace_back(j->index.pc);
			edge.emplace_back(j->index.pc);
			edge.emplace_back(j->index.pa);
			j = open.erase(j);
		}
		removeEdge(edge);
		for (auto j = edge.end(); j != edge.begin();) {
			ti.pb = *(--j);
			ti.pa = *(--j);
			ti.pc = m;
			open.emplace_back(getCircumcircle(ptr, ti));
		}
	}
	closed.insert(closed.end(), open.begin(), open.end());
	ptr->points.pop_back(); ptr->points.pop_back(); ptr->points.pop_back();

	for (auto j = closed.end(); j != closed.begin(); ) {
		--j;
		if (j->index.pa < amount && j->index.pb < amount && j->index.pc < amount) {
			res.emplace_back(j->index.pa);
			res.emplace_back(j->index.pb);
			res.emplace_back(j->index.pc);
		}
	}
	std::vector<int> edges;
	int  a = -1, b = -1, c = -1;
	int pa = -1, pb = -1, pc = -1;
	Edges e[4];
	for (int i = res.size(); i;) {
		a = res[--i];
		b = res[--i];
		c = res[--i];
		e[0] = { a,b };
		e[1] = { b,c };
		e[2] = { c,a };
		pa = -1, pb = -1, pc = -1;
		for (int j = edges.size(); j;) {
			e[3] = { edges[--j],edges[--j] };
			if (pa == -1 && equals(e[0], e[3])) pa = j;
			if (pb == -1 && equals(e[1], e[3])) pb = j;
			if (pc == -1 && equals(e[2], e[3])) pc = j;
		}
		if (pa == -1) { edges.emplace_back(a); edges.emplace_back(b); }
		if (pb == -1) { edges.emplace_back(b); edges.emplace_back(c); }
		if (pc == -1) { edges.emplace_back(c); edges.emplace_back(a); }
	}
	return edges;
}