-
-
Save ialhashim/0a2554076a6cf32831ca to your computer and use it in GitHub Desktop.
| template<class Vector3> | |
| std::pair<Vector3, Vector3> best_plane_from_points(const std::vector<Vector3> & c) | |
| { | |
| // copy coordinates to matrix in Eigen format | |
| size_t num_atoms = c.size(); | |
| Eigen::Matrix< Vector3::Scalar, Eigen::Dynamic, Eigen::Dynamic > coord(3, num_atoms); | |
| for (size_t i = 0; i < num_atoms; ++i) coord.col(i) = c[i]; | |
| // calculate centroid | |
| Vector3 centroid(coord.row(0).mean(), coord.row(1).mean(), coord.row(2).mean()); | |
| // subtract centroid | |
| coord.row(0).array() -= centroid(0); coord.row(1).array() -= centroid(1); coord.row(2).array() -= centroid(2); | |
| // we only need the left-singular matrix here | |
| // http://math.stackexchange.com/questions/99299/best-fitting-plane-given-a-set-of-points | |
| auto svd = coord.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV); | |
| Vector3 plane_normal = svd.matrixU().rightCols<1>(); | |
| return std::make_pair(centroid, plane_normal); | |
| } | |
| template<class Vector3> | |
| std::pair < Vector3, Vector3 > best_line_from_points(const std::vector<Vector3> & c) | |
| { | |
| // copy coordinates to matrix in Eigen format | |
| size_t num_atoms = c.size(); | |
| Eigen::Matrix< Vector3::Scalar, Eigen::Dynamic, Eigen::Dynamic > centers(num_atoms, 3); | |
| for (size_t i = 0; i < num_atoms; ++i) centers.row(i) = c[i]; | |
| Vector3 origin = centers.colwise().mean(); | |
| Eigen::MatrixXd centered = centers.rowwise() - origin.transpose(); | |
| Eigen::MatrixXd cov = centered.adjoint() * centered; | |
| Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(cov); | |
| Vector3 axis = eig.eigenvectors().col(2).normalized(); | |
| return std::make_pair(origin, axis); | |
| } |
Some help for those who are new with Eigen (as me), you should include:
#include <Eigen/Core> #include <Eigen/Dense>
and the 18th line worked for me like this:
auto plane_normal = svd.matrixU().rightCols(1);
Otherwise, it works great. Thanks for the snippet.
I have an issue fitting a plane with this 3D point cloud:
It seems that the resulted plane doesn't fit the dataset but is parallel to it!
I don't understand the geometric reason for this and sincerely.
Someone can have a suggestion?
For the record: I found a sign error into my code!
The functions work properly.
Thanks
I wonder if using SelfAdjointEigenSolver for fitting line is correct.
That is because the document says:
Computes eigenvalues and eigenvectors of selfadjoint matrices.
and centers cannot be selfadjoint unless num_atoms == 3, I think.
For { {0,0,0}, {0,0,1} } and { {0,0,0}, {0,0,0} } the same line {0,0,1} is fit.
Is there a way to distinguish the former, valid input from the latter, degenerate input?
...and I was looking for line fitting with Eigen - thanks!