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

Inner class

release/4.3a0
Frank 2016-01-25 13:33:17 -08:00
parent 2542d20367
commit 91482a7e2b
3 changed files with 55 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
gtsam/navigation/AggregateImuReadings.cpp
View File

@ -27,45 +27,24 @@ namespace gtsam {
AggregateImuReadings::AggregateImuReadings(const boost::shared_ptr<Params>& p,
const Bias& estimatedBias)
: p_(p), estimatedBias_(estimatedBias), deltaTij_(0.0) {
zeta_.setZero();
cov_.setZero();
}
// Tangent space sugar.
namespace sugar {
static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) { return v.segment<3>(0); }
static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) { return v.segment<3>(3); }
static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) { return v.segment<3>(6); }
typedef const Vector9 constV9;
static Eigen::Block<constV9, 3, 1> dR(constV9& v) { return v.segment<3>(0); }
static Eigen::Block<constV9, 3, 1> dP(constV9& v) { return v.segment<3>(3); }
static Eigen::Block<constV9, 3, 1> dV(constV9& v) { return v.segment<3>(6); }
} // namespace sugar
// See extensive discussion in ImuFactor.lyx
Vector9 AggregateImuReadings::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) {
using namespace sugar;
const Vector3 theta = dR(zeta);
const Vector3 v = dV(zeta);
AggregateImuReadings::TangentVector AggregateImuReadings::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) {
// Calculate exact mean propagation
Matrix3 H;
const Matrix3 R = Rot3::Expmap(theta, H).matrix();
const Matrix3 R = Rot3::Expmap(zeta.theta(), H).matrix();
const Matrix3 invH = H.inverse();
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
Vector9 zetaPlus;
dR(zetaPlus) = dR(zeta) + invH * w_body * dt; // theta
dP(zetaPlus) = dP(zeta) + v * dt + a_nav * dt22; // position
dV(zetaPlus) = dV(zeta) + a_nav * dt; // velocity
TangentVector zetaPlus(zeta.theta() + invH * w_body * dt,
zeta.position() + zeta.velocity() * dt + a_nav * dt22,
zeta.velocity() + a_nav * dt);
if (A) {
// First order (small angle) approximation of derivative of invH*w:
@ -122,17 +101,17 @@ NavState AggregateImuReadings::predict(const NavState& state_i,
const Bias& bias_i,
OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 6> H2) const {
using namespace sugar;
Vector9 zeta = zeta_;
TangentVector zeta = zeta_;
// Correct for initial velocity and gravity
Rot3 Ri = state_i.attitude();
Matrix3 Rit = Ri.transpose();
Vector3 gt = deltaTij_ * p_->n_gravity;
dP(zeta) += Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt);
dV(zeta) += Rit * gt;
zeta.position() +=
Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt);
zeta.velocity() += Rit * gt;
return state_i.retract(zeta);
return state_i.retract(zeta.vector());
}
SharedGaussian AggregateImuReadings::noiseModel() const {

42
gtsam/navigation/AggregateImuReadings.h
View File

@ -17,6 +17,7 @@
#pragma once
#include <Eigen/Dense>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/linear/NoiseModel.h>
@ -32,6 +33,35 @@ class AggregateImuReadings {
typedef imuBias::ConstantBias Bias;
typedef PreintegrationBase::Params 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:
const boost::shared_ptr<Params> p_;
const Bias estimatedBias_;
@ -39,14 +69,15 @@ class AggregateImuReadings {
double deltaTij_; ///< sum of time increments
/// Current estimate of zeta_k
Vector9 zeta_;
TangentVector zeta_;
Matrix9 cov_;
public:
AggregateImuReadings(const boost::shared_ptr<Params>& p,
const Bias& estimatedBias = Bias());
const Vector9& zeta() const { return zeta_; }
Vector3 theta() const { return zeta_.theta(); }
const Vector9& zeta() const { return zeta_.vector(); }
const Matrix9& zetaCov() const { return cov_; }
/**
@ -70,12 +101,11 @@ class AggregateImuReadings {
/// @deprecated: Explicitly calculate covariance
Matrix9 preintMeasCov() const;
Vector3 theta() const { return zeta_.head<3>(); }
// Update integrated vector on tangent manifold zeta with acceleration
// readings a_body and gyro readings w_body, bias-corrected in body frame.
static Vector9 UpdateEstimate(const Vector3& a_body, const Vector3& w_body,
double dt, const Vector9& zeta,
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);

5
gtsam/navigation/tests/testAggregateImuReadings.cpp
View File

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