Front partially implemented (join is not done), pivot selecting bad

oriented triangles

src/Edge.cpp

@@ -15,6 +15,7 @@ Edge::Edge()
 	ballCenter = middlePoint = PointNormal();
 	pivotingRadius = 0;
 	active = false;
+	setId();
 }
 
 Edge::Edge(const PointData &_v0, const PointData &_v1, const PointData &_opposite, const PointNormal &_ballCenter)
@@ -32,6 +33,7 @@ Edge::Edge(const PointData &_v0, const PointData &_v1, const PointData &_opposit
 	pivotingRadius = (m - c).norm();
 
 	active = true;
+	setId();
 }
 
 Edge::Edge(const Edge &_other)
@@ -44,6 +46,7 @@ Edge::Edge(const Edge &_other)
 
 	middlePoint = _other.middlePoint;
 	active = _other.active;
+	id = _other.id;
 }
 
 Edge::~Edge()
@@ -62,6 +65,7 @@ Edge &Edge::operator=(const Edge &_other)
 
 		middlePoint = _other.middlePoint;
 		active = _other.active;
+		id = _other.id;
 	}
 
 	return *this;

src/Edge.h

@@ -38,19 +38,18 @@ class Edge
 	PointNormal getBallCenter() const;
 	double getPivotingRadius() const;
 
-	inline void linkForward(const EdgePtr &_other)
+private:
+	inline void setId()
 	{
-		next = _other;
-		//_other->next.get() = this;
+		static long currentId = 0;
+		id = currentId++;
 	}
 
-private:
 	vector<PointData> vertices;
 	PointData oppositeVertex;
 	PointNormal ballCenter;
 	PointNormal middlePoint;
 	double pivotingRadius;
 	bool active;
-
-	EdgePtr prev, next;
+	long id;
 };

src/Front.cpp

@@ -6,20 +6,53 @@
 
 Front::Front()
 {
+	front.clear();
+	pos = front.begin();
 }
 
 Front::~Front()
 {
 }
 
-EdgePtr Front::getActiveEdge() const
+EdgePtr Front::getActiveEdge()
 {
 	EdgePtr edge;
+
+	if (!front.empty())
+	{
+		bool firstLoop = true;
+		for (list<EdgePtr>::iterator it = pos;; it++)
+		{
+			if (it == front.end())
+				it = front.begin();
+			if (!firstLoop && it == pos)
+				break;
+
+			if ((*it)->isActive())
+			{
+				pos = it;
+				edge = *it;
+				break;
+			}
+
+			firstLoop = false;
+		}
+	}
+
 	return edge;
 }
 
 void Front::addEdges(const TrianglePtr &_triangle)
 {
+	for (int i = 0; i < 3; i++)
+	{
+		front.push_back(_triangle->getEdge(i));
+
+		PointData data = front.back()->getVertex(0);
+		frontPoints[data.first] = data.second;
+		data = front.back()->getVertex(1);
+		frontPoints[data.first] = data.second;
+	}
 }
 
 void Front::joinAndFix(const pair<int, TrianglePtr> &_data)

src/Front.h

@@ -6,19 +6,23 @@
 
 #include "Triangle.h"
 #include <map>
+#include <list>
+
+using namespace std;
 
 class Front
 {
 public:
 	Front();
 	~Front();
 
-	EdgePtr getActiveEdge() const;
+	EdgePtr getActiveEdge();
 	void addEdges(const TrianglePtr &_triangle);
 	void joinAndFix(const pair<int, TrianglePtr> &_data);
 	bool inFront(PointNormal *_point);
 
 private:
-
+	list<EdgePtr> front;
+	list<EdgePtr>::iterator pos;
 	map<PointNormal *, int> frontPoints;
 };

src/Helper.cpp

@@ -29,7 +29,7 @@ PointCloud<Normal>::Ptr Helper::getNormals(const PointCloud<PointXYZ>::Ptr &_clo
 	PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
 
 	search::KdTree<PointXYZ>::Ptr kdtree(new search::KdTree<PointXYZ>);
-	NormalEstimationOMP<PointXYZ, Normal> normalEstimation;
+	NormalEstimation<PointXYZ, Normal> normalEstimation;
 	normalEstimation.setInputCloud(_cloud);
 
 	if (_searchRadius > 0)

src/Pivoter.cpp

@@ -26,12 +26,83 @@ bool Pivoter::isUsed(const int _index) const
 
 void Pivoter::setUsed(const int _index)
 {
-	used[_index];
+	used[_index] = true;
 }
 
 pair<int, TrianglePtr> Pivoter::pivot(const EdgePtr &_edge)
 {
-	return make_pair(-1, TrianglePtr());
+	PointData v0 = _edge->getVertex(0);
+	PointData v1 = _edge->getVertex(1);
+	Vector3f opposite = _edge->getOppositeVertex().first->getVector3fMap();
+	PointNormal edgeMiddle = _edge->getMiddlePoint();
+	double pivotingRadius = _edge->getPivotingRadius();
+
+	// Create a plane passing for the middle point and perpendicular to the edge
+	Vector3f middle = edgeMiddle.getVector3fMap();
+	Vector3f ballCenter = _edge->getBallCenter().getVector3fMap();
+	Vector3f vertex0 = v0.first->getVector3fMap();
+
+	Vector3f diff1 = 100 * (vertex0 - middle);
+	Vector3f diff2 = 100 * (ballCenter - middle);
+
+	Vector3f y = diff1.cross(diff2).normalized();
+	Vector3f normal = diff2.cross(y).normalized();
+	Hyperplane<float, 3> plane = Hyperplane<float, 3>(normal, middle);
+
+	// Get neighborhood
+	vector<int> indices;
+	vector<float> squaredDistances;
+	kdtree.radiusSearch(edgeMiddle, ballRadius * 2, indices, squaredDistances);
+
+	Vector3f zeroAngle = (opposite - middle).normalized();
+	zeroAngle = plane.projection(zeroAngle).normalized();
+
+	double currentAngle = M_PI;
+	pair<int, TrianglePtr> output = make_pair(-1, TrianglePtr());
+
+	// Iterate over the neighborhood pivoting the ball
+	for (size_t i = 0; i < indices.size(); i++)
+	{
+		// Skip the points already used
+		int index = indices[i];
+		if (used[index])
+			continue;
+
+		Vector3f point = cloud->at(index).getVector3fMap();
+		double distanceToPlane = plane.absDistance(point);
+
+		/**
+		 * If the distance to the plane is less than the ball radius, then intersection between a ball
+		 * centered in the point and the plane exists
+		 */
+		if (distanceToPlane <= ballRadius)
+		{
+			Vector3f circleNormal;
+			pair<Vector3f, double> circle = getCircumscribedCircle(v0.second, v1.second, index, circleNormal);
+			Vector3f center;
+			if (getBallCenter(circle, circleNormal, center))
+			{
+				Vector3f projectedCenter = plane.projection(center);
+				double cosAngle = zeroAngle.dot(projectedCenter.normalized());
+				if (fabs(cosAngle) > 1)
+					cosAngle = sign<double>(cosAngle);
+				double angle = acos(cosAngle);
+
+				// TODO check if the ball is empty??
+
+				// TODO fix point selection according to the angle
+				if (output.first == -1 || currentAngle > angle)
+				{
+					currentAngle = angle;
+					output = make_pair(index, TrianglePtr(new Triangle(v0.first, v1.first, &cloud->points[index], v0.second, v1.second, index, center, ballRadius)));
+				}
+
+			}
+		}
+	}
+
+	// Extract result
+	return output;
 }
 
 TrianglePtr Pivoter::findSeed()

src/Triangle.cpp

@@ -3,6 +3,7 @@
  * 2015
  */
 #include "Triangle.h"
+#include "Helper.h"
 
 Triangle::Triangle()
 {
@@ -21,6 +22,16 @@ Triangle::Triangle(const PointNormal &_v0, const PointNormal &_v1, const PointNo
 	ballRadius = _ballRadius;
 }
 
+Triangle::Triangle(PointNormal *_v0, PointNormal *_v1, PointNormal *_v2, const int _index0, const int _index1, const int _index2, const Vector3f &_ballCenter, const double _ballRadius)
+{
+	vertices.resize(3);
+	vertices[0] = make_pair(_v0, _index0);
+	vertices[1] = make_pair(_v1, _index1);
+	vertices[2] = make_pair(_v2, _index2);
+	ballCenter = Helper::makePointNormal(_ballCenter.x(), _ballCenter.y(), _ballCenter.z());
+	ballRadius = _ballRadius;
+}
+
 Triangle::Triangle(const Triangle &_other)
 {
 	vertices = _other.vertices;

src/Triangle.h

@@ -22,6 +22,7 @@ class Triangle
 public:
 	Triangle();
 	Triangle(const PointNormal &_v0, const PointNormal &_v1, const PointNormal &_v2, const int _index0, const int _index1, const int _index2, const PointNormal &_ballCenter, const double _ballRadius);
+	Triangle(PointNormal *_v0, PointNormal *_v1, PointNormal *_v2, const int _index0, const int _index1, const int _index2, const Vector3f &_ballCenter, const double _ballRadius);
 	Triangle(const Triangle &_other);
 	~Triangle();
 
@@ -43,12 +44,12 @@ class Triangle
 			return -1;
 	}
 
-	inline Edge getEdge(const int _n) const
+	inline EdgePtr getEdge(const int _n) const
 	{
 		int index0 = _n % 3;
 		int index1 = (_n + 1) % 3;
 		int index2 = (_n + 2) % 3;
-		return Edge(vertices[index0], vertices[index1], vertices[index2], ballCenter);
+		return EdgePtr(new Edge(vertices[index0], vertices[index1], vertices[index2], ballCenter));
 	}
 
 	inline PointNormal getBallCenter() const

src/Writer.cpp

@@ -233,7 +233,7 @@ void Writer::generateNormals(const PointCloud<PointNormal>::Ptr &_cloud, ofstrea
 		_output << "0 ";
 	_output << "\n\n";
 
-	float factor = 0.005;
+	float factor = 0.002;
 	_output << "# points coordinates\n";
 	for (size_t i = 0; i < _cloud->size(); i++)
 	{

src/main.cpp

@@ -19,6 +19,12 @@ using namespace pcl;
 
 int main(int _argn, char **_argv)
 {
+	// TODO check if the seed selection is selecting correctly a ball which is above the surface according to the normals
+	// TODO check if the pivoting method is selecting correctly the next point according to the angle
+	// TODO fix the pivoting method to take into account points that can't be added because the face normal is downwards
+	//
+
+
 	system("rm -rf ./output/*");
 
 	if (_argn < 2)
@@ -78,53 +84,6 @@ int main(int _argn, char **_argv)
 		Writer::writeMesh("mesh", cloud, mesh, true);
 	}
 
-	////////////////////////////////
-
-//	DataHolder holder(cloud);
-//	Ball ball = Ball(holder, ballRadius);
-//	Front front;
-//
-//	// Create the mesh
-//	while (true)
-//	{
-//		Edge *edge;
-//		while (front.getActiveEdge(&edge))
-//		{
-//			pair<int, Triangle> pivotData = ball.pivot(edge);
-//			if (pivotData.first != -1 && (!holder.used[pivotData.first] || front.inFront(pivotData.first)))
-//			{
-//				holder.used[pivotData.first] = true;
-//				outputMesh.push_back(pivotData.second);
-//
-//				// TODO join and glue operations must set edges as active/disabled
-//				//front.join(edge, &cloud->at(pivotData.first), pivotData);
-////				join(edge, p, front);
-////				if (inFront(Edge(edge.getPoint(0), p), front))
-////					glue();
-////				if (inFront(Edge(edge.getPoint(1), p), front))
-////					glue();
-//
-//				Writer::writeMesh("mesh", cloud, outputMesh, true);
-//			}
-//			else
-//			{
-//				// Mark edge as boundary
-//				edge->setActive(false);
-//			}
-//		}
-//
-//		Triangle seed;
-//		if (ball.findSeedTriangle(seed))
-//		{
-//			outputMesh.push_back(seed);
-//			front.addEdges(seed);
-//		}
-//		else
-//			break;
-//
-//		Writer::writeMesh("mesh", cloud, outputMesh, true);
-//	}
-
 	cout << "Writing output mesh\n";
 	Writer::writeMesh("mesh", cloud, mesh);