Use eigen operations only

PointCloudLibrary · Oct 25, 2019 · a98e206 · a98e206
1 parent a923a36
commit a98e206
Showing 1 changed file with 21 additions and 15 deletions.
diff --git 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 <typename Vector, typename Matrix> static EigenVector<Matrix, typename Matrix::Scalar>
+/**
+ * @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 <typename Matrix> static EigenVector<Matrix, typename Matrix::Scalar>
 get_largest_3x3_eigenvector (const Matrix scaledMatrix)
 {
-  std::array<Vector, 3> vector {scaledMatrix.row (0).cross (scaledMatrix.row (1)),
-                                scaledMatrix.row (0).cross (scaledMatrix.row (2)),
-                                scaledMatrix.row (1).cross (scaledMatrix.row (2))};
-  std::array<Scalar, 3> 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<Matrix> {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<Vector> (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<Vector> (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<Vector> (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<Vector> (tmp);
+      const auto vec_len = detail::get_largest_3x3_eigenvector (tmp);
       evecs.col (i) = vec_len.vector;
       eigenVecLen[i] = vec_len.length;
     }