From cc79d169223a783d45e5ee463c4e87a203b290df Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Fri, 25 Oct 2019 15:28:41 +0900 Subject: [PATCH 1/4] Refactor out common function for simpler code --- common/include/pcl/common/impl/eigen.hpp | 183 ++++++----------------- 1 file changed, 42 insertions(+), 141 deletions(-) diff --git a/common/include/pcl/common/impl/eigen.hpp b/common/include/pcl/common/impl/eigen.hpp index cca8a3c23c2..2862373f6a2 100644 --- a/common/include/pcl/common/impl/eigen.hpp +++ b/common/include/pcl/common/impl/eigen.hpp @@ -39,6 +39,7 @@ #ifndef PCL_COMMON_EIGEN_IMPL_HPP_ #define PCL_COMMON_EIGEN_IMPL_HPP_ +#include #include #include @@ -248,6 +249,33 @@ pcl::computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix:: eigenvector = vec3 / std::sqrt (len3); } +namespace pcl { +namespace detail{ +template +struct EigenVector { + Matrix vector; + Scalar length; +}; + +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 (); }); + + const auto index = std::distance (len.cbegin (), + std::max_element (len.cbegin (), len.cend ())); + + const auto eigenvector = vector[index] / std::sqrt(len[index]); + return EigenVector{eigenvector, len[index]}; +} +} +} + ////////////////////////////////////////////////////////////////////////////////////////// template inline void pcl::eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector) @@ -269,20 +297,7 @@ pcl::eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& ei scaledMat.diagonal ().array () -= eigenvalues (0); - Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1)); - Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2)); - Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2)); - - Scalar len1 = vec1.squaredNorm (); - Scalar len2 = vec2.squaredNorm (); - Scalar len3 = vec3.squaredNorm (); - - if (len1 >= len2 && len1 >= len3) - eigenvector = vec1 / std::sqrt (len1); - else if (len2 >= len1 && len2 >= len3) - eigenvector = vec2 / std::sqrt (len2); - else - eigenvector = vec3 / std::sqrt (len3); + eigenvector = detail::get_largest_3x3_eigenvector (scaledMat).vector; } ////////////////////////////////////////////////////////////////////////////////////////// @@ -328,21 +343,7 @@ pcl::eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals) tmp = scaledMat; tmp.diagonal ().array () -= evals (2); - Vector vec1 = tmp.row (0).cross (tmp.row (1)); - Vector vec2 = tmp.row (0).cross (tmp.row (2)); - Vector vec3 = tmp.row (1).cross (tmp.row (2)); - - Scalar len1 = vec1.squaredNorm (); - Scalar len2 = vec2.squaredNorm (); - Scalar len3 = vec3.squaredNorm (); - - if (len1 >= len2 && len1 >= len3) - evecs.col (2) = vec1 / std::sqrt (len1); - else if (len2 >= len1 && len2 >= len3) - evecs.col (2) = vec2 / std::sqrt (len2); - else - evecs.col (2) = vec3 / std::sqrt (len3); - + 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)); } @@ -353,130 +354,30 @@ pcl::eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals) tmp = scaledMat; tmp.diagonal ().array () -= evals (0); - Vector vec1 = tmp.row (0).cross (tmp.row (1)); - Vector vec2 = tmp.row (0).cross (tmp.row (2)); - Vector vec3 = tmp.row (1).cross (tmp.row (2)); - - Scalar len1 = vec1.squaredNorm (); - Scalar len2 = vec2.squaredNorm (); - Scalar len3 = vec3.squaredNorm (); - - if (len1 >= len2 && len1 >= len3) - evecs.col (0) = vec1 / std::sqrt (len1); - else if (len2 >= len1 && len2 >= len3) - evecs.col (0) = vec2 / std::sqrt (len2); - else - evecs.col (0) = vec3 / std::sqrt (len3); - + 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)); } else { - Matrix tmp; - tmp = scaledMat; - tmp.diagonal ().array () -= evals (2); + std::array eigenVecLen; - Vector vec1 = tmp.row (0).cross (tmp.row (1)); - Vector vec2 = tmp.row (0).cross (tmp.row (2)); - Vector vec3 = tmp.row (1).cross (tmp.row (2)); - - Scalar len1 = vec1.squaredNorm (); - Scalar len2 = vec2.squaredNorm (); - Scalar len3 = vec3.squaredNorm (); -#ifdef _WIN32 - Scalar *mmax = new Scalar[3]; -#else - Scalar mmax[3]; -#endif - unsigned int min_el = 2; - unsigned int max_el = 2; - if (len1 >= len2 && len1 >= len3) - { - mmax[2] = len1; - evecs.col (2) = vec1 / std::sqrt (len1); - } - else if (len2 >= len1 && len2 >= len3) - { - mmax[2] = len2; - evecs.col (2) = vec2 / std::sqrt (len2); - } - else + for (int i = 0; i < 3; ++i) { - mmax[2] = len3; - evecs.col (2) = vec3 / std::sqrt (len3); + Matrix tmp = scaledMat; + tmp.diagonal ().array () -= evals (i); + const auto vec_len = detail::get_largest_3x3_eigenvector (tmp); + evecs.col (i) = vec_len.vector; + eigenVecLen[i] = vec_len.length; } - tmp = scaledMat; - tmp.diagonal ().array () -= evals (1); - - vec1 = tmp.row (0).cross (tmp.row (1)); - vec2 = tmp.row (0).cross (tmp.row (2)); - vec3 = tmp.row (1).cross (tmp.row (2)); + minmax_it = std::minmax_element (eigenVecLen.cbegin (), eigenVecLen.cend ()); + int min_idx = std::distance (eigenVecLen.cbegin (), minmax_it.first); + int max_idx = std::distance (eigenVecLen.cbegin (), minmax_it.second); + int mid_el = 3 - min_el - max_el; - len1 = vec1.squaredNorm (); - len2 = vec2.squaredNorm (); - len3 = vec3.squaredNorm (); - if (len1 >= len2 && len1 >= len3) - { - mmax[1] = len1; - evecs.col (1) = vec1 / std::sqrt (len1); - min_el = len1 <= mmax[min_el] ? 1 : min_el; - max_el = len1 > mmax[max_el] ? 1 : max_el; - } - else if (len2 >= len1 && len2 >= len3) - { - mmax[1] = len2; - evecs.col (1) = vec2 / std::sqrt (len2); - min_el = len2 <= mmax[min_el] ? 1 : min_el; - max_el = len2 > mmax[max_el] ? 1 : max_el; - } - else - { - mmax[1] = len3; - evecs.col (1) = vec3 / std::sqrt (len3); - min_el = len3 <= mmax[min_el] ? 1 : min_el; - max_el = len3 > mmax[max_el] ? 1 : max_el; - } - - tmp = scaledMat; - tmp.diagonal ().array () -= evals (0); - - vec1 = tmp.row (0).cross (tmp.row (1)); - vec2 = tmp.row (0).cross (tmp.row (2)); - vec3 = tmp.row (1).cross (tmp.row (2)); - - len1 = vec1.squaredNorm (); - len2 = vec2.squaredNorm (); - len3 = vec3.squaredNorm (); - if (len1 >= len2 && len1 >= len3) - { - mmax[0] = len1; - evecs.col (0) = vec1 / std::sqrt (len1); - min_el = len3 <= mmax[min_el] ? 0 : min_el; - max_el = len3 > mmax[max_el] ? 0 : max_el; - } - else if (len2 >= len1 && len2 >= len3) - { - mmax[0] = len2; - evecs.col (0) = vec2 / std::sqrt (len2); - min_el = len3 <= mmax[min_el] ? 0 : min_el; - max_el = len3 > mmax[max_el] ? 0 : max_el; - } - else - { - mmax[0] = len3; - evecs.col (0) = vec3 / std::sqrt (len3); - min_el = len3 <= mmax[min_el] ? 0 : min_el; - max_el = len3 > mmax[max_el] ? 0 : max_el; - } - - unsigned mid_el = 3 - min_el - max_el; evecs.col (min_el) = evecs.col ( (min_el + 1) % 3).cross (evecs.col ( (min_el + 2) % 3)).normalized (); evecs.col (mid_el) = evecs.col ( (mid_el + 1) % 3).cross (evecs.col ( (mid_el + 2) % 3)).normalized (); -#ifdef _WIN32 - delete [] mmax; -#endif } // Rescale back to the original size. evals *= scale; From 76a43b02238c4fd2d90c5a0c8a951d10f3359b6a Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Thu, 31 Oct 2019 14:23:19 +0900 Subject: [PATCH 2/4] Using eigen operations instead of ad-hoc replacements Doc changes --- common/include/pcl/common/impl/eigen.hpp | 66 ++++++++++++++---------- 1 file changed, 38 insertions(+), 28 deletions(-) diff --git a/common/include/pcl/common/impl/eigen.hpp b/common/include/pcl/common/impl/eigen.hpp index 2862373f6a2..ed7313fe2ab 100644 --- a/common/include/pcl/common/impl/eigen.hpp +++ b/common/include/pcl/common/impl/eigen.hpp @@ -251,30 +251,40 @@ pcl::computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix:: namespace pcl { namespace detail{ -template +template struct EigenVector { - Matrix vector; + Vector vector; Scalar length; -}; - -template static EigenVector -get_largest_3x3_eigenvector (const Matrix scaledMatrix) +}; // struct EigenVector + +/** + * @brief returns the unit vector along the largest eigen value as well as the + * length of the largest eigenvector + * @tparam Vector Requested result type, needs to be explicitly provided and has + * to be implicitly constructible from ConstRowExpr + * @tparam Matrix deduced input type providing similar in API as Eigen::Matrix + */ +template static EigenVector +getLargest3x3Eigenvector (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 (); }); - - const auto index = std::distance (len.cbegin (), - std::max_element (len.cbegin (), len.cend ())); - - const auto eigenvector = vector[index] / std::sqrt(len[index]); - return EigenVector{eigenvector, len[index]}; -} -} + using Scalar = typename Matrix::Scalar; + using Index = typename Matrix::Index; + + Matrix crossProduct; + crossProduct << scaledMatrix.row (0).cross (scaledMatrix.row (1)), + scaledMatrix.row (0).cross (scaledMatrix.row (2)), + scaledMatrix.row (1).cross (scaledMatrix.row (2)); + + // expression template, no evaluation here + const auto len = crossProduct.rowwise ().norm (); + + Index index; + const Scalar length = len.maxCoeff (&index); // <- first evaluation + return EigenVector {crossProduct.row (index) / length, + length}; } +} // namespace detail +} // namespace pcl ////////////////////////////////////////////////////////////////////////////////////////// template inline void @@ -297,7 +307,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::getLargest3x3Eigenvector (scaledMat).vector; } ////////////////////////////////////////////////////////////////////////////////////////// @@ -343,7 +353,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::getLargest3x3Eigenvector (tmp).vector; evecs.col (1) = evecs.col (2).unitOrthogonal (); evecs.col (0) = evecs.col (1).cross (evecs.col (2)); } @@ -354,7 +364,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::getLargest3x3Eigenvector (tmp).vector; evecs.col (1) = evecs.col (0).unitOrthogonal (); evecs.col (2) = evecs.col (0).cross (evecs.col (1)); } @@ -366,18 +376,18 @@ 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::getLargest3x3Eigenvector (tmp); evecs.col (i) = vec_len.vector; eigenVecLen[i] = vec_len.length; } - minmax_it = std::minmax_element (eigenVecLen.cbegin (), eigenVecLen.cend ()); + const auto minmax_it = std::minmax_element (eigenVecLen.cbegin (), eigenVecLen.cend ()); int min_idx = std::distance (eigenVecLen.cbegin (), minmax_it.first); int max_idx = std::distance (eigenVecLen.cbegin (), minmax_it.second); - int mid_el = 3 - min_el - max_el; + int mid_idx = 3 - min_idx - max_idx; - evecs.col (min_el) = evecs.col ( (min_el + 1) % 3).cross (evecs.col ( (min_el + 2) % 3)).normalized (); - evecs.col (mid_el) = evecs.col ( (mid_el + 1) % 3).cross (evecs.col ( (mid_el + 2) % 3)).normalized (); + evecs.col (min_idx) = evecs.col ( (min_idx + 1) % 3).cross (evecs.col ( (min_idx + 2) % 3)).normalized (); + evecs.col (mid_idx) = evecs.col ( (mid_idx + 1) % 3).cross (evecs.col ( (mid_idx + 2) % 3)).normalized (); } // Rescale back to the original size. evals *= scale; From 675bedf37d019268175cfe63c874c04c9649e716 Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Thu, 31 Oct 2019 14:24:10 +0900 Subject: [PATCH 3/4] Adding tests and documentation --- test/common/test_eigen.cpp | 15 +++++++++++++++ test/features/test_gasd_estimation.cpp | 4 ++-- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/test/common/test_eigen.cpp b/test/common/test_eigen.cpp index 7f4a79983ea..e02ded1f0d0 100644 --- a/test/common/test_eigen.cpp +++ b/test/common/test_eigen.cpp @@ -680,6 +680,21 @@ TEST (PCL, eigen33d) const Scalar epsilon = 2e-5; const unsigned iterations = 1000000; + // special case + r_matrix = Eigen::Matrix(3, 2, 1).asDiagonal(); + c_matrix = r_matrix; + + eigen33(r_matrix, r_vectors, r_eigenvalues); + // check if the main vector is the positive Z direction. In this case it is equal to (0,0,1) + EXPECT_LE(fabs(r_vectors.col(0)[0]), epsilon); + EXPECT_LE(fabs(r_vectors.col(0)[1]), epsilon); + EXPECT_LE(fabs(r_vectors.col(0)[2] - 1), epsilon); + + eigen33(c_matrix, c_vectors, c_eigenvalues); + EXPECT_LE(fabs(c_vectors.col(0)[0]), epsilon); + EXPECT_LE(fabs(c_vectors.col(0)[1]), epsilon); + EXPECT_LE(fabs(c_vectors.col(0)[2] - 1), epsilon); + // test floating point row-major : row-major for (unsigned idx = 0; idx < iterations; ++idx) { diff --git a/test/features/test_gasd_estimation.cpp b/test/features/test_gasd_estimation.cpp index e8e3e251c87..cc3ad0354b0 100644 --- a/test/features/test_gasd_estimation.cpp +++ b/test/features/test_gasd_estimation.cpp @@ -178,7 +178,7 @@ TEST (PCL, GASDShapeAndColorEstimationNoInterp) gasd.compute (descriptor); const float ref_values[984] = - { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0101, 0, 0, 0, 0, 3.53535, 1.26263, 0, 0, 0, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.757576, 0, 0, 0, 0, 3.53535, 1.26263, 0, 0, 0, 1.51515, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.757576, 0, 0, 0, 0, 0.757576, 6.06061, 0, 0, 0, 0, 4.0404, 1.51515, 0, 0, 0, 0, 6.81818, 0, 0, 0, 0, 0, 1.76768, 1.76768, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.50505, 0, 0, 0, 0, 0, 3.78788, 0.50505, 0, 0, 0, 0, 5.80808, 0, 0, 0, 0, 0, 2.0202, @@ -238,7 +238,7 @@ TEST(PCL, GASDShapeAndColorEstimationQuadrilinearInterp) gasd.compute (descriptor); const float ref_values[984] = - { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0101, 0, 0, 0, 0, 3.53535, 1.26263, 0, 0, 0, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.757576, 0, 0, 0, 0, 3.53535, 1.26263, 0, 0, 0, 1.51515, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.757576, 0, 0, 0, 0, 0.757576, 6.06061, 0, 0, 0, 0, 4.0404, 1.51515, 0, 0, 0, 0, 6.81818, 0, 0, 0, 0, 0, 1.76768, 1.76768, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.50505, 0, 0, 0, 0, 0, 3.78788, 0.50505, 0, 0, 0, 0, 5.80808, 0, 0, 0, 0, 0, 2.0202, From c675f08c82737dec3db2b6cd6fa2bc5703e22e5a Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Fri, 1 Nov 2019 22:29:48 +0900 Subject: [PATCH 4/4] Annnotations and better test code --- common/include/pcl/common/impl/eigen.hpp | 2 ++ test/common/test_eigen.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/common/include/pcl/common/impl/eigen.hpp b/common/include/pcl/common/impl/eigen.hpp index ed7313fe2ab..a0e59e85c16 100644 --- a/common/include/pcl/common/impl/eigen.hpp +++ b/common/include/pcl/common/impl/eigen.hpp @@ -381,6 +381,8 @@ pcl::eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals) eigenVecLen[i] = vec_len.length; } + // @TODO: might be redundant or over-complicated as per @SergioRAgostinho + // see: https://github.com/PointCloudLibrary/pcl/pull/3441#discussion_r341024181 const auto minmax_it = std::minmax_element (eigenVecLen.cbegin (), eigenVecLen.cend ()); int min_idx = std::distance (eigenVecLen.cbegin (), minmax_it.first); int max_idx = std::distance (eigenVecLen.cbegin (), minmax_it.second); diff --git a/test/common/test_eigen.cpp b/test/common/test_eigen.cpp index e02ded1f0d0..d5eaf899efd 100644 --- a/test/common/test_eigen.cpp +++ b/test/common/test_eigen.cpp @@ -686,14 +686,14 @@ TEST (PCL, eigen33d) eigen33(r_matrix, r_vectors, r_eigenvalues); // check if the main vector is the positive Z direction. In this case it is equal to (0,0,1) - EXPECT_LE(fabs(r_vectors.col(0)[0]), epsilon); - EXPECT_LE(fabs(r_vectors.col(0)[1]), epsilon); - EXPECT_LE(fabs(r_vectors.col(0)[2] - 1), epsilon); + EXPECT_NEAR(r_vectors(0, 0), 0, epsilon); + EXPECT_NEAR(r_vectors(0, 1), 0, epsilon); + EXPECT_NEAR(r_vectors(0, 2), 1, epsilon); eigen33(c_matrix, c_vectors, c_eigenvalues); - EXPECT_LE(fabs(c_vectors.col(0)[0]), epsilon); - EXPECT_LE(fabs(c_vectors.col(0)[1]), epsilon); - EXPECT_LE(fabs(c_vectors.col(0)[2] - 1), epsilon); + EXPECT_NEAR(c_vectors(0, 0), 0, epsilon); + EXPECT_NEAR(c_vectors(0, 1), 0, epsilon); + EXPECT_NEAR(c_vectors(0, 2), 1, epsilon); // test floating point row-major : row-major for (unsigned idx = 0; idx < iterations; ++idx)