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

Bug fix in Rot3 for logmap(identity)

release/4.3a0
Richard Roberts 2010-01-10 22:58:30 +00:00
parent 1dcc864d97
commit b0fd5396ad
2 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/Rot3.h
View File

@ -158,7 +158,7 @@ namespace gtsam {
// Log map at identity - return the canonical coordinates of this rotation
inline Vector logmap(const Rot3& R) {
double tr = R.r1().x()+R.r2().y()+R.r3().z();
if (tr==3.0) return ones(3); // todo: identity?
if (tr==3.0) return zero(3);
if (tr==-1.0) throw std::domain_error("Rot3::log: trace == -1 not yet handled :-(");;
double theta = acos((tr-1.0)/2.0);
return (theta/2.0/sin(theta))*Vector_(3,

8
cpp/testRot3.cpp
View File

@ -121,6 +121,14 @@ TEST(Rot3, log)
Vector w = Vector_(3, 0.1, 0.4, 0.2);
Rot3 R = rodriguez(w);
CHECK(assert_equal(w, logmap(R)));
Vector w5 = Vector_(3, 0.0, 0.0, 0.0);
Rot3 R5 = rodriguez(w5);
CHECK(assert_equal(w5, logmap(R5)));
// Vector w6 = Vector_(3, M_PI, 0.0, 0.0);
// Rot3 R6 = rodriguez(w6);
// CHECK(assert_equal(w6, logmap(R6)));
}
/* ************************************************************************* */