I want to fit a plane to a 3D point cloud. I use a RANSAC approach, where I sample several points from the point cloud, calculate the plane and store the plane with the smallest error. The error is the distance between the points and the plane. I want to do this in C++, using Eigen.
So far, I sample points from the point cloud and center the data. Now, I need to fit the plane to the samples points. I know I need to solve Mx = 0, but how do I do this? So far I have M (my samples), I want to know x (the plane) and this fit needs to be as close to 0 as possible. I have no idea where to continue from here. All I have are my sampled points and I need more data. I feel like all information is there, I am overlooking something.Answer1:
From you question I assume that you are familiar with the Ransac algorithm, so I will spare you of lengthy talks.
In a first step, you sample three random points. You can use the <a href="http://eigen.tuxfamily.org/dox/classEigen_1_1DenseBase.html#a8e759dafdd9ecc446d397b7f5435f60a" rel="nofollow">Random</a> class for that but picking them not truly random usually gives better results. To those points, you can simply fit a plane using <a href="http://eigen.tuxfamily.org/dox/classEigen_1_1Hyperplane.html#aabfcda23a80edfd62387bb3d930f4b96" rel="nofollow">Hyperplane::Through</a>.
In the second step, you repetitively cross out some points with large <a href="http://eigen.tuxfamily.org/dox/classEigen_1_1Hyperplane.html#a8383bb22fa5f6024856f2c781f6ec0db" rel="nofollow">Hyperplane::absDistance</a> and perform a least-squares fit on the remaining ones. It may look like this:
Vector3f mu = mean(points); Matrix3f covar = covariance(points, mu); Vector3 normal = smallest_eigenvector(covar); JacobiSVD<Matrix3f> svd(covariance, ComputeFullU); Vector3f normal = svd.matrixU().col(2); Hyperplane<float, 3> result(normal, mu);
Unfortunately, the functions
covariance are not built-in, but they are rather straightforward to code.