12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

exponential map approximation

release/4.3a0
Frank Dellaert 2010-03-02 02:24:38 +00:00
parent 224a2f82db
commit c62ebe3ea8
2 changed files with 16 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cpp/Matrix.cpp
View File

@ -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

7
cpp/Matrix.h
View File

@ -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)\