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

Bugfix ScalarTraits

release/4.3a0
dellaert 2014-12-21 19:00:15 +01:00
parent 0c74ed8686
commit ede01ac6d3
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/base/concepts.h
View File

@ -243,7 +243,9 @@ struct ScalarTraits {
} }
static TangentVector Local(Scalar origin, Scalar other) { static TangentVector Local(Scalar origin, Scalar other) {
return TangentVector(other - origin); TangentVector result;
result(0) = other - origin;
return result;
} }
static Scalar Retract(Scalar origin, const TangentVector& v) { static Scalar Retract(Scalar origin, const TangentVector& v) {
@ -254,7 +256,7 @@ struct ScalarTraits {
ChartJacobian Hother = boost::none) { ChartJacobian Hother = boost::none) {
if (Horigin) (*Horigin)[0] = -1.0; if (Horigin) (*Horigin)[0] = -1.0;
if (Hother) (*Hother)[0] = 1.0; if (Hother) (*Hother)[0] = 1.0;
return TangentVector(other - origin); return Local(origin,other);
} }
static Scalar Retract(Scalar origin, const TangentVector& v, static Scalar Retract(Scalar origin, const TangentVector& v,
@ -291,12 +293,12 @@ struct ScalarTraits {
} }
static Scalar Identity() { return 0; } static Scalar Identity() { return 0; }
static TangentVector Logmap(Scalar m) {return TangentVector(m);} static TangentVector Logmap(Scalar m) {return Local(0,m);}
static Scalar Expmap(const TangentVector& v) { return v[0];} static Scalar Expmap(const TangentVector& v) { return v[0];}
static TangentVector Logmap(Scalar m, ChartJacobian Hm) { static TangentVector Logmap(Scalar m, ChartJacobian Hm) {
if (Hm) (*Hm)[0] = 1.0; if (Hm) (*Hm)[0] = 1.0;
return TangentVector(m); return Local(0,m);
} }
static Scalar Expmap(const TangentVector& v, ChartJacobian Hv) { static Scalar Expmap(const TangentVector& v, ChartJacobian Hv) {