From 819d3273f42a33c9b9f3702045f464e61d4572bf Mon Sep 17 00:00:00 2001 From: Vadim Indelman Date: Mon, 6 Feb 2012 15:15:01 +0000 Subject: [PATCH] Added Jacobians' calculation in compose and inverse functions. --- gtsam/base/LieVector.h | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 5a1b3d580..5db89fc49 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -77,7 +77,12 @@ struct LieVector : public Vector { } /** compose with another object */ - inline LieVector compose(const LieVector& p) const { + inline LieVector compose(const LieVector& p, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = eye(dim()); + if(H2) *H2 = eye(p.dim()); + return LieVector(vector() + p); } @@ -91,7 +96,9 @@ struct LieVector : public Vector { } /** invert the object and yield a new one */ - inline LieVector inverse() const { + inline LieVector inverse(boost::optional H=boost::none) const { + if(H) *H = -eye(dim()); + return LieVector(-1.0 * vector()); }