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

Added Jacobians' calculation in compose and inverse functions.

release/4.3a0
Vadim Indelman 2012-02-06 15:15:01 +00:00
parent 9f66fa20be
commit 819d3273f4
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/base/LieVector.h
View File

@ -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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H=boost::none) const {
if(H) *H = -eye(dim());
return LieVector(-1.0 * vector());
}