Created
November 14, 2014 23:17
-
-
Save ialhashim/0a2554076a6cf32831ca to your computer and use it in GitHub Desktop.
Fitting 3D points to a plane or a line
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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); | |
} |
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?
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
For the record: I found a sign error into my code!
The functions work properly.
Thanks