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

expmap/logmap derivatives

release/4.3a0
dellaert 2015-07-19 00:39:47 -07:00
parent a5d49a261e
commit 272a563022
1 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/base/Lie.h
View File

@ -83,6 +83,25 @@ struct LieGroup {
return Class::Logmap(between(g));
}
Class expmap(const TangentVector& v, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Jacobian D_g_v;
Class g = Class::Expmap(v,H2 ? &D_g_v : 0);
Class h = compose(g,H1,H2);
if (H2) *H2 = (*H2) * D_g_v;
return h;
}
TangentVector logmap(const Class& g, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Class h = between(g,H1,H2);
Jacobian D_v_h;
TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0);
if (H1) *H1 = D_v_h * (*H1);
if (H2) *H2 = D_v_h * (*H2);
return v;
}
Class retract(const TangentVector& v) const {
return compose(Class::ChartAtOrigin::Retract(v));
}