From c62ebe3ea88f50eb2770f5ec4c0c64fecc7cc7e4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 2 Mar 2010 02:24:38 +0000 Subject: [PATCH] exponential map approximation --- cpp/Matrix.cpp | 12 ++++++++++-- cpp/Matrix.h | 7 ++++++- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp index 074ad44a4..39bf7bf08 100644 --- a/cpp/Matrix.cpp +++ b/cpp/Matrix.cpp @@ -1048,8 +1048,6 @@ Matrix inverse_square_root(const Matrix& A) { return inv; } - - /* ************************************************************************* */ Matrix square_root_positive(const Matrix& A) { size_t m = A.size2(), n = A.size1(); @@ -1067,6 +1065,16 @@ Matrix square_root_positive(const Matrix& A) { return vector_scale(S, V); // V*S; } +/* ************************************************************************* */ +Matrix expm(const Matrix& A, int K) { + Matrix E = eye(A.size1()), A_k = eye(4); + for (int k=1;k<=K;k++) { + A_k = A_k*A/k; + E = E + A_k; + } + return E; +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/Matrix.h b/cpp/Matrix.h index 491e6ff26..d851ee092 100644 --- a/cpp/Matrix.h +++ b/cpp/Matrix.h @@ -365,7 +365,12 @@ Matrix RtR(const Matrix& A); /** Return the inverse of a S.P.D. matrix. Inversion is done via Cholesky decomposition. */ Matrix cholesky_inverse(const Matrix &A); - +/** + * Numerical exponential map, naive approach, not industrial strength !!! + * @param A matrix to exponentiate + * @param K number of iterations + */ +Matrix expm(const Matrix& A, int K=7); // macro for unit tests #define EQUALITY(expected,actual)\