Best Fitting Plane of a 3D Point Cloud with PCA and GSL

best_fitting_plane.cpp

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);
	
	gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc(3);
	gsl_eigen_symmv(m, eval, evec, w);
	gsl_eigen_symmv_free(w);
	gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);

	gsl_vector_view evec_i = gsl_matrix_column(evec, 0);
		
	Plane p(gsl_vector_get(&evec_i.vector, 0),
			gsl_vector_get(&evec_i.vector, 1),
			gsl_vector_get(&evec_i.vector, 2),
			points[0].x * gsl_vector_get(&evec_i.vector, 0) +
			points[0].y * gsl_vector_get(&evec_i.vector, 1) +
			points[0].z * gsl_vector_get(&evec_i.vector, 2));
	
	gsl_vector_free(eval);
	gsl_matrix_free(evec);
	
	return p;
}