BIG: switch to NavState delta pose
dellaert
parent
e3d36da188
commit
325ede23fe
|
|
@ -61,16 +61,17 @@ void PreintegratedImuMeasurements::resetIntegration() {
|
|||
#define D_v_v(H) (H)->block<3,3>(6,6)
|
||||
//------------------------------------------------------------------------------
|
||||
void PreintegratedImuMeasurements::integrateMeasurement(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
|
||||
OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt,
|
||||
OptionalJacobian<9, 9> outF, OptionalJacobian<9, 9> G) {
|
||||
|
||||
const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test
|
||||
static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished();
|
||||
|
||||
// Update preintegrated measurements (also get Jacobian)
|
||||
Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
|
||||
Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
updatePreintegratedMeasurements(measuredAcc, measuredOmega, deltaT,
|
||||
&D_incrR_integratedOmega, &F);
|
||||
Matrix93 G1, G2;
|
||||
updatePreintegratedMeasurements(measuredAcc, measuredOmega, dt,
|
||||
&D_incrR_integratedOmega, &F, &G1, &G2);
|
||||
|
||||
// first order covariance propagation:
|
||||
// as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF
|
||||
|
|
@ -78,35 +79,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
|
|||
// preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G'
|
||||
// NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise
|
||||
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
|
||||
// NOTE 2: computation of G * (1/deltaT) * measurementCovariance * G.transpose() done block-wise,
|
||||
// as G and measurementCovariance are block-diagonal matrices
|
||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose();
|
||||
D_R_R(&preintMeasCov_) += D_incrR_integratedOmega * p().gyroscopeCovariance
|
||||
* D_incrR_integratedOmega.transpose() * deltaT;
|
||||
D_t_t(&preintMeasCov_) += p().integrationCovariance * deltaT;
|
||||
D_v_v(&preintMeasCov_) += dRij * p().accelerometerCovariance * dRij.transpose() * deltaT;
|
||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose()
|
||||
+ G1 * (p().accelerometerCovariance / dt) * G1.transpose()
|
||||
+ Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt)
|
||||
+ G2 * (p().gyroscopeCovariance / dt) * G2.transpose();
|
||||
|
||||
Matrix3 F_pos_noiseacc;
|
||||
F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT;
|
||||
D_t_t(&preintMeasCov_) += (1 / deltaT) * F_pos_noiseacc
|
||||
* p().accelerometerCovariance * F_pos_noiseacc.transpose();
|
||||
Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance
|
||||
* dRij.transpose(); // has 1/deltaT
|
||||
D_t_v(&preintMeasCov_) += temp;
|
||||
D_v_t(&preintMeasCov_) += temp.transpose();
|
||||
|
||||
// F_test and G_test are given as output for testing purposes and are not needed by the factor
|
||||
if (F_test) {
|
||||
(*F_test) << F;
|
||||
}
|
||||
if (G_test) {
|
||||
// This in only for testing & documentation, while the actual computation is done block-wise
|
||||
// omegaNoise intNoise accNoise
|
||||
(*G_test) <<
|
||||
D_incrR_integratedOmega * deltaT, Z_3x3, Z_3x3, // angle
|
||||
Z_3x3, I_3x3 * deltaT, F_pos_noiseacc, // pos
|
||||
Z_3x3, Z_3x3, dRij * deltaT; // vel
|
||||
}
|
||||
if (outF) *outF = F;
|
||||
if (G) *G << G2, Gi*dt, G1; // NOTE(frank): order here is R,P,V
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
PreintegratedImuMeasurements::PreintegratedImuMeasurements(
|
||||
|
|
|
|||
|
|
@ -96,13 +96,13 @@ public:
|
|||
* Add a single IMU measurement to the preintegration.
|
||||
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
|
||||
* @param measuredOmega Measured angular velocity (as given by the sensor)
|
||||
* @param deltaT Time interval between this and the last IMU measurement
|
||||
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
|
||||
* @param Fout, Gout Jacobians used internally (only needed for testing)
|
||||
* @param dt Time interval between this and the last IMU measurement
|
||||
* @param F, F Jacobians used internally (only needed for testing)
|
||||
*/
|
||||
void integrateMeasurement(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, double deltaT,
|
||||
OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none);
|
||||
const Vector3& measuredOmega, double dt, //
|
||||
OptionalJacobian<9, 9> F = boost::none, //
|
||||
OptionalJacobian<9, 9> G = boost::none);
|
||||
|
||||
/// Return pre-integrated measurement covariance
|
||||
Matrix preintMeasCov() const { return preintMeasCov_; }
|
||||
|
|
|
|||
|
|
@ -322,14 +322,14 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt,
|
|||
OptionalJacobian<9, 9> H2) const {
|
||||
const Rot3& nRb = R_;
|
||||
const Velocity3& n_v = v_; // derivative is Ri !
|
||||
const double dt2 = dt * dt;
|
||||
const double dt22 = 0.5 * dt * dt;
|
||||
|
||||
Vector9 xi;
|
||||
Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri;
|
||||
dR(xi) = dR(pim);
|
||||
dP(xi) = dP(pim)
|
||||
+ dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0)
|
||||
+ (0.5 * dt2) * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0);
|
||||
+ dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0);
|
||||
dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0);
|
||||
|
||||
if (omegaCoriolis) {
|
||||
|
|
@ -342,7 +342,7 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt,
|
|||
if (H1) {
|
||||
if (!omegaCoriolis)
|
||||
H1->setZero(); // if coriolis H1 is already initialized
|
||||
D_t_R(H1) += dt * D_dP_Ri1 + (0.5 * dt2) * D_dP_Ri2;
|
||||
D_t_R(H1) += dt * D_dP_Ri1 + dt22 * D_dP_Ri2;
|
||||
D_t_v(H1) += dt * D_dP_nv * Ri;
|
||||
D_v_R(H1) += dt * D_dV_Ri;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -29,10 +29,8 @@ namespace gtsam {
|
|||
/// Re-initialize PreintegratedMeasurements
|
||||
void PreintegrationBase::resetIntegration() {
|
||||
deltaTij_ = 0.0;
|
||||
deltaRij_ = Rot3();
|
||||
deltaXij_ = NavState();
|
||||
delRdelBiasOmega_ = Z_3x3;
|
||||
deltaPij_ = Vector3::Zero();
|
||||
deltaVij_ = Vector3::Zero();
|
||||
delPdelBiasAcc_ = Z_3x3;
|
||||
delPdelBiasOmega_ = Z_3x3;
|
||||
delVdelBiasAcc_ = Z_3x3;
|
||||
|
|
@ -43,21 +41,19 @@ void PreintegrationBase::resetIntegration() {
|
|||
void PreintegrationBase::print(const string& s) const {
|
||||
cout << s << endl;
|
||||
cout << " deltaTij [" << deltaTij_ << "]" << endl;
|
||||
cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
|
||||
cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
|
||||
cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
|
||||
cout << " deltaRij.ypr = (" << deltaRij().ypr().transpose() << ")" << endl;
|
||||
cout << " deltaPij [ " << deltaPij().transpose() << " ]" << endl;
|
||||
cout << " deltaVij [ " << deltaVij().transpose() << " ]" << endl;
|
||||
biasHat_.print(" biasHat");
|
||||
}
|
||||
|
||||
/// Needed for testable
|
||||
bool PreintegrationBase::equals(const PreintegrationBase& other,
|
||||
double tol) const {
|
||||
return deltaRij_.equals(other.deltaRij_, tol)
|
||||
&& fabs(deltaTij_ - other.deltaTij_) < tol
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
|
||||
return fabs(deltaTij_ - other.deltaTij_) < tol
|
||||
&& deltaXij_.equals(other.deltaXij_, tol)
|
||||
&& biasHat_.equals(other.biasHat_, tol)
|
||||
&& equal_with_abs_tol(deltaPij_, other.deltaPij_, tol)
|
||||
&& equal_with_abs_tol(deltaVij_, other.deltaVij_, tol)
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
|
||||
|
|
@ -65,8 +61,8 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
|
|||
}
|
||||
|
||||
void PreintegrationBase::updatePreintegratedMeasurements(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega,
|
||||
const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* F) {
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
|
||||
Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2) {
|
||||
|
||||
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
|
||||
// (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
|
||||
|
|
@ -91,43 +87,31 @@ void PreintegrationBase::updatePreintegratedMeasurements(
|
|||
correctedAcc = bRs * correctedAcc - correctedOmega.cross(b_velocity_bs);
|
||||
}
|
||||
|
||||
// Calculate acceleration in *current* i frame, i.e., before rotation update below
|
||||
// Save current rotation for updating Jacobians
|
||||
const Rot3 oldRij = deltaXij_.attitude();
|
||||
|
||||
// Do update in one fell swoop
|
||||
deltaTij_ += dt;
|
||||
deltaXij_ = deltaXij_.update(correctedAcc, correctedOmega, dt, F, G1, G2);
|
||||
|
||||
// Update Jacobians
|
||||
// TODO(frank): we are repeating some computation here: accessible in F ?
|
||||
Matrix3 D_acc_R;
|
||||
const Matrix3 dRij = deltaRij_.matrix(); // expensive
|
||||
const Vector3 i_acc = deltaRij_.rotate(correctedAcc, D_acc_R);
|
||||
oldRij.rotate(correctedAcc, D_acc_R);
|
||||
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
|
||||
|
||||
// NavState iTj(deltaRij_, deltaPij_, deltaVij_);
|
||||
// iTj = iTj.update();
|
||||
|
||||
// rotation vector describing rotation increment computed from the
|
||||
// current rotation rate measurement
|
||||
const Vector3 integratedOmega = correctedOmega * deltaT;
|
||||
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
|
||||
|
||||
// Update deltaTij and rotation
|
||||
deltaTij_ += deltaT;
|
||||
Matrix3 D_Rij_incrR;
|
||||
deltaRij_ = deltaRij_.compose(incrR, D_Rij_incrR);
|
||||
|
||||
// Update Jacobian
|
||||
const Vector3 integratedOmega = correctedOmega * dt;
|
||||
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
|
||||
const Matrix3 incrRt = incrR.transpose();
|
||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
|
||||
- *D_incrR_integratedOmega * deltaT;
|
||||
- *D_incrR_integratedOmega * dt;
|
||||
|
||||
double dt22 = 0.5 * deltaT * deltaT;
|
||||
deltaPij_ += dt22 * i_acc + deltaT * deltaVij_;
|
||||
deltaVij_ += deltaT * i_acc;
|
||||
|
||||
*F << // angle pos vel
|
||||
D_Rij_incrR, Z_3x3, Z_3x3, // angle
|
||||
dt22 * D_acc_R, I_3x3, I_3x3 * deltaT, // pos
|
||||
deltaT * D_acc_R, Z_3x3, I_3x3; // vel
|
||||
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - dt22 * dRij;
|
||||
delPdelBiasOmega_ += deltaT * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
|
||||
delVdelBiasAcc_ += -dRij * deltaT;
|
||||
delVdelBiasOmega_ += D_acc_biasOmega * deltaT;
|
||||
double dt22 = 0.5 * dt * dt;
|
||||
const Matrix3 dRij = oldRij.matrix(); // expensive
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
|
||||
delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
|
||||
delVdelBiasAcc_ += -dRij * dt;
|
||||
delVdelBiasOmega_ += D_acc_biasOmega * dt;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -135,24 +119,26 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
|
|||
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
|
||||
// Correct deltaRij, derivative is delRdelBiasOmega_
|
||||
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
|
||||
Matrix3 D_deltaRij_bias;
|
||||
Matrix3 D_correctedRij_bias;
|
||||
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
|
||||
const Rot3 deltaRij = deltaRij_.expmap(biasInducedOmega, boost::none, H ? &D_deltaRij_bias : 0);
|
||||
if (H) D_deltaRij_bias *= delRdelBiasOmega_;
|
||||
const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none,
|
||||
H ? &D_correctedRij_bias : 0);
|
||||
if (H)
|
||||
D_correctedRij_bias *= delRdelBiasOmega_;
|
||||
|
||||
Vector9 xi;
|
||||
Matrix3 D_dR_deltaRij;
|
||||
Matrix3 D_dR_correctedRij;
|
||||
// TODO(frank): could line below be simplified? It is equivalent to
|
||||
// LogMap(deltaRij_.compose(Expmap(delRdelBiasOmega_ * biasIncr.gyroscope())))
|
||||
NavState::dR(xi) = Rot3::Logmap(deltaRij, H ? &D_dR_deltaRij : 0);
|
||||
NavState::dP(xi) = deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer()
|
||||
NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
|
||||
NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
|
||||
+ delPdelBiasOmega_ * biasIncr.gyroscope();
|
||||
NavState::dV(xi) = deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer()
|
||||
NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer()
|
||||
+ delVdelBiasOmega_ * biasIncr.gyroscope();
|
||||
|
||||
if (H) {
|
||||
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
|
||||
D_dR_bias << Z_3x3, D_dR_deltaRij * D_deltaRij_bias;
|
||||
D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias;
|
||||
D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
|
||||
D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
|
||||
(*H) << D_dR_bias, D_dP_bias, D_dV_bias;
|
||||
|
|
|
|||
|
|
@ -100,9 +100,14 @@ public:
|
|||
|
||||
protected:
|
||||
|
||||
double deltaTij_; ///< Time interval from i to j
|
||||
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
|
||||
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
||||
double deltaTij_; ///< Time interval from i to j
|
||||
|
||||
/**
|
||||
* Preintegrated navigation state, from frame i to frame j
|
||||
* 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]
|
||||
*/
|
||||
NavState deltaXij_;
|
||||
|
||||
/// Parameters
|
||||
boost::shared_ptr<Params> p_;
|
||||
|
|
@ -110,12 +115,10 @@ protected:
|
|||
/// Acceleration and gyro bias used for preintegration
|
||||
imuBias::ConstantBias biasHat_;
|
||||
|
||||
Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
|
||||
Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
|
||||
|
||||
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
|
||||
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
||||
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
|
||||
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
|
||||
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
|
||||
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
|
||||
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
|
||||
|
||||
/// Default constructor for serialization
|
||||
|
|
@ -147,19 +150,19 @@ public:
|
|||
return deltaTij_;
|
||||
}
|
||||
const Rot3& deltaRij() const {
|
||||
return deltaRij_;
|
||||
return deltaXij_.attitude();
|
||||
}
|
||||
const Matrix3& delRdelBiasOmega() const {
|
||||
return delRdelBiasOmega_;
|
||||
Vector3 deltaPij() const {
|
||||
return deltaXij_.position().vector();
|
||||
}
|
||||
Vector3 deltaVij() const {
|
||||
return deltaXij_.velocity();
|
||||
}
|
||||
const imuBias::ConstantBias& biasHat() const {
|
||||
return biasHat_;
|
||||
}
|
||||
const Vector3& deltaPij() const {
|
||||
return deltaPij_;
|
||||
}
|
||||
const Vector3& deltaVij() const {
|
||||
return deltaVij_;
|
||||
const Matrix3& delRdelBiasOmega() const {
|
||||
return delRdelBiasOmega_;
|
||||
}
|
||||
const Matrix3& delPdelBiasAcc() const {
|
||||
return delPdelBiasAcc_;
|
||||
|
|
@ -188,7 +191,7 @@ public:
|
|||
/// Update preintegrated measurements
|
||||
void updatePreintegratedMeasurements(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, const double deltaT,
|
||||
Matrix3* D_incrR_integratedOmega, Matrix9* F);
|
||||
Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2);
|
||||
|
||||
/// Given the estimate of the bias, return a NavState tangent vector
|
||||
/// summarizing the preintegrated IMU measurements so far
|
||||
|
|
@ -220,11 +223,9 @@ private:
|
|||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(p_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(biasHat_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaPij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaVij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@ using symbol_shorthand::B;
|
|||
static const Vector3 kGravityAlongNavZDown(0, 0, 9.81);
|
||||
static const Vector3 kZeroOmegaCoriolis(0, 0, 0);
|
||||
static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1);
|
||||
static const imuBias::ConstantBias kZeroBiasHat, kZeroBias;
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace {
|
||||
|
|
@ -139,46 +140,51 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
|
|||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, StraightLine) {
|
||||
// Set up IMU measurements
|
||||
static const double g = 10; // make this easy to check
|
||||
static const double a = 0.2, dt = 3.0;
|
||||
const double exact = dt * dt / 2;
|
||||
Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0);
|
||||
static const double g = 10; // make gravity 10 to make this easy to check
|
||||
static const double v = 5.0, a = 0.2, dt = 3.0;
|
||||
const double dt22 = dt * dt / 2;
|
||||
|
||||
// Set up body pointing towards y axis, and start at 10,20,0 with zero velocity
|
||||
// TODO(frank): clean up Rot3 mess
|
||||
// Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X
|
||||
// The body itself has Z axis pointing down
|
||||
static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
static const Point3 initial_position(10, 20, 0);
|
||||
static const NavState state1(nRb, initial_position, Velocity3(0, 0, 0));
|
||||
static const Vector3 initial_velocity(v,0,0);
|
||||
static const NavState state1(nRb, initial_position, initial_velocity);
|
||||
|
||||
imuBias::ConstantBias biasHat, bias;
|
||||
// set up acceleration in X direction, no angular velocity.
|
||||
// Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z
|
||||
Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0);
|
||||
|
||||
// Create parameters assuming nav frame has Z up
|
||||
boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
|
||||
PreintegratedImuMeasurements::Params::MakeSharedU(g); // Up convention!
|
||||
PreintegratedImuMeasurements::Params::MakeSharedU(g);
|
||||
|
||||
PreintegratedImuMeasurements pim(p, biasHat);
|
||||
// Now, preintegrate for 3 seconds, in 10 steps
|
||||
PreintegratedImuMeasurements pim(p, kZeroBiasHat);
|
||||
for (size_t i = 0; i < 10; i++)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10);
|
||||
|
||||
// Check integrated quantities in body frame: gravity measured by IMU is integrated!
|
||||
Vector3 b_deltaP(a * dt22, 0, -g * dt22);
|
||||
Vector3 b_deltaV(a * dt, 0, -g * dt);
|
||||
EXPECT(assert_equal(Rot3(), pim.deltaRij()));
|
||||
EXPECT(assert_equal(Vector3(a * exact, 0, -g * exact), pim.deltaPij()));
|
||||
EXPECT(assert_equal(Vector3(a * dt, 0, -g * dt), pim.deltaVij()));
|
||||
EXPECT(assert_equal(b_deltaP, pim.deltaPij()));
|
||||
EXPECT(assert_equal(b_deltaV, pim.deltaVij()));
|
||||
|
||||
// Check bias-corrected delta: also in body frame
|
||||
Vector9 expectedBC;
|
||||
expectedBC << 0, 0, 0, a * exact, 0, -g * exact, a * dt, 0, -g * dt;
|
||||
EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(bias)));
|
||||
expectedBC << Vector3(0, 0, 0), b_deltaP, b_deltaV;
|
||||
EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(kZeroBias)));
|
||||
|
||||
// Check prediction, note we move along x in body, along y in nav
|
||||
NavState expected(nRb, Point3(10, 20 + a * exact, 0),
|
||||
Velocity3(0, a * dt, 0));
|
||||
EXPECT(assert_equal(expected, pim.predict(state1, bias)));
|
||||
// Check prediction in nav, note we move along x in body, along y in nav
|
||||
Point3 expected_position(10 + v*dt, 20 + a * dt22, 0);
|
||||
Velocity3 expected_velocity(v, a * dt, 0);
|
||||
NavState expected(nRb, expected_position, expected_velocity);
|
||||
EXPECT(assert_equal(expected, pim.predict(state1, kZeroBias)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, PreintegratedMeasurements) {
|
||||
// Linearization point
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredAcc(0.1, 0.0, 0.0);
|
||||
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
|
||||
|
|
@ -192,7 +198,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
|
|||
double expectedDeltaT1(0.5);
|
||||
|
||||
// Actual preintegrated values
|
||||
PreintegratedImuMeasurements actual1(bias, kMeasuredAccCovariance,
|
||||
PreintegratedImuMeasurements actual1(kZeroBiasHat, kMeasuredAccCovariance,
|
||||
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
|
||||
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
|
|
@ -222,7 +228,6 @@ TEST(ImuFactor, PreintegratedMeasurements) {
|
|||
/* ************************************************************************* */
|
||||
// Common linearization point and measurements for tests
|
||||
namespace common {
|
||||
static const imuBias::ConstantBias biasHat, bias; // Bias
|
||||
static const Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
|
||||
Point3(5.0, 1.0, 0));
|
||||
static const Vector3 v1(Vector3(0.5, 0.0, 0.0));
|
||||
|
|
@ -252,28 +257,28 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
|
|||
p->integrationCovariance = kIntegrationErrorCovariance;
|
||||
p->use2ndOrderCoriolis = true;
|
||||
|
||||
PreintegratedImuMeasurements pim(p, bias);
|
||||
PreintegratedImuMeasurements pim(p, kZeroBiasHat);
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// biasCorrectedDelta
|
||||
Matrix96 actualH;
|
||||
pim.biasCorrectedDelta(bias, actualH);
|
||||
pim.biasCorrectedDelta(kZeroBias, actualH);
|
||||
Matrix expectedH = numericalDerivative11<Vector9, imuBias::ConstantBias>(
|
||||
boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
|
||||
boost::none), bias);
|
||||
boost::none), kZeroBias);
|
||||
EXPECT(assert_equal(expectedH, actualH));
|
||||
|
||||
Matrix9 aH1;
|
||||
Matrix96 aH2;
|
||||
NavState predictedState = pim.predict(state1, bias, aH1, aH2);
|
||||
NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2);
|
||||
Matrix eH1 = numericalDerivative11<NavState, NavState>(
|
||||
boost::bind(&PreintegrationBase::predict, pim, _1, bias, boost::none,
|
||||
boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none,
|
||||
boost::none), state1);
|
||||
EXPECT(assert_equal(eH1, aH1));
|
||||
Matrix eH2 = numericalDerivative11<NavState, imuBias::ConstantBias>(
|
||||
boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none,
|
||||
boost::none), bias);
|
||||
boost::none), kZeroBias);
|
||||
EXPECT(assert_equal(eH2, aH2));
|
||||
return;
|
||||
|
||||
|
|
@ -282,11 +287,11 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
|
|||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, ErrorAndJacobians) {
|
||||
using namespace common;
|
||||
PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance,
|
||||
PreintegratedImuMeasurements pim(kZeroBiasHat, kMeasuredAccCovariance,
|
||||
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
|
||||
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
EXPECT(assert_equal(state2, pim.predict(state1, bias)));
|
||||
EXPECT(assert_equal(state2, pim.predict(state1, kZeroBias)));
|
||||
|
||||
// Create factor
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
|
||||
|
|
@ -296,14 +301,14 @@ TEST(ImuFactor, ErrorAndJacobians) {
|
|||
Vector expectedError(9);
|
||||
expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0;
|
||||
EXPECT(
|
||||
assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, bias)));
|
||||
assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, kZeroBias)));
|
||||
|
||||
Values values;
|
||||
values.insert(X(1), x1);
|
||||
values.insert(V(1), v1);
|
||||
values.insert(X(2), x2);
|
||||
values.insert(V(2), v2);
|
||||
values.insert(B(1), bias);
|
||||
values.insert(B(1), kZeroBias);
|
||||
EXPECT(assert_equal(expectedError, factor.unwhitenedError(values)));
|
||||
|
||||
// Make sure linearization is correct
|
||||
|
|
@ -312,16 +317,16 @@ TEST(ImuFactor, ErrorAndJacobians) {
|
|||
|
||||
// Actual Jacobians
|
||||
Matrix H1a, H2a, H3a, H4a, H5a;
|
||||
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
|
||||
(void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a, H5a);
|
||||
|
||||
// Make sure rotation part is correct when error is interpreted as axis-angle
|
||||
// Jacobians are around zero, so the rotation part is the same as:
|
||||
Matrix H1Rot3 = numericalDerivative11<Rot3, Pose3>(
|
||||
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
|
||||
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), x1);
|
||||
EXPECT(assert_equal(H1Rot3, H1a.topRows(3)));
|
||||
|
||||
Matrix H3Rot3 = numericalDerivative11<Rot3, Pose3>(
|
||||
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
|
||||
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), x2);
|
||||
EXPECT(assert_equal(H3Rot3, H3a.topRows(3)));
|
||||
|
||||
// Evaluate error with wrong values
|
||||
|
|
@ -330,7 +335,7 @@ TEST(ImuFactor, ErrorAndJacobians) {
|
|||
expectedError << 0, 0, 0, 0, 0, 0, -0.0724744871, -0.040715657, -0.151952901;
|
||||
EXPECT(
|
||||
assert_equal(expectedError,
|
||||
factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-2));
|
||||
factor.evaluateError(x1, v1, x2, v2_wrong, kZeroBias), 1e-2));
|
||||
EXPECT(assert_equal(expectedError, factor.unwhitenedError(values), 1e-2));
|
||||
|
||||
// Make sure the whitening is done correctly
|
||||
|
|
@ -503,9 +508,6 @@ TEST(ImuFactor, fistOrderExponential) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
|
||||
// Linearization point
|
||||
imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
|
||||
|
||||
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
|
||||
|
||||
// Measurements
|
||||
|
|
@ -526,28 +528,28 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
|
|||
|
||||
// Actual preintegrated values
|
||||
PreintegratedImuMeasurements preintegrated =
|
||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
||||
evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas,
|
||||
deltaTs);
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelPdelBias = numericalDerivative11<Vector,
|
||||
imuBias::ConstantBias>(
|
||||
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs,
|
||||
measuredOmegas, deltaTs), bias);
|
||||
measuredOmegas, deltaTs), kZeroBias);
|
||||
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
|
||||
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
|
||||
|
||||
Matrix expectedDelVdelBias = numericalDerivative11<Vector,
|
||||
imuBias::ConstantBias>(
|
||||
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs,
|
||||
measuredOmegas, deltaTs), bias);
|
||||
measuredOmegas, deltaTs), kZeroBias);
|
||||
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
|
||||
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
|
||||
|
||||
Matrix expectedDelRdelBias =
|
||||
numericalDerivative11<Rot3, imuBias::ConstantBias>(
|
||||
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
|
||||
measuredAccs, measuredOmegas, deltaTs), bias);
|
||||
measuredAccs, measuredOmegas, deltaTs), kZeroBias);
|
||||
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
|
||||
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
|
||||
|
||||
|
|
@ -565,9 +567,6 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
|
||||
// Linearization point
|
||||
imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
|
||||
|
||||
// Measurements
|
||||
const Vector3 measuredAcc(0.1, 0.0, 0.0);
|
||||
const Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
|
||||
|
|
@ -587,20 +586,19 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
|
|||
deltaTs.push_back(deltaT);
|
||||
}
|
||||
// Actual preintegrated values
|
||||
PreintegratedImuMeasurements preintegrated =
|
||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
||||
deltaTs);
|
||||
PreintegratedImuMeasurements pim = evaluatePreintegratedMeasurements(
|
||||
kZeroBias, measuredAccs, measuredOmegas, deltaTs);
|
||||
|
||||
// so far we only created a nontrivial linearization point for the preintegrated measurements
|
||||
// Now we add a new measurement and ask for Jacobians
|
||||
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
|
||||
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
|
||||
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
|
||||
const Rot3 deltaRij_old = pim.deltaRij(); // before adding new measurement
|
||||
const Vector3 deltaVij_old = pim.deltaVij(); // before adding new measurement
|
||||
const Vector3 deltaPij_old = pim.deltaPij(); // before adding new measurement
|
||||
|
||||
Matrix oldPreintCovariance = preintegrated.preintMeasCov();
|
||||
Matrix oldPreintCovariance = pim.preintMeasCov();
|
||||
|
||||
Matrix Factual, Gactual;
|
||||
preintegrated.integrateMeasurement(measuredAcc, measuredOmega, deltaT,
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT,
|
||||
Factual, Gactual);
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
|
@ -673,15 +671,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
|
|||
* Factual.transpose()
|
||||
+ (1 / deltaT) * Gactual * measurementCovariance * Gactual.transpose();
|
||||
|
||||
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
|
||||
Matrix newPreintCovarianceActual = pim.preintMeasCov();
|
||||
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
|
||||
// Linearization point
|
||||
imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
|
||||
|
||||
// Measurements
|
||||
list<Vector3> measuredAccs, measuredOmegas;
|
||||
list<double> deltaTs;
|
||||
|
|
@ -699,7 +694,7 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
|
|||
}
|
||||
// Actual preintegrated values
|
||||
PreintegratedImuMeasurements preintegrated =
|
||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
||||
evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas,
|
||||
deltaTs);
|
||||
|
||||
// so far we only created a nontrivial linearization point for the preintegrated measurements
|
||||
|
|
|
|||
Loading…
Reference in New Issue