diff --git a/common/include/pcl/common/impl/eigen.hpp b/common/include/pcl/common/impl/eigen.hpp index 2862373f6a2..cc9897bc617 100644 --- a/common/include/pcl/common/impl/eigen.hpp +++ b/common/include/pcl/common/impl/eigen.hpp @@ -257,21 +257,27 @@ struct EigenVector { Scalar length; }; -template static EigenVector +/** + * @brief returns the unit vector along the largest eigen value as well as the + * length of the largest eigenvector + * @tparam Matrix matrix type providing similar in API as Eigen::Matrix + */ +template static EigenVector get_largest_3x3_eigenvector (const Matrix scaledMatrix) { - std::array vector {scaledMatrix.row (0).cross (scaledMatrix.row (1)), - scaledMatrix.row (0).cross (scaledMatrix.row (2)), - scaledMatrix.row (1).cross (scaledMatrix.row (2))}; - std::array len; - std::transform (vector.cbegin(), vector.cend (), len.begin (), - [](const auto& vec) { return vec.squaredNorm (); }); + Matrix crossProduct; + crossProduct << scaledMatrix.row (0).cross (scaledMatrix.row (1)), + scaledMatrix.row (0).cross (scaledMatrix.row (2)), + scaledMatrix.row (1).cross (scaledMatrix.row (2)); - const auto index = std::distance (len.cbegin (), - std::max_element (len.cbegin (), len.cend ())); + // expression template + const auto len = crossProduct.rowwise ().norm (); + const auto normalized = crossProduct.rowwise ().normalized (); - const auto eigenvector = vector[index] / std::sqrt(len[index]); - return EigenVector{eigenvector, len[index]}; + // first evaluation + typename Matrix::Index index; + const auto length = len.maxCoeff (index); + return EigenVector{normalized.row (index), length}; } } } @@ -297,7 +303,7 @@ pcl::eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& ei scaledMat.diagonal ().array () -= eigenvalues (0); - eigenvector = detail::get_largest_3x3_eigenvector (scaledMat).vector; + eigenvector = detail::get_largest_3x3_eigenvector (scaledMat).vector; } ////////////////////////////////////////////////////////////////////////////////////////// @@ -343,7 +349,7 @@ pcl::eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals) tmp = scaledMat; tmp.diagonal ().array () -= evals (2); - evecs.col (2) = detail::get_largest_3x3_eigenvector (tmp).vector; + evecs.col (2) = detail::get_largest_3x3_eigenvector (tmp).vector; evecs.col (1) = evecs.col (2).unitOrthogonal (); evecs.col (0) = evecs.col (1).cross (evecs.col (2)); } @@ -354,7 +360,7 @@ pcl::eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals) tmp = scaledMat; tmp.diagonal ().array () -= evals (0); - evecs.col (0) = detail::get_largest_3x3_eigenvector (tmp).vector; + evecs.col (0) = detail::get_largest_3x3_eigenvector (tmp).vector; evecs.col (1) = evecs.col (0).unitOrthogonal (); evecs.col (2) = evecs.col (0).cross (evecs.col (1)); } @@ -366,7 +372,7 @@ pcl::eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals) { Matrix tmp = scaledMat; tmp.diagonal ().array () -= evals (i); - const auto vec_len = detail::get_largest_3x3_eigenvector (tmp); + const auto vec_len = detail::get_largest_3x3_eigenvector (tmp); evecs.col (i) = vec_len.vector; eigenVecLen[i] = vec_len.length; }