atandrau

void PointCloud::computeCovarianceMatrix() {
	double means[3] = {0, 0, 0};
	for (int i = 0; i < points.size(); i++)
		means[0] += points[i].x,
		means[1] += points[i].y,
		means[2] += points[i].z;
	means[0] /= points.size(), means[1] /= points.size(), means[2] /= points.size();
	
	for (int i = 0; i < 3; i++)
		for (int j = 0; j < 3; j++) {

atandrau

Plane PointCloud::bestFittingPlane() {
	computeCovarianceMatrix();
	
	gsl_matrix * m = gsl_matrix_alloc(3, 3);
	for (int i = 0; i < 3; i++)
		for (int j = 0; j < 3; j++)
			gsl_matrix_set(m, i, j, covarianceMatrix[i][j]);
	
	gsl_vector *eval = gsl_vector_alloc(3);
	gsl_matrix *evec = gsl_matrix_alloc(3, 3);

atandrau

double Point::distanceTo(Plane p) {
    return abs( p.a * x   + p.b * y   + p.c * z   + p.d) /
	   sqrt(p.a * p.a + p.b * p.b + p.c * p.c);
}

atandrau

double PointCloud::computePlanarity(int pointIndex) {
	PointCloud p;
	p.points.push_back(points[pointIndex]);
	for (int i = 0; i < (int)points.size(); i++) {
		if(i != pointIndex && points[i].distanceTo(points[pointIndex]) <= MaxEPS) {
			p.points.push_back(points[i]);
		}
	}
	
	Plane plane = p.bestFittingPlane();