Fix behaviour of eigen33 function if smallest eigenvalue is not unique

The documentation of the eigen33 function says: "if the smallest eigenvalue is not unique, this function may return any eigenvector that is consistent to the eigenvalue"
Currently however, in that case the returned eigenvector is usually a vector of NaNs.
This commit applies the same logic as the other eigen33 function below. The effect on run time is minimal (one subtraction and one comparison).
In practice, it is rare that the smallest eigenvalue is not unique but it can happen, for example when normals are estimated with a very small neighbourhood. In the PCL tests, this is the case in the NormalRefinement test in test_filters.cpp

Author: mvieth

common/include/pcl/common/impl/eigen.hpp

@@ -308,10 +308,21 @@ eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenve
   computeRoots (scaledMat, eigenvalues);
 
   eigenvalue = eigenvalues (0) * scale;
-
-  scaledMat.diagonal ().array () -= eigenvalues (0);
-
-  eigenvector = detail::getLargest3x3Eigenvector<Vector> (scaledMat).vector;
+  if ( (eigenvalues (1) - eigenvalues (0)) > Eigen::NumTraits < Scalar > ::epsilon ()) {
+    // usual case: first and second are not equal (so first and third are also not equal).
+    // second and third could be equal, but that does not matter here
+    scaledMat.diagonal ().array () -= eigenvalues (0);
+    eigenvector = detail::getLargest3x3Eigenvector<Vector> (scaledMat).vector;
+  }
+  else if ( (eigenvalues (2) - eigenvalues (0)) > Eigen::NumTraits < Scalar > ::epsilon ()) {
+    // first and second equal: choose any unit vector that is orthogonal to third eigenvector
+    scaledMat.diagonal ().array () -= eigenvalues (2);
+    eigenvector = detail::getLargest3x3Eigenvector<Vector> (scaledMat).vector.unitOrthogonal ();
+  }
+  else {
+    // all three equal: just use an arbitrary unit vector
+    eigenvector << Scalar (1.0), Scalar (0.0), Scalar (0.0);
+  }
 }