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

Pose3::between now 4 times faster (in Debug, did not test in Release)

release/4.3a0
Frank Dellaert 2013-04-05 06:44:16 +00:00
parent b40db06bf5
commit 620583f3c3
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/geometry/Pose3.cpp
View File

@ -226,11 +226,9 @@ namespace gtsam {
// between = compose(p2,inverse(p1));
Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Matrix invH;
Pose3 invp1 = inverse(invH);
Matrix composeH1;
Pose3 result = invp1.compose(p2, composeH1, H2);
if (H1) *H1 = composeH1 * invH;
Pose3 result = inverse()*p2;
if (H1) *H1 = -result.inverse().adjointMap();
if (H2) *H2 = I6;
return result;
}

3
gtsam/geometry/tests/timePose3.cpp
View File

@ -40,10 +40,13 @@ int main()
double x=1.0/norm, y=4.0/norm, z=2.0/norm;
Vector v = Vector_(6,x,y,z,0.1,0.2,-0.1);
Pose3 T = Pose3::Expmap(Vector_(6,0.1,0.1,0.2,0.1, 0.4, 0.2)), T2 = T.retract(v);
Matrix H1,H2;
TEST("retract", T.retract(v))
TEST("Expmap", T*Pose3::Expmap(v))
TEST("localCoordinates", T.localCoordinates(T2))
TEST("between", T.between(T2))
TEST("between-derivatives", T.between(T2,H1,H2))
TEST("Logmap", Pose3::Logmap(T.between(T2)))
return 0;