Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Refactor pcl::eigen33() to reduce potential errors (and remove bug) #3441

Merged
merged 4 commits into from
Nov 4, 2019
Merged

Refactor pcl::eigen33() to reduce potential errors (and remove bug) #3441

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
cc79d16
Refactor out common function for simpler code
kunaltyagi Oct 25, 2019
76a43b0
Using eigen operations instead of ad-hoc replacements
kunaltyagi Oct 31, 2019
675bedf
Adding tests and documentation
kunaltyagi Oct 31, 2019
c675f08
Annnotations and better test code
kunaltyagi Nov 1, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
199 changes: 56 additions & 143 deletions common/include/pcl/common/impl/eigen.hpp
View file
Open in desktop
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 ();
SergioRAgostinho marked this conversation as resolved.
Show resolved Hide resolved
}
// Rescale back to the original size.
evals *= scale;
Expand Down
15 changes: 15 additions & 0 deletions test/common/test_eigen.cpp
View file
Open in desktop
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
Open in desktop
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