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

ImuBias loses its mojo (superfluous Lie/Manifold stuff)

release/4.3a0
dellaert 2014-12-22 17:26:44 +01:00
parent 355b938f3a
commit 55fe170d4f
1 changed files with 3 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
gtsam/navigation/ImuBias.h
View File

@ -135,22 +135,6 @@ namespace imuBias {
&& equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol);
}
/// @}
/// @name Manifold
/// @{
/// dimension of the variable - used to autodetect sizes
inline static size_t Dim() { return dimension; }
/// return dimensionality of tangent space
inline size_t dim() const { return dimension; }
/** Update the bias with a tangent space update */
inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); }
/** @return the local coordinates of another object */
inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); }
/// @}
/// @name Group
/// @{
@ -158,52 +142,21 @@ namespace imuBias {
/** identity for group operation */
static ConstantBias identity() { return ConstantBias(); }
/** invert the object and yield a new one */
/** inverse */
inline ConstantBias operator-() const {
return ConstantBias(-biasAcc_, -biasGyro_);
}
/** invert the object and yield a new one */
inline ConstantBias inverse(boost::optional<Matrix&> H=boost::none) const {
if(H) *H = -gtsam::Matrix::Identity(6,6);
return - (*this);
}
/** addition */
ConstantBias operator+(const ConstantBias& b) const {
return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_);
}
ConstantBias compose(const ConstantBias& b,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = gtsam::Matrix::Identity(6,6);
if(H2) *H2 = gtsam::Matrix::Identity(6,6);
return *this + b;
}
/** subtraction */
ConstantBias operator-(const ConstantBias& b) const {
return ConstantBias(b.biasAcc_ - biasAcc_, b.biasGyro_ - biasGyro_);
}
/** between operation */
ConstantBias between(const ConstantBias& b,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -gtsam::Matrix::Identity(6,6);
if(H2) *H2 = gtsam::Matrix::Identity(6,6);
return b - *this;
}
/// @}
/// @name Lie Group
/// @{
/** Expmap around identity */
static inline ConstantBias Expmap(const Vector& v) { return ConstantBias(v.head(3), v.tail(3)); }
/** Logmap around identity - just returns with default cast back */
static inline Vector Logmap(const ConstantBias& b) { return b.vector(); }
/// @}
private: