下述是拟合平面:
template<class Vector3>
std::pair<Vector3, Vector3> best_plane_from_points(const std::vector<Vector3> & c)
{// copy coordinates to matrix in Eigen formatsize_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 centroidVector3 centroid(coord.row(0).mean(), coord.row(1).mean(), coord.row(2).mean());// subtract centroidcoord.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-pointsauto 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 formatsize_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);
}