Half implemented join method, fixed bugs

src/Edge.cpp

@@ -8,6 +8,12 @@
 
 using namespace Eigen;
 
+ostream &operator<<(ostream &_stream, const Edge &_edge)
+{
+	_stream << "(" << _edge.vertices[0].second << ", " << _edge.vertices[1].second << ")";
+	return _stream;
+}
+
 Edge::Edge()
 {
 	vertices.clear();

src/Edge.h

@@ -7,6 +7,7 @@
 #include <pcl/common/common.h>
 #include <boost/shared_ptr.hpp>
 #include <vector>
+#include <ostream>
 
 using namespace pcl;
 using namespace std;
@@ -25,6 +26,8 @@ class Edge
 	Edge(const Edge &_other);
 	~Edge();
 
+	friend ostream &operator<<(ostream &_stream, const Edge &_edge);
+
 	Edge &operator=(const Edge &_other);
 	bool operator<(const Edge &_other) const;
 	bool operator==(const Edge &_other) const;

src/Front.cpp

@@ -55,8 +55,37 @@ void Front::addEdges(const TrianglePtr &_triangle)
 	}
 }
 
-void Front::joinAndFix(const pair<int, TrianglePtr> &_data)
+void Front::joinAndFix(const pair<int, TrianglePtr> &_data, Pivoter &_pivoter)
 {
+	if (!_pivoter.isUsed(_data.first))
+	{
+		/**
+		 * This is the easy case, the new point has not been used
+		 */
+
+		// Add new edges
+		EdgePtr edge1 = _data.second->getEdge(1);
+		EdgePtr edge2 = _data.second->getEdge(2);
+		front.insert(pos, edge1);
+		front.insert(pos, edge2);
+
+		cout << "\tEdge added: " << *edge1 << "\n";
+		cout << "\tEdge added: " << *edge2 << "\n";
+
+		// Remove replaced edge
+		cout << "\tEdge removed: " << **pos << "\n";
+		front.erase(pos);
+
+		// Move iterator to the first added new edge
+		advance(pos, -2);
+		//pos--;
+
+		// Finally mark the point as used
+		_pivoter.setUsed(_data.first);
+	}
+	else
+	{
+	}
 }
 
 bool Front::inFront(PointNormal *_point)

src/Front.h

@@ -5,6 +5,7 @@
 #pragma once
 
 #include "Triangle.h"
+#include "Pivoter.h"
 #include <map>
 #include <list>
 
@@ -18,7 +19,7 @@ class Front
 
 	EdgePtr getActiveEdge();
 	void addEdges(const TrianglePtr &_triangle);
-	void joinAndFix(const pair<int, TrianglePtr> &_data);
+	void joinAndFix(const pair<int, TrianglePtr> &_data, Pivoter &_pivoter);
 	bool inFront(PointNormal *_point);
 
 private:

src/Helper.cpp

@@ -88,3 +88,16 @@ PointNormal Helper::makePointNormal(const float _x, const float _y, const float
 	point.curvature = _curvature;
 	return point;
 }
+
+PointNormal Helper::makePointNormal(const Vector3f &_data)
+{
+	PointNormal point;
+	point.x = _data.x();
+	point.y = _data.y();
+	point.z = _data.z();
+	point.normal_x = 0;
+	point.normal_y = 0;
+	point.normal_z = 0;
+	point.curvature = 0;
+	return point;
+}

src/Helper.h

@@ -12,6 +12,7 @@
 
 using namespace std;
 using namespace pcl;
+using namespace Eigen;
 
 // Sign function
 template<typename T> int sign(T val)
@@ -27,7 +28,8 @@ class Helper
 	static bool getCloudAndNormals(const string &_inputFile, PointCloud<PointNormal>::Ptr &_cloud, const double _estimationRadius = -1);
 	static int getActiveEdge(vector<Edge> &_front);
 
-	static PointNormal makePointNormal(const float _x, const float _y, const float _z, const float _nx = 0, const float _ny = 0, const float _nz = 1, const float _curvature = 0);
+	static PointNormal makePointNormal(const float _x, const float _y, const float _z, const float _nx = 0, const float _ny = 0, const float _nz = 0, const float _curvature = 0);
+	static PointNormal makePointNormal(const Vector3f &_data);
 private:
 	Helper();
 	~Helper();

src/Pivoter.cpp

@@ -33,17 +33,15 @@ pair<int, TrianglePtr> Pivoter::pivot(const EdgePtr &_edge)
 {
 	PointData v0 = _edge->getVertex(0);
 	PointData v1 = _edge->getVertex(1);
-	Vector3f opposite = _edge->getOppositeVertex().first->getVector3fMap();
+	PointData op = _edge->getOppositeVertex();
+
 	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 diff1 = 100 * (v0.first->getVector3fMap() - middle);
+	Vector3f diff2 = 100 * (_edge->getBallCenter().getVector3fMap() - middle);
 
 	Vector3f y = diff1.cross(diff2).normalized();
 	Vector3f normal = diff2.cross(y).normalized();
@@ -54,7 +52,7 @@ pair<int, TrianglePtr> Pivoter::pivot(const EdgePtr &_edge)
 	vector<float> squaredDistances;
 	kdtree.radiusSearch(edgeMiddle, ballRadius * 2, indices, squaredDistances);
 
-	Vector3f zeroAngle = (opposite - middle).normalized();
+	Vector3f zeroAngle = ((Vector3f) (op.first->getVector3fMap() - middle)).normalized();
 	zeroAngle = plane.projection(zeroAngle).normalized();
 
 	double currentAngle = M_PI;
@@ -63,33 +61,31 @@ pair<int, TrianglePtr> Pivoter::pivot(const EdgePtr &_edge)
 	// 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])
+		if (v0.second == index || v1.second == index || op.second == index || 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 point = cloud->at(index).getVector3fMap();
+		if (plane.absDistance(point) <= ballRadius)
 		{
-			Vector3f circleNormal;
-			pair<Vector3f, double> circle = getCircumscribedCircle(v0.second, v1.second, index, circleNormal);
 			Vector3f center;
-			if (getBallCenter(circle, circleNormal, center))
+			if (getBallCenter(v0.second, v1.second, index, center))
 			{
+				PointNormal ballCenter = Helper::makePointNormal(center);
+				vector<int> neighborhood = getNeighbors(ballCenter, ballRadius);
+				if (!isEmpty(neighborhood, _edge->getVertex(0).second, _edge->getVertex(1).second, index))
+					continue;
+
 				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)
 				{
@@ -116,9 +112,7 @@ TrianglePtr Pivoter::findSeed()
 			continue;
 
 		// Get the point's neighborhood
-		vector<int> indices;
-		vector<float> squaredDistances;
-		kdtree.radiusSearch(cloud->at(index0), ballRadius * 2, indices, squaredDistances);
+		vector<int> indices = getNeighbors(cloud->at(index0), ballRadius * 2);
 		if (indices.size() < 3)
 			continue;
 
@@ -135,26 +129,19 @@ TrianglePtr Pivoter::findSeed()
 				if (index1 == index2 || index2 == index0 || used[index2])
 					continue;
 
-				cout << "Testing (" << index0 << ", " << index1 << ", " << index2 << ")\n";
-
-				Vector3f normal;
-				pair<Vector3f, double> circle = getCircumscribedCircle(index0, index1, index2, normal);
-				if (circle.second < 0)
-					continue;
+				cout << "\tTesting (" << index0 << ", " << index1 << ", " << index2 << ")\n";
 
 				Vector3f center;
-				if (getBallCenter(circle, normal, center))
+				if (getBallCenter(index0, index1, index2, center))
 				{
-					vector<int> neighborhood;
-					vector<float> dists;
-					PointNormal ballCenter = Helper::makePointNormal(center.x(), center.y(), center.z());
-					kdtree.radiusSearch(ballCenter, ballRadius, neighborhood, dists);
+					PointNormal ballCenter = Helper::makePointNormal(center);
+					vector<int> neighborhood = getNeighbors(ballCenter, ballRadius);
 
 					//Writer::writeMesh("seedTesting", cloud, vector<TrianglePtr>(), TrianglePtr(new Triangle(cloud->at(index0), cloud->at(index1), cloud->at(index2), index0, index1, index2, ballCenter, ballRadius)), true);
 
 					if (isEmpty(neighborhood, index0, index1, index2))
 					{
-						cout << "Seed found (" << index0 << ", " << index1 << ", " << index2 << ")\n";
+						cout << "\tSeed found (" << index0 << ", " << index1 << ", " << index2 << ")\n";
 
 						seed = TrianglePtr(new Triangle(cloud->at(index0), cloud->at(index1), cloud->at(index2), index0, index1, index2, ballCenter, ballRadius));
 						used[index0] = used[index1] = used[index2] = true;
@@ -214,6 +201,25 @@ pair<Vector3f, double> Pivoter::getCircumscribedCircle(const int _index0, const
 	return make_pair(circumscribedCircleCenter, circumscribedCircleRadius);
 }
 
+bool Pivoter::getBallCenter(const int _index0, const int _index1, const int _index2, Vector3f &_center) const
+{
+	bool status = false;
+
+	Vector3f normal;
+	pair<Vector3f, double> circle = getCircumscribedCircle(_index0, _index1, _index2, normal);
+	if (circle.second > 0)
+	{
+		double squaredDistance = ballRadius * ballRadius - circle.second * circle.second;
+		if (squaredDistance > 0)
+		{
+			double distance = sqrt(fabs(squaredDistance));
+			_center = circle.first + distance * normal;
+			status = true;
+		}
+	}
+	return status;
+}
+
 bool Pivoter::isEmpty(const vector<int> &_data, const int _index0, const int _index1, const int _index2) const
 {
 	if (_data.size() > 3)
@@ -231,3 +237,11 @@ bool Pivoter::isEmpty(const vector<int> &_data, const int _index0, const int _in
 
 	return true;
 }
+
+vector<int> Pivoter::getNeighbors(const PointNormal &_point, const double _radius) const
+{
+	vector<int> indices;
+	vector<float> distances;
+	kdtree.radiusSearch(_point, _radius, indices, distances);
+	return indices;
+}

src/Pivoter.h

@@ -29,23 +29,11 @@ class Pivoter
 	void setUsed(const int _index);
 
 private:
-	bool isEmpty(const vector<int> &_data, const int _index0, const int _index1, const int _index2) const;
-
 	pair<Vector3f, double> getCircumscribedCircle(const int _index0, const int _index1, const int _index2, Vector3f &_normal) const;
-	inline bool getBallCenter(const pair<Vector3f, double> &_circumscribedCircle, const Vector3f &_normal, Vector3f &_ballCenter) const
-	{
-		bool status = false;
-
-		double squaredDistance = ballRadius * ballRadius - _circumscribedCircle.second * _circumscribedCircle.second;
-		if (squaredDistance > 0)
-		{
-			double distance = sqrt(fabs(squaredDistance));
-			_ballCenter = _circumscribedCircle.first + distance * _normal;
-			status = true;
-		}
-
-		return status;
-	}
+	bool getBallCenter(const int _index0, const int _index1, const int _index2, Vector3f &_center) const;
+
+	bool isEmpty(const vector<int> &_data, const int _index0, const int _index1, const int _index2) const;
+	vector<int> getNeighbors(const PointNormal &_point, const double _radius) const;
 
 	KdTreeFLANN<PointNormal> kdtree;
 	PointCloud<PointNormal>::Ptr cloud;

src/Writer.h

@@ -44,9 +44,11 @@ class Writer
 	{
 		static int sequential = 0;
 
-		string name = OUTPUT_FOLDER + _name + "_";
+		string name = OUTPUT_FOLDER + _name;
 		if (_addSequential)
-			name += SSTR(sequential++);
+		{
+			name += "_" + SSTR(sequential++);
+		}
 		name += _extension;
 
 		return name;

src/main.cpp

@@ -24,7 +24,6 @@ int main(int _argn, char **_argv)
 	// 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)
@@ -57,13 +56,15 @@ int main(int _argn, char **_argv)
 		EdgePtr edge;
 		while ((edge = front.getActiveEdge()) != NULL)
 		{
+			cout << "Testing edge " << *edge << "\n";
+
 			pair<int, TrianglePtr> data = pivoter.pivot(edge);
 			if (data.first != -1 && (!pivoter.isUsed(data.first) || front.inFront(&cloud->points[data.first])))
 			{
-				pivoter.setUsed(data.first);
+				cout << "Adding point " << data.first << " to front\n";
 				mesh.push_back(data.second);
-				front.joinAndFix(data);
-				Writer::writeMesh("mesh", cloud, mesh, true);
+				front.joinAndFix(data, pivoter);
+				Writer::writeMesh("addedPoint", cloud, mesh, data.second, true);
 			}
 			else
 				// Mark edge as boundary