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

Added optional derivatives to 'inverse' and 'compose' for variable type 'PoseRTV'

release/4.3a0
Stephen Williams 2012-10-22 17:31:41 +00:00
parent 227f9c1620
commit f9494679d2
2 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -93,12 +93,19 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
PoseRTV PoseRTV::inverse() const { PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); }
PoseRTV PoseRTV::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5);
return PoseRTV(Rt_.inverse(), v_.inverse()); return PoseRTV(Rt_.inverse(), v_.inverse());
} }
/* ************************************************************************* */ /* ************************************************************************* */
PoseRTV PoseRTV::compose(const PoseRTV& p) const { PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); }
PoseRTV PoseRTV::compose(const PoseRTV& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5);
if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5);
return PoseRTV(Rt_.compose(p.Rt_), v_.compose(p.v_)); return PoseRTV(Rt_.compose(p.Rt_), v_.compose(p.v_));
} }

13
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -88,12 +88,17 @@ public:
static PoseRTV Expmap(const Vector& v); static PoseRTV Expmap(const Vector& v);
static Vector Logmap(const PoseRTV& p); static Vector Logmap(const PoseRTV& p);
PoseRTV inverse() const;
PoseRTV compose(const PoseRTV& p) const;
static PoseRTV identity() { return PoseRTV(); } static PoseRTV identity() { return PoseRTV(); }
/** Derivatives calculated numerically */
PoseRTV inverse(boost::optional<Matrix&> H1=boost::none) const;
/** Derivatives calculated numerically */
PoseRTV compose(const PoseRTV& p,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** Derivatives calculated numerically */ /** Derivatives calculated numerically */
PoseRTV between(const PoseRTV& p, PoseRTV between(const PoseRTV& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1=boost::none,