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

Got rid of cumbersome TangentVector struct

release/4.3a0
Frank Dellaert 2016-01-30 12:42:01 -08:00
parent 2a244cac12
commit f498a96582
3 changed files with 59 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
gtsam/navigation/PreintegrationBase.cpp
View File

@ -37,7 +37,7 @@ PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
//------------------------------------------------------------------------------
void PreintegrationBase::resetIntegration() {
deltaTij_ = 0.0;
deltaXij_ = TangentVector();
zeta_ = Vector9();
zeta_H_biasAcc_.setZero();
zeta_H_biasOmega_.setZero();
}
@ -64,7 +64,7 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
const bool params_match = p_->equals(*other.p_, tol);
return params_match && fabs(deltaTij_ - other.deltaTij_) < tol
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(deltaXij_.vector(), other.deltaXij_.vector(), tol)
&& equal_with_abs_tol(zeta_, other.zeta_, tol)
&& equal_with_abs_tol(zeta_H_biasAcc_, other.zeta_H_biasAcc_, tol)
&& equal_with_abs_tol(zeta_H_biasOmega_, other.zeta_H_biasOmega_, tol);
}
@ -120,42 +120,52 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
//------------------------------------------------------------------------------
// See extensive discussion in ImuFactor.lyx
PreintegrationBase::TangentVector PreintegrationBase::UpdateEstimate(
const Vector3& a_body, const Vector3& w_body, double dt,
const TangentVector& zeta, OptionalJacobian<9, 9> A,
OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) {
Vector9 PreintegrationBase::UpdateEstimate(const Vector3& a_body,
const Vector3& w_body, double dt,
const Vector9& zeta,
OptionalJacobian<9, 9> A,
OptionalJacobian<9, 3> B,
OptionalJacobian<9, 3> C) {
const auto theta = zeta.segment<3>(0);
const auto position = zeta.segment<3>(3);
const auto velocity = zeta.segment<3>(6);
// Calculate exact mean propagation
Matrix3 H;
const Matrix3 R = Rot3::Expmap(zeta.theta(), H).matrix();
const Matrix3 R = Rot3::Expmap(theta, H).matrix();
const Matrix3 invH = H.inverse();
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
TangentVector zetaPlus(zeta.theta() + invH * w_body * dt,
zeta.position() + zeta.velocity() * dt + a_nav * dt22,
zeta.velocity() + a_nav * dt);
Vector9 zetaPlus;
zetaPlus << //
theta + invH* w_body* dt, // theta
position + velocity* dt + a_nav* dt22, // position
velocity + a_nav* dt; // velocity
if (A) {
#define USE_NUMERICAL_DERIVATIVE
#ifdef USE_NUMERICAL_DERIVATIVE
auto f = [w_body](const Vector3& theta) {
return Rot3::ExpmapDerivative(theta).inverse() * w_body;
};
const Matrix3 invHw_H_theta =
numericalDerivative11<Vector3, Vector3>(f, theta);
#else
// First order (small angle) approximation of derivative of invH*w:
// const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body);
// NOTE(frank): Rot3::ExpmapDerivative(w_body) is also an approximation (but less accurate):
// If we replace approximation with numerical derivative we get better accuracy:
auto f = [w_body](const Vector3& theta) {
return Rot3::ExpmapDerivative(theta).inverse() * w_body;
};
const Matrix3 invHw_H_theta = numericalDerivative11<Vector3, Vector3>(f, zeta.theta());
// TODO(frank): find a cheap closed form solution (look at Iserles)
// NOTE(frank): Rot3::ExpmapDerivative(w_body) is a less accurate approximation
const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body);
#endif
// Exact derivative of R*a with respect to theta:
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * H;
A->setIdentity();
// theta:
A->block<3, 3>(0, 0).noalias() += invHw_H_theta * dt;
// position:
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22;
A->block<3, 3>(3, 6) = I_3x3 * dt;
// velocity:
A->block<3, 3>(6, 0) = a_nav_H_theta * dt;
A->block<3, 3>(0, 0).noalias() += invHw_H_theta * dt; // theta
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
}
if (B) {
B->block<3, 3>(0, 0) = Z_3x3;
@ -172,7 +182,7 @@ PreintegrationBase::TangentVector PreintegrationBase::UpdateEstimate(
}
//------------------------------------------------------------------------------
PreintegrationBase::TangentVector PreintegrationBase::updatedDeltaXij(
Vector9 PreintegrationBase::updatedZeta(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt,
OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B,
OptionalJacobian<9, 3> C) const {
@ -182,7 +192,7 @@ PreintegrationBase::TangentVector PreintegrationBase::updatedDeltaXij(
const Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
// Do update in one fell swoop
return UpdateEstimate(correctedAcc, correctedOmega, dt, deltaXij_, A, B, C);
return UpdateEstimate(correctedAcc, correctedOmega, dt, zeta_, A, B, C);
} else {
// More complicated derivatives in case of sensor displacement
Vector3 correctedAcc, correctedOmega;
@ -196,8 +206,8 @@ PreintegrationBase::TangentVector PreintegrationBase::updatedDeltaXij(
// Do update in one fell swoop
Matrix93 D_updated_correctedAcc, D_updated_correctedOmega;
const PreintegrationBase::TangentVector updated =
UpdateEstimate(correctedAcc, correctedOmega, dt, deltaXij_, A,
const Vector9 updated =
UpdateEstimate(correctedAcc, correctedOmega, dt, zeta_, A,
((B || C) ? &D_updated_correctedAcc : 0),
(C ? &D_updated_correctedOmega : 0));
if (B) *B = D_updated_correctedAcc* D_correctedAcc_measuredAcc;
@ -217,7 +227,7 @@ void PreintegrationBase::update(const Vector3& measuredAcc,
Matrix93* B, Matrix93* C) {
// Do update
deltaTij_ += dt;
deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt, A, B, C);
zeta_ = updatedZeta(measuredAcc, measuredOmega, dt, A, B, C);
// D_plus_abias = D_plus_zeta * D_zeta_abias + D_plus_a * D_a_abias
zeta_H_biasAcc_ = (*A) * zeta_H_biasAcc_ - (*B);

79
gtsam/navigation/PreintegrationBase.h
View File

@ -60,44 +60,6 @@ class PreintegrationBase {
typedef imuBias::ConstantBias Bias;
typedef PreintegrationParams Params;
/// The IMU is integrated in the tangent space, represented by a Vector9
/// This small inner class provides some convenient constructors and efficient
/// access to the orientation, position, and velocity components
class TangentVector {
Vector9 v_;
typedef const Vector9 constV9;
public:
TangentVector() { v_.setZero(); }
TangentVector(const Vector9& v) : v_(v) {}
TangentVector(const Vector3& theta, const Vector3& pos,
const Vector3& vel) {
this->theta() = theta;
this->position() = pos;
this->velocity() = vel;
}
const Vector9& vector() const { return v_; }
Eigen::Block<Vector9, 3, 1> theta() { return v_.segment<3>(0); }
Eigen::Block<constV9, 3, 1> theta() const { return v_.segment<3>(0); }
Eigen::Block<Vector9, 3, 1> position() { return v_.segment<3>(3); }
Eigen::Block<constV9, 3, 1> position() const { return v_.segment<3>(3); }
Eigen::Block<Vector9, 3, 1> velocity() { return v_.segment<3>(6); }
Eigen::Block<constV9, 3, 1> velocity() const { return v_.segment<3>(6); }
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & bs::make_nvp("v_", bs::make_array(v_.data(), v_.size()));
}
};
protected:
/// Parameters. Declared mutable only for deprecated predict method.
@ -108,7 +70,7 @@ class PreintegrationBase {
boost::shared_ptr<Params> p_;
/// Acceleration and gyro bias used for preintegration
imuBias::ConstantBias biasHat_;
Bias biasHat_;
/// Time interval from i to j
double deltaTij_;
@ -118,8 +80,7 @@ class PreintegrationBase {
* Note: relative position does not take into account velocity at time i, see deltap+, in [2]
* Note: velocity is now also in frame i, as opposed to deltaVij in [2]
*/
/// Current estimate of deltaXij_k
TangentVector deltaXij_;
Vector9 zeta_;
Matrix93 zeta_H_biasAcc_; ///< Jacobian of preintegrated zeta w.r.t. acceleration bias
Matrix93 zeta_H_biasOmega_; ///< Jacobian of preintegrated zeta w.r.t. angular rate bias
@ -175,14 +136,14 @@ public:
const imuBias::ConstantBias& biasHat() const { return biasHat_; }
const double& deltaTij() const { return deltaTij_; }
const Vector9& zeta() const { return deltaXij_.vector(); }
const Vector9& zeta() const { return zeta_; }
Vector3 theta() const { return deltaXij_.theta(); }
Vector3 deltaPij() const { return deltaXij_.position(); }
Vector3 deltaVij() const { return deltaXij_.velocity(); }
Vector3 theta() const { return zeta_.head<3>(); }
Vector3 deltaPij() const { return zeta_.segment<3>(3); }
Vector3 deltaVij() const { return zeta_.tail<3>(); }
Rot3 deltaRij() const { return Rot3::Expmap(deltaXij_.theta()); }
NavState deltaXij() const { return NavState::Retract(deltaXij_.vector()); }
Rot3 deltaRij() const { return Rot3::Expmap(theta()); }
NavState deltaXij() const { return NavState::Retract(zeta_); }
const Matrix93& zeta_H_biasAcc() const { return zeta_H_biasAcc_; }
const Matrix93& zeta_H_biasOmega() const { return zeta_H_biasOmega_; }
@ -212,20 +173,18 @@ public:
// Update integrated vector on tangent manifold zeta with acceleration
// readings a_body and gyro readings w_body, bias-corrected in body frame.
static TangentVector UpdateEstimate(const Vector3& a_body,
const Vector3& w_body, double dt,
const TangentVector& zeta,
OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none);
static Vector9 UpdateEstimate(const Vector3& a_body, const Vector3& w_body,
double dt, const Vector9& zeta,
OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none);
/// Calculate the updated preintegrated measurement, does not modify
/// It takes measured quantities in the j frame
PreintegrationBase::TangentVector updatedDeltaXij(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt,
OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none) const;
Vector9 updatedZeta(const Vector3& measuredAcc, const Vector3& measuredOmega,
double dt, OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none) const;
/// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame
@ -284,9 +243,9 @@ private:
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & bs::make_nvp("zeta_", bs::make_array(zeta_.data(), zeta_.size()));
ar & bs::make_nvp("zeta_H_biasAcc_", bs::make_array(zeta_H_biasAcc_.data(), zeta_H_biasAcc_.size()));
ar & bs::make_nvp("zeta_H_biasOmega_", bs::make_array(zeta_H_biasOmega_.data(), zeta_H_biasOmega_.size()));
}

4
gtsam/navigation/tests/testPreintegrationBase.cpp
View File

@ -39,9 +39,7 @@ static boost::shared_ptr<PreintegrationBase::Params> defaultParams() {
}
Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
PreintegrationBase::TangentVector zeta_plus =
PreintegrationBase::UpdateEstimate(a, w, kDt, zeta);
return zeta_plus.vector();
return PreintegrationBase::UpdateEstimate(a, w, kDt, zeta);
}
/* ************************************************************************* */