diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 2570283fd..9a95d9158 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -89,7 +89,11 @@ namespace gtsam { } /** between operation */ - inline LieVector between(const LieVector& l2) const { + inline LieVector between(const LieVector& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(dim()); + if(H2) *H2 = eye(l2.dim()); return LieVector(l2.vector() - vector()); }