- #include <lapackpp/lavd.h>
- #include <lapackpp/lasvd.h>
- #include <lapackpp/laexcp.h>
- CML::Plane3d GeomUtil::computeLeastSquaresPlane(const std::vector<CML::Vector3d>& points,
- bool* error)
- {
- if (points.empty()) {
- if (error != NULL) { *error = true; }
- return CML::Plane3d(0,1,0,0);
- }
- CML::Vector3d center(0,0,0);
- std::vector<CML::Vector3d>::const_iterator itr, end = points.end();
- for (itr = points.begin(); itr != end; ++itr) {
- center += *itr;
- }
- center *= 1.0/points.size();
- LaGenMatDouble W = LaGenMatDouble::zeros(3,3);
- for (itr = points.begin(); itr != end; ++itr) {
- CML::Vector3d p = *itr - center;
- W(0,0) += p.x * p.x;
- W(0,1) += p.x * p.y;
- W(0,2) += p.x * p.z;
- W(1,0) += p.y * p.x;
- W(1,1) += p.y * p.y;
- W(1,2) += p.y * p.z;
- W(2,0) += p.z * p.x;
- W(2,1) += p.z * p.y;
- W(2,2) += p.z * p.z;
- }
- LaVectorDouble Sigma(3);
- LaGenMatDouble U;
- LaGenMatDouble VT(3,3);
- try {
- LaSVD_IP(W, Sigma, U, VT);
- }
- catch (LaException e) {
- if (error != NULL) { *error = true; }
- return CML::Plane3d(CML::Vector3d(0,1,0), center);
- }
- if (error != NULL) {
- *error = (Sigma(2) < 1.0e-16);
- }
- CML::Vector3d normal(VT(2,0), VT(2,1), VT(2,2));
- return CML::Plane3d(normal, center);
- }