Eigen: лучшая посадка плоскости на n точек

Я только вхожу в Эйген и пытаюсь подогнать плоскость к ряду трехмерных точек, используя пример, который я нашел в Интернете. Однако мой результат всегда равен нулю. я имею:

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>();
std::cout << plane_normal << std::endl;return std::make_pair(centroid, plane_normal);

}void main()
{
Eigen::Vector3f point1(2, 2, 2);
Eigen::Vector3f point2(4, 2, 3);
Eigen::Vector3f point3(4, 2,1);
Eigen::Vector3f point4(2, 2,3);

std::vector<Eigen::Vector3f> points;

points.push_back(point1);
points.push_back(point2);
points.push_back(point3);
points.push_back(point4);best_plane_from_points(points);

std::cin.get();
}

Это правильный подход для использования здесь? Или в Eigen есть лучшая функция делать то, что я хочу?

Подвести итоги:

У меня есть n точек в трехмерном пространстве. Я хочу рассчитать лучшую плоскость подгонки из этих точек и вернуть углы Эйлера этой плоскости.

Спасибо за помощь.