Add InvertPSDMatrix. This helper function lets the user compute the inverse or the pseudo-inverse of a matrix. Change-Id: Ia257859ad901debce2ea19f47907faf0b7b94759
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 7c42442..5b6e99c 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -330,6 +330,7 @@ ceres_test(graph_algorithms) ceres_test(householder_vector) ceres_test(is_close) + ceres_test(invert_psd_matrix) ceres_test(implicit_schur_complement) ceres_test(iterative_schur_complement_solver) ceres_test(jet)
diff --git a/internal/ceres/invert_psd_matrix.h b/internal/ceres/invert_psd_matrix.h new file mode 100644 index 0000000..34daf88 --- /dev/null +++ b/internal/ceres/invert_psd_matrix.h
@@ -0,0 +1,79 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_INVERT_PSD_MATRIX_H_ +#define CERES_INTERNAL_INVERT_PSD_MATRIX_H_ + +#include "ceres/internal/eigen.h" +#include "glog/logging.h" +#include "Eigen/Dense" + +namespace ceres { +namespace internal { + +// Helper routine to compute the inverse or pseudo-inverse of a +// symmetric positive semi-definite matrix. +// +// assume_full_rank controls whether a Cholesky factorization or an +// Singular Value Decomposition is used to compute the inverse and the +// pseudo-inverse respectively. +// +// The template parameter kSize can either be Eigen::Dynamic or a +// positive integer equal to the number of rows of m. +template <int kSize> +typename EigenTypes<kSize, kSize>::Matrix InvertPSDMatrix( + const bool assume_full_rank, + const typename EigenTypes<kSize, kSize>::Matrix& m) { + const int size = m.rows(); + + // If the matrix can be assumed to be full rank, then just use the + // Cholesky factorization to invert it. + if (assume_full_rank) { + return m.template selfadjointView<Eigen::Upper>().llt().solve( + Matrix::Identity(size, size)); + } + + Eigen::JacobiSVD<typename EigenTypes<kSize, kSize>::Matrix> svd( + m, Eigen::ComputeThinU | Eigen::ComputeThinV); + const double tolerance = + std::numeric_limits<double>::epsilon() * size * svd.singularValues()(0); + + return svd.matrixV() * + (svd.singularValues().array() > tolerance) + .select(svd.singularValues().array().inverse(), 0) + .matrix() + .asDiagonal() * + svd.matrixU().adjoint(); +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_INVERT_PSD_MATRIX_H_
diff --git a/internal/ceres/invert_psd_matrix_test.cc b/internal/ceres/invert_psd_matrix_test.cc new file mode 100644 index 0000000..5da9c11 --- /dev/null +++ b/internal/ceres/invert_psd_matrix_test.cc
@@ -0,0 +1,86 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2017 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/invert_psd_matrix.h" + +#include "ceres/internal/eigen.h" +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +static const bool kFullRank = true; +static const bool kRankDeficient = false; + +template <int kSize> +typename EigenTypes<kSize, kSize>::Matrix RandomPSDMatrixWithEigenValues( + const typename EigenTypes<kSize>::Vector& eigenvalues) { + typename EigenTypes<kSize, kSize>::Matrix m; + m.setRandom(); + Eigen::SelfAdjointEigenSolver<typename EigenTypes<kSize, kSize>::Matrix> es( + m); + return es.eigenvectors() * eigenvalues.asDiagonal() * + es.eigenvectors().transpose(); +} + +TEST(InvertPSDMatrix, Identity3x3) { + const Matrix m = Matrix::Identity(3, 3); + const Matrix inverse_m = InvertPSDMatrix<3>(kFullRank, m); + EXPECT_NEAR((inverse_m - m).norm() / m.norm(), + 0.0, + std::numeric_limits<double>::epsilon()); +} + +TEST(InvertPSDMatrix, FullRank5x5) { + EigenTypes<5>::Vector eigenvalues; + eigenvalues.setRandom(); + eigenvalues = eigenvalues.array().abs().matrix(); + const Matrix m = RandomPSDMatrixWithEigenValues<5>(eigenvalues); + const Matrix inverse_m = InvertPSDMatrix<5>(kFullRank, m); + EXPECT_NEAR((m * inverse_m - Matrix::Identity(5,5)).norm() / 5.0, 0.0, + std::numeric_limits<double>::epsilon()); +} + +TEST(InvertPSDMatrix, RankDeficient5x5) { + EigenTypes<5>::Vector eigenvalues; + eigenvalues.setRandom(); + eigenvalues = eigenvalues.array().abs().matrix(); + eigenvalues(3) = 0.0; + const Matrix m = RandomPSDMatrixWithEigenValues<5>(eigenvalues); + const Matrix inverse_m = InvertPSDMatrix<5>(kRankDeficient, m); + Matrix pseudo_identity = Matrix::Identity(5, 5); + pseudo_identity(3, 3) = 0.0; + EXPECT_NEAR((m * inverse_m * m - m).norm() / m.norm(), + 0.0, + 10 * std::numeric_limits<double>::epsilon()); +} + +} // namespace internal +} // namespace ceres