| [eigen] inconsistency in fast eigen decomposition |
[ Thread Index |
Date Index
| More lists.tuxfamily.org/eigen Archives
]
Hi all,
Gael, we've been using the fast eigen decomposition suggested by you in an e-mail back in 09/02/10 (regarding
SelfAdjointEigenSolver<Matrix3f> - http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/08/msg00283.html)
without any major issues for the past few months.
We've recently hit a problem, and I was wondering whether there is a fix for it that doesn't involve switching to
another SVD/eigendecomposition method in Eigen, as the implementation that we're using (see attached source) is still
blazing fast.
Here's the snippet of code that exhibits the problem (not that it's a c&p so please ignore the style/format):
Eigen3::Matrix3f covariance_matrix_f;
covariance_matrix_f << 5.64909, 5.64909, 0.0860435, 5.64909, 5.64909, 0.0860435, 0.0860435, 0.0860435, 2.92133;
Eigen3::SelfAdjointEigenSolver<Eigen3::Matrix3f> ei_symm (covariance_matrix_f);
Eigen3::Vector3f eigen_values = ei_symm.eigenvalues ();
Eigen3::Matrix3f eigen_vectors = ei_symm.eigenvectors ();
cerr << eigen_vectors << endl;
cerr << eigen_values << endl;
cerr << "Normal: " << eigen_vectors (0, 0) << ", " << eigen_vectors (1, 0) << ", " << eigen_vectors (2, 0) << endl;
Eigen3::Vector3f eigen_val;
Eigen3::Matrix3f eigen_vec;
eigen33 (covariance_matrix_f, eigen_vec, eigen_val);
cerr << eigen_vec << endl;
cerr << eigen_val << endl;
cerr << "Normal: " << eigen_vec (0, 0) << ", " << eigen_vec (1, 0) << ", " << eigen_vec (2, 0) << endl;
----
The output is:
0.707107 -0.0102705 0.707032
-0.707107 -0.0102656 0.707032
3.48268e-06 0.999894 0.0145212
3.41638e-08
2.91956
11.2999
Normal: 0.707107, -0.707107, 3.48268e-06
-0.00781202 0.0102683 0.707032
-0.00781202 0.0102683 0.707032
0.999939 -0.999895 0.0145207
-1.01014e-06
2.91956
11.2999
Normal: -0.00781202, -0.00781202, 0.999939
---
Thanks in advance.
Cheers,
Radu.
--
http://pointclouds.org
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@xxxxxxxx>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
// The computeRoots function included in this is based on materials
// covered by the following copyright and license:
//
// Geometric Tools, LLC
// Copyright (c) 1998-2010
// Distributed under the Boost Software License, Version 1.0.
//
// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:
//
// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
#ifndef PCL_EIGEN_H_
#define PCL_EIGEN_H_
#include <Eigen3/Core>
#include <Eigen3/Eigenvalues>
#include <Eigen3/Geometry>
template<typename Matrix, typename Roots> inline void
computeRoots (const Matrix& rkA, Roots& adRoot)
{
typedef typename Matrix::Scalar Scalar;
const Scalar msInv3 = 1.0/3.0;
const Scalar msRoot3 = Eigen3::internal::sqrt (Scalar (3.0));
Scalar dA00 = rkA (0, 0);
Scalar dA01 = rkA (0, 1);
Scalar dA02 = rkA (0, 2);
Scalar dA11 = rkA (1, 1);
Scalar dA12 = rkA (1, 2);
Scalar dA22 = rkA (2, 2);
// The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
// eigenvalues are the roots to this equation, all guaranteed to be
// real-valued, because the matrix is symmetric.
Scalar dC0 = dA00*dA11*dA22 + Scalar (2)*dA01*dA02*dA12 - dA00*dA12*dA12 - dA11*dA02*dA02 - dA22*dA01*dA01;
Scalar dC1 = dA00*dA11 - dA01*dA01 + dA00*dA22 - dA02*dA02 + dA11*dA22 - dA12*dA12;
Scalar dC2 = dA00 + dA11 + dA22;
// Construct the parameters used in classifying the roots of the equation
// and in solving the equation for the roots in closed form.
Scalar dC2Div3 = dC2*msInv3;
Scalar dADiv3 = (dC1 - dC2*dC2Div3)*msInv3;
if (dADiv3 > Scalar (0))
dADiv3 = Scalar (0);
Scalar dMBDiv2 = Scalar (0.5) * (dC0 + dC2Div3 * (Scalar (2) * dC2Div3*dC2Div3 - dC1));
Scalar dQ = dMBDiv2*dMBDiv2 + dADiv3*dADiv3*dADiv3;
if (dQ > Scalar (0))
dQ = Scalar (0);
// Compute the eigenvalues by solving for the roots of the polynomial.
Scalar dMagnitude = Eigen3::internal::sqrt (-dADiv3);
Scalar dAngle = std::atan2 (Eigen3::internal::sqrt (-dQ), dMBDiv2) * msInv3;
Scalar dCos = Eigen3::internal::cos (dAngle);
Scalar dSin = Eigen3::internal::sin (dAngle);
adRoot (0) = dC2Div3 + 2.f*dMagnitude*dCos;
adRoot (1) = dC2Div3 - dMagnitude * (dCos + msRoot3*dSin);
adRoot (2) = dC2Div3 - dMagnitude * (dCos - msRoot3*dSin);
// Sort in increasing order.
if (adRoot (0) >= adRoot (1))
std::swap (adRoot (0), adRoot (1));
if (adRoot (1) >= adRoot (2))
{
std::swap (adRoot (1), adRoot (2));
if (adRoot (0) >= adRoot (1))
std::swap (adRoot (0), adRoot (1));
}
}
template<typename Matrix, typename Vector> inline void
eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals)
{
typedef typename Matrix::Scalar Scalar;
// Scale the matrix so its entries are in [-1,1]. The scaling is applied
// only when at least one matrix entry has magnitude larger than 1.
Scalar scale = mat.cwiseAbs ()/*.template triangularView<Lower>()*/.maxCoeff ();
scale = (std::max) (scale,Scalar (1));
Matrix scaledMat = mat / scale;
// Compute the eigenvalues
pcl::computeRoots (scaledMat, evals);
// compute the eigen vectors
// here we assume 3 differents eigenvalues
Matrix tmp;
tmp = scaledMat;
tmp.diagonal ().array () -= evals (0);
evecs.col (0) = tmp.row (0).cross (tmp.row (1)).normalized ();
tmp = scaledMat;
tmp.diagonal ().array () -= evals (1);
evecs.col (1) = tmp.row (0).cross (tmp.row (1)).normalized ();
tmp = scaledMat;
tmp.diagonal ().array () -= evals (2);
evecs.col (2) = tmp.row (0).cross (tmp.row (1)).normalized ();
// Rescale back to the original size.
evals *= scale;
}
#endif //#ifndef PCL_EIGEN_H_