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

Fixed derivatives...

release/4.3a0
dellaert 2014-12-28 21:31:45 +01:00
parent bed5132ca1
commit 9367dc2fb5
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/base/Lie.h
View File

@ -110,7 +110,7 @@ struct LieGroup {
Class retract(const TangentVector& v, // Class retract(const TangentVector& v, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const { ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Jacobian D_g_v; Jacobian D_g_v;
Class g = Class::ChartAtOrigin::Retract(v,D_g_v); Class g = Class::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0);
Class h = compose(g,H1,H2); Class h = compose(g,H1,H2);
if (H2) *H2 = (*H2) * D_g_v; if (H2) *H2 = (*H2) * D_g_v;
return h; return h;
@ -120,7 +120,7 @@ struct LieGroup {
ChartJacobian H1, ChartJacobian H2 = boost::none) const { ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Class h = between(g,H1,H2); Class h = between(g,H1,H2);
Jacobian D_v_h; Jacobian D_v_h;
TangentVector v = Class::ChartAtOrigin::Local(h, D_v_h); TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
if (H1) *H1 = D_v_h * (*H1); if (H1) *H1 = D_v_h * (*H1);
if (H2) *H2 = D_v_h * (*H2); if (H2) *H2 = D_v_h * (*H2);
return v; return v;