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

Inlined compose and between derivatives in expmap/logmap

release/4.3a0
dellaert 2015-07-24 15:10:00 +02:00
parent 6058d045ae
commit e296f320f5
1 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/base/Lie.h
View File

@ -110,19 +110,20 @@ struct LieGroup {
Class retract(const TangentVector& v, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Jacobian D_g_v;
Class g = Class::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0);
Class h = compose(g,H1,H2);
if (H2) *H2 = (*H2) * D_g_v;
Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0);
Class h = compose(g); // derivatives inlined below
if (H1) *H1 = g.inverse().AdjointMap();
if (H2) *H2 = D_g_v;
return h;
}
TangentVector localCoordinates(const Class& g, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Class h = between(g,H1,H2);
Class h = between(g); // derivatives inlined below
Jacobian D_v_h;
TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
if (H1) *H1 = D_v_h * (*H1);
if (H2) *H2 = D_v_h * (*H2);
if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
if (H2) *H2 = D_v_h;
return v;
}