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

Added comparison to brute force matrix exponential

release/4.3a0
Paul Drews 2015-06-11 13:56:35 -04:00
parent 5ceb7d9ddc
commit f6ac546cc3
1 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -190,6 +190,15 @@ TEST(Similarity3, ExpLogMap) {
Similarity3 expZero = Similarity3::Expmap(zeros);
Similarity3 ident = Similarity3::identity();
EXPECT(assert_equal(expZero, ident));
// Compare to matrix exponential calculated using matlab expm
Rot3 Rexpm(0.9358, -0.2832, 0.2102,
0.3029, 0.9506, -0.0680,
-0.1805, 0.1273, 0.9753);
Point3 texpm(0.2724, 0.3825, 0.4213);
Similarity3 Sexpm(Rexpm, texpm, 2.0138);
Similarity3 Scalc = Similarity3::Expmap(expected);
EXPECT(assert_equal(Sexpm, Scalc, 1e-3));
}
//******************************************************************************
// Group action on Point3 (with simpler transform)