Skip to content

Commit

Permalink
Merge pull request #3441 from kunaltyagi/eigen_refactor
Browse files Browse the repository at this point in the history 
Refactor `pcl::eigen33()` to reduce potential errors (and remove bug)
  • Loading branch information
@SergioRAgostinho
SergioRAgostinho authored Nov 4, 2019
2 parents 34d543d + c675f08 commit b3343e0
Show file tree
Hide file tree
Showing 3 changed files with 73 additions and 145 deletions.
199 changes: 56 additions & 143 deletions common/include/pcl/common/impl/eigen.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#ifndef PCL_COMMON_EIGEN_IMPL_HPP_
#define PCL_COMMON_EIGEN_IMPL_HPP_

#include <array>
#include <algorithm>

#include <pcl/console/print.h>
Expand Down Expand Up @@ -248,6 +249,43 @@ pcl::computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix::
eigenvector = vec3 / std::sqrt (len3);
}

namespace pcl {
namespace detail{
template <typename Vector, typename Scalar>
struct EigenVector {
Vector vector;
Scalar length;
}; // 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 <typename Vector, typename Matrix> static EigenVector<Vector, typename Matrix::Scalar>
getLargest3x3Eigenvector (const Matrix scaledMatrix)
{
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<Vector, Scalar> {crossProduct.row (index) / length,
length};
}
} // namespace detail
} // namespace pcl

//////////////////////////////////////////////////////////////////////////////////////////
template <typename Matrix, typename Vector> inline void
pcl::eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
Expand All @@ -269,20 +307,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::getLargest3x3Eigenvector<Vector> (scaledMat).vector;
}

//////////////////////////////////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -328,21 +353,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::getLargest3x3Eigenvector<Vector> (tmp).vector;
evecs.col (1) = evecs.col (2).unitOrthogonal ();
evecs.col (0) = evecs.col (1).cross (evecs.col (2));
}
Expand All @@ -353,130 +364,32 @@ 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::getLargest3x3Eigenvector<Vector> (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);

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
{
mmax[2] = len3;
evecs.col (2) = vec3 / std::sqrt (len3);
}
std::array<Scalar, 3> eigenVecLen;

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));

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
for (int i = 0; i < 3; ++i)
{
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;
Matrix tmp = scaledMat;
tmp.diagonal ().array () -= evals (i);
const auto vec_len = detail::getLargest3x3Eigenvector<Vector> (tmp);
evecs.col (i) = vec_len.vector;
eigenVecLen[i] = vec_len.length;
}

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;
}
// @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);
int mid_idx = 3 - min_idx - max_idx;

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
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;
Expand Down
15 changes: 15 additions & 0 deletions test/common/test_eigen.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -680,6 +680,21 @@ TEST (PCL, eigen33d)
const Scalar epsilon = 2e-5;
const unsigned iterations = 1000000;

// special case
r_matrix = Eigen::Matrix<Scalar, 3, 1>(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_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_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)
{
Expand Down
4 changes: 2 additions & 2 deletions test/features/test_gasd_estimation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down

0 comments on commit b3343e0

Please sign in to comment.