-
-
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); | |
} |
...and I was looking for line fitting with Eigen - thanks!
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.
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?
Thanks for this code snippet! I was looking for a sample of plane fitting using Eigen. This was exactly what I needed.