No more second-order integration flag
dellaert
parent
0bb73bae9e
commit
c6b68e6795
|
|
@ -145,15 +145,16 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
|
|||
const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
|
||||
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit,
|
||||
const bool use2ndOrderIntegration) {
|
||||
if (!use2ndOrderIntegration)
|
||||
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
|
||||
biasHat_ = biasHat;
|
||||
boost::shared_ptr<Params> p = boost::make_shared<Params>();
|
||||
boost::shared_ptr<Params> p = Params::MakeSharedFRD();
|
||||
p->gyroscopeCovariance = measuredOmegaCovariance;
|
||||
p->accelerometerCovariance = measuredAccCovariance;
|
||||
p->integrationCovariance = integrationErrorCovariance;
|
||||
p->biasAccCovariance = biasAccCovariance;
|
||||
p->biasOmegaCovariance = biasOmegaCovariance;
|
||||
p->biasAccOmegaInit = biasAccOmegaInit;
|
||||
p->use2ndOrderIntegration = use2ndOrderIntegration;
|
||||
p_ = p;
|
||||
resetIntegration();
|
||||
preintMeasCov_.setZero();
|
||||
|
|
@ -259,7 +260,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
|
|||
//------------------------------------------------------------------------------
|
||||
CombinedImuFactor::CombinedImuFactor(
|
||||
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
|
||||
const CombinedPreintegratedMeasurements& pim, const Vector3& gravity,
|
||||
const CombinedPreintegratedMeasurements& pim, const Vector3& b_gravity,
|
||||
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
|
||||
const bool use2ndOrderCoriolis)
|
||||
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
|
||||
|
|
@ -267,7 +268,7 @@ CombinedImuFactor::CombinedImuFactor(
|
|||
_PIM_(pim) {
|
||||
boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
|
||||
boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
|
||||
p->gravity = gravity;
|
||||
p->b_gravity = b_gravity;
|
||||
p->omegaCoriolis = omegaCoriolis;
|
||||
p->body_P_sensor = body_P_sensor;
|
||||
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
|
||||
|
|
@ -278,11 +279,11 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
|||
Pose3& pose_j, Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias_i,
|
||||
CombinedPreintegratedMeasurements& pim,
|
||||
const Vector3& gravity,
|
||||
const Vector3& b_gravity,
|
||||
const Vector3& omegaCoriolis,
|
||||
const bool use2ndOrderCoriolis) {
|
||||
// use deprecated predict
|
||||
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, gravity,
|
||||
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity,
|
||||
omegaCoriolis, use2ndOrderCoriolis);
|
||||
pose_j = pvb.pose;
|
||||
vel_j = pvb.velocity;
|
||||
|
|
|
|||
|
|
@ -66,9 +66,26 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
|
|||
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
|
||||
Matrix6 biasAccOmegaInit; ///< covariance of bias used for pre-integration
|
||||
|
||||
Params():biasAccCovariance(I_3x3), biasOmegaCovariance(I_3x3), biasAccOmegaInit(I_6x6) {}
|
||||
/// See two named constructors below for good values of b_gravity in body frame
|
||||
Params(const Vector3& b_gravity) :
|
||||
PreintegrationBase::Params(b_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
|
||||
I_3x3), biasAccOmegaInit(I_6x6) {
|
||||
}
|
||||
|
||||
// Default Params for Front-Right-Down convention: b_gravity points along positive Z-axis
|
||||
static boost::shared_ptr<Params> MakeSharedFRD(double g = 9.81) {
|
||||
return boost::make_shared<Params>(Vector3(0, 0, g));
|
||||
}
|
||||
|
||||
// Default Params for Front-Left-Up convention: b_gravity points along negative Z-axis
|
||||
static boost::shared_ptr<Params> MakeSharedFLU(double g = 9.81) {
|
||||
return boost::make_shared<Params>(Vector3(0, 0, -g));
|
||||
}
|
||||
|
||||
private:
|
||||
/// Default constructor makes unitialized params struct
|
||||
Params() {}
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
|
|
@ -134,12 +151,13 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
|
|||
Matrix preintMeasCov() const { return preintMeasCov_; }
|
||||
|
||||
/// deprecated constructor
|
||||
/// NOTE(frank): assumes FRD convention, only second order integration supported
|
||||
PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
|
||||
const Matrix3& measuredAccCovariance,
|
||||
const Matrix3& measuredOmegaCovariance,
|
||||
const Matrix3& integrationErrorCovariance,
|
||||
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
|
||||
const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = false);
|
||||
const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true);
|
||||
|
||||
private:
|
||||
/// Serialization function
|
||||
|
|
@ -245,7 +263,7 @@ public:
|
|||
/// @deprecated constructor
|
||||
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
|
||||
Key bias_j, const CombinedPreintegratedMeasurements& pim,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
const Vector3& b_gravity, const Vector3& omegaCoriolis,
|
||||
const boost::optional<Pose3>& body_P_sensor = boost::none,
|
||||
const bool use2ndOrderCoriolis = false);
|
||||
|
||||
|
|
@ -253,7 +271,7 @@ public:
|
|||
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
|
||||
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
|
||||
CombinedPreintegratedMeasurements& pim,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
const Vector3& b_gravity, const Vector3& omegaCoriolis,
|
||||
const bool use2ndOrderCoriolis = false);
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -38,8 +38,8 @@ void PreintegratedImuMeasurements::print(const string& s) const {
|
|||
//------------------------------------------------------------------------------
|
||||
bool PreintegratedImuMeasurements::equals(
|
||||
const PreintegratedImuMeasurements& other, double tol) const {
|
||||
return PreintegrationBase::equals(other, tol) &&
|
||||
equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
|
||||
return PreintegrationBase::equals(other, tol)
|
||||
&& equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -51,8 +51,7 @@ void PreintegratedImuMeasurements::resetIntegration() {
|
|||
//------------------------------------------------------------------------------
|
||||
void PreintegratedImuMeasurements::integrateMeasurement(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
|
||||
OptionalJacobian<9, 9> F_test,
|
||||
OptionalJacobian<9, 9> G_test) {
|
||||
OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
|
||||
|
||||
Vector3 correctedAcc, correctedOmega;
|
||||
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
|
||||
|
|
@ -60,16 +59,17 @@ void PreintegratedImuMeasurements::integrateMeasurement(
|
|||
|
||||
// rotation increment computed from the current rotation rate measurement
|
||||
const Vector3 integratedOmega = correctedOmega * deltaT;
|
||||
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
|
||||
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
|
||||
// rotation increment computed from the current rotation rate measurement
|
||||
const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega);
|
||||
|
||||
// Update Jacobians
|
||||
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
|
||||
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr,
|
||||
deltaT);
|
||||
|
||||
// Update preintegrated measurements (also get Jacobian)
|
||||
const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test
|
||||
Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test
|
||||
Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F);
|
||||
|
||||
// first order covariance propagation:
|
||||
|
|
@ -88,14 +88,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
|
|||
* p().gyroscopeCovariance * D_Rincr_integratedOmega.transpose() * deltaT;
|
||||
|
||||
Matrix3 F_pos_noiseacc;
|
||||
if (p().use2ndOrderIntegration) {
|
||||
F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT;
|
||||
preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc
|
||||
* p().accelerometerCovariance * F_pos_noiseacc.transpose();
|
||||
Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance * dRij.transpose(); // has 1/deltaT
|
||||
preintMeasCov_.block<3, 3>(0, 3) += temp;
|
||||
preintMeasCov_.block<3, 3>(3, 0) += temp.transpose();
|
||||
}
|
||||
F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT;
|
||||
preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc
|
||||
* p().accelerometerCovariance * F_pos_noiseacc.transpose();
|
||||
Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance
|
||||
* dRij.transpose(); // has 1/deltaT
|
||||
preintMeasCov_.block<3, 3>(0, 3) += temp;
|
||||
preintMeasCov_.block<3, 3>(3, 0) += temp.transpose();
|
||||
|
||||
// F_test and G_test are given as output for testing purposes and are not needed by the factor
|
||||
if (F_test) {
|
||||
|
|
@ -103,13 +102,10 @@ void PreintegratedImuMeasurements::integrateMeasurement(
|
|||
}
|
||||
if (G_test) {
|
||||
// This in only for testing & documentation, while the actual computation is done block-wise
|
||||
if (!p().use2ndOrderIntegration)
|
||||
F_pos_noiseacc = Z_3x3;
|
||||
|
||||
// intNoise accNoise omegaNoise
|
||||
(*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos
|
||||
Z_3x3, dRij * deltaT, Z_3x3, // vel
|
||||
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
|
||||
(*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos
|
||||
Z_3x3, dRij * deltaT, Z_3x3, // vel
|
||||
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
|
||||
}
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -117,12 +113,13 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
|
|||
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
|
||||
const Matrix3& measuredOmegaCovariance,
|
||||
const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) {
|
||||
if (!use2ndOrderIntegration)
|
||||
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
|
||||
biasHat_ = biasHat;
|
||||
boost::shared_ptr<Params> p = boost::make_shared<Params>();
|
||||
boost::shared_ptr<Params> p = Params::MakeSharedFRD();
|
||||
p->gyroscopeCovariance = measuredOmegaCovariance;
|
||||
p->accelerometerCovariance = measuredAccCovariance;
|
||||
p->integrationCovariance = integrationErrorCovariance;
|
||||
p->use2ndOrderIntegration = use2ndOrderIntegration;
|
||||
p_ = p;
|
||||
resetIntegration();
|
||||
}
|
||||
|
|
@ -131,10 +128,10 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
|
|||
// ImuFactor methods
|
||||
//------------------------------------------------------------------------------
|
||||
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
||||
const PreintegratedImuMeasurements& pim)
|
||||
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
|
||||
pose_j, vel_j, bias),
|
||||
_PIM_(pim) {}
|
||||
const PreintegratedImuMeasurements& pim) :
|
||||
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
|
||||
pose_j, vel_j, bias), _PIM_(pim) {
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
|
||||
|
|
@ -151,7 +148,8 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
|
|||
Base::print("");
|
||||
_PIM_.print(" preintegrated measurements:");
|
||||
// Print standard deviations on covariance only
|
||||
cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl;
|
||||
cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose()
|
||||
<< endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -168,21 +166,19 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
|
|||
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3,
|
||||
boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const {
|
||||
return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
|
||||
H1, H2, H3, H4, H5);
|
||||
H1, H2, H3, H4, H5);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
||||
const PreintegratedMeasurements& pim,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
const boost::optional<Pose3>& body_P_sensor,
|
||||
const bool use2ndOrderCoriolis)
|
||||
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
|
||||
pose_j, vel_j, bias),
|
||||
_PIM_(pim) {
|
||||
boost::shared_ptr<PreintegratedMeasurements::Params> p =
|
||||
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
|
||||
p->gravity = gravity;
|
||||
const PreintegratedMeasurements& pim, const Vector3& b_gravity,
|
||||
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
|
||||
const bool use2ndOrderCoriolis) :
|
||||
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
|
||||
pose_j, vel_j, bias), _PIM_(pim) {
|
||||
boost::shared_ptr<PreintegratedMeasurements::Params> p = boost::make_shared<
|
||||
PreintegratedMeasurements::Params>(pim.p());
|
||||
p->b_gravity = b_gravity;
|
||||
p->omegaCoriolis = omegaCoriolis;
|
||||
p->body_P_sensor = body_P_sensor;
|
||||
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
|
||||
|
|
@ -191,16 +187,14 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
|||
|
||||
//------------------------------------------------------------------------------
|
||||
void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
Pose3& pose_j, Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias_i,
|
||||
PreintegratedMeasurements& pim,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
const bool use2ndOrderCoriolis) {
|
||||
Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
|
||||
PreintegratedMeasurements& pim, const Vector3& b_gravity,
|
||||
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) {
|
||||
// use deprecated predict
|
||||
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, gravity,
|
||||
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity,
|
||||
omegaCoriolis, use2ndOrderCoriolis);
|
||||
pose_j = pvb.pose;
|
||||
vel_j = pvb.velocity;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -108,11 +108,12 @@ public:
|
|||
Matrix preintMeasCov() const { return preintMeasCov_; }
|
||||
|
||||
/// @deprecated constructor
|
||||
/// NOTE(frank): assumes FRD convention, only second order integration supported
|
||||
PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat,
|
||||
const Matrix3& measuredAccCovariance,
|
||||
const Matrix3& measuredOmegaCovariance,
|
||||
const Matrix3& integrationErrorCovariance,
|
||||
bool use2ndOrderIntegration = false);
|
||||
bool use2ndOrderIntegration = true);
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -209,14 +210,14 @@ public:
|
|||
/// @deprecated constructor
|
||||
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
||||
const PreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
const Vector3& b_gravity, const Vector3& omegaCoriolis,
|
||||
const boost::optional<Pose3>& body_P_sensor = boost::none,
|
||||
const bool use2ndOrderCoriolis = false);
|
||||
|
||||
/// @deprecated use PreintegrationBase::predict
|
||||
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
|
||||
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
|
||||
PreintegratedMeasurements& pim, const Vector3& gravity,
|
||||
PreintegratedMeasurements& pim, const Vector3& b_gravity,
|
||||
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -248,25 +248,19 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
|
|||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector9 NavState::predictXi(const Vector9& pim, double dt,
|
||||
const Vector3& gravity, const boost::optional<Vector3>& omegaCoriolis,
|
||||
bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
|
||||
OptionalJacobian<9, 9> H2) const {
|
||||
Vector9 NavState::integrateTangent(const Vector9& pim, double dt,
|
||||
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis,
|
||||
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
|
||||
const Rot3& nRb = R_;
|
||||
const Velocity3& n_v = v_; // derivative is Ri !
|
||||
const double dt2 = dt * dt;
|
||||
|
||||
Vector9 delta;
|
||||
Matrix3 D_dP_Ri, D_dP_dP, D_dV_Ri, D_dV_dV;
|
||||
Matrix3 D_dP_Ri, D_dP_nv;
|
||||
dR(delta) = dR(pim);
|
||||
// TODO(frank):
|
||||
// - why do we integrate n_v here ?
|
||||
// - the dP and dV should be in body ! How come semi-retract works out ??
|
||||
// - should we rename t_ to p_? if not, we should rename dP do dT
|
||||
dP(delta) = nRb.rotate(dP(pim), H1 ? &D_dP_Ri : 0, H2 ? &D_dP_dP : 0)
|
||||
+ n_v * dt + 0.5 * gravity * dt2;
|
||||
dV(delta) = nRb.rotate(dV(pim), H1 ? &D_dV_Ri : 0, H2 ? &D_dV_dV : 0)
|
||||
+ gravity * dt;
|
||||
dP(delta) = dP(pim)
|
||||
+ dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri : 0, H2 ? &D_dP_nv : 0);
|
||||
dV(delta) = dV(pim);
|
||||
|
||||
if (omegaCoriolis) {
|
||||
delta += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1);
|
||||
|
|
@ -278,15 +272,11 @@ Vector9 NavState::predictXi(const Vector9& pim, double dt,
|
|||
if (H1) {
|
||||
if (!omegaCoriolis)
|
||||
H1->setZero(); // if coriolis H1 is already initialized
|
||||
D_t_R(H1) += D_dP_Ri;
|
||||
D_t_v(H1) += I_3x3 * dt * Ri;
|
||||
D_v_R(H1) += D_dV_Ri;
|
||||
D_t_R(H1) += dt * D_dP_Ri;
|
||||
D_t_v(H1) += dt * D_dP_nv * Ri;
|
||||
}
|
||||
if (H2) {
|
||||
H2->setZero();
|
||||
D_R_R(H2) << I_3x3;
|
||||
D_t_t(H2) << D_dP_dP;
|
||||
D_v_v(H2) << D_dV_dV;
|
||||
H2->setIdentity();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -294,13 +284,12 @@ Vector9 NavState::predictXi(const Vector9& pim, double dt,
|
|||
}
|
||||
//------------------------------------------------------------------------------
|
||||
NavState NavState::predict(const Vector9& pim, double dt,
|
||||
const Vector3& gravity, const boost::optional<Vector3>& omegaCoriolis,
|
||||
bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
|
||||
OptionalJacobian<9, 9> H2) const {
|
||||
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis,
|
||||
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
|
||||
|
||||
Matrix9 D_delta_state, D_delta_pim;
|
||||
Vector9 delta = predictXi(pim, dt, gravity, omegaCoriolis,
|
||||
use2ndOrderCoriolis, H1 ? &D_delta_state : 0, H2 ? &D_delta_pim : 0);
|
||||
Vector9 delta = integrateTangent(pim, dt, omegaCoriolis, use2ndOrderCoriolis,
|
||||
H1 ? &D_delta_state : 0, H2 ? &D_delta_pim : 0);
|
||||
|
||||
Matrix9 D_predicted_state, D_predicted_delta;
|
||||
NavState predicted = retract(delta, H1 ? &D_predicted_state : 0,
|
||||
|
|
|
|||
|
|
@ -33,6 +33,9 @@ typedef Vector3 Velocity3;
|
|||
*/
|
||||
class NavState: public LieGroup<NavState, 9> {
|
||||
private:
|
||||
|
||||
// TODO(frank):
|
||||
// - should we rename t_ to p_? if not, we should rename dP do dT
|
||||
Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav
|
||||
Point3 t_; ///< position n_t, in nav frame
|
||||
Velocity3 v_; ///< velocity n_v in nav frame
|
||||
|
|
@ -207,16 +210,16 @@ public:
|
|||
Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
|
||||
OptionalJacobian<9, 9> H = boost::none) const;
|
||||
|
||||
/// Combine preintegrated measurements, in the form of a tangent space vector,
|
||||
/// with gravity, velocity, and Coriolis forces into a tangent space update.
|
||||
Vector9 predictXi(const Vector9& pim, double dt, const Vector3& gravity,
|
||||
/// Integrate a tangent vector forwards on tangent space, taking into account
|
||||
/// Coriolis forces if omegaCoriolis is given.
|
||||
Vector9 integrateTangent(const Vector9& pim, double dt,
|
||||
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
|
||||
false, OptionalJacobian<9, 9> H1 = boost::none,
|
||||
OptionalJacobian<9, 9> H2 = boost::none) const;
|
||||
|
||||
/// Combine preintegrated measurements, in the form of a tangent space vector,
|
||||
/// with gravity, velocity, and Coriolis forces into a new state.
|
||||
NavState predict(const Vector9& pim, double dt, const Vector3& gravity,
|
||||
/// Integrate a tangent vector forwards on tangent space, taking into account
|
||||
/// Coriolis forces if omegaCoriolis is given. Calls retract after to yield a NavState
|
||||
NavState predict(const Vector9& pim, double dt,
|
||||
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
|
||||
false, OptionalJacobian<9, 9> H1 = boost::none,
|
||||
OptionalJacobian<9, 9> H2 = boost::none) const;
|
||||
|
|
|
|||
|
|
@ -64,14 +64,11 @@ void PreintegrationBase::updatePreintegratedMeasurements(
|
|||
OptionalJacobian<9, 9> F) {
|
||||
|
||||
const Matrix3 dRij = deltaRij_.matrix(); // expensive
|
||||
const Vector3 temp = dRij * correctedAcc * deltaT;
|
||||
const Vector3 j_acc = dRij * correctedAcc; // acceleration in current frame
|
||||
|
||||
if (!p().use2ndOrderIntegration) {
|
||||
deltaPij_ += deltaVij_ * deltaT;
|
||||
} else {
|
||||
deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
|
||||
}
|
||||
deltaVij_ += temp;
|
||||
double dt22 = 0.5 * deltaT * deltaT;
|
||||
deltaPij_ += deltaVij_ * deltaT + dt22 * j_acc;
|
||||
deltaVij_ += deltaT * j_acc;
|
||||
|
||||
Matrix3 R_i, F_angles_angles;
|
||||
if (F)
|
||||
|
|
@ -81,10 +78,7 @@ void PreintegrationBase::updatePreintegratedMeasurements(
|
|||
if (F) {
|
||||
const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT;
|
||||
Matrix3 F_pos_angles;
|
||||
if (p().use2ndOrderIntegration)
|
||||
F_pos_angles = 0.5 * F_vel_angles * deltaT;
|
||||
else
|
||||
F_pos_angles = Z_3x3;
|
||||
F_pos_angles = 0.5 * F_vel_angles * deltaT;
|
||||
|
||||
// pos vel angle
|
||||
*F << //
|
||||
|
|
@ -101,13 +95,8 @@ void PreintegrationBase::updatePreintegratedJacobians(
|
|||
const Matrix3 dRij = deltaRij_.matrix(); // expensive
|
||||
const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT
|
||||
* delRdelBiasOmega_;
|
||||
if (!p().use2ndOrderIntegration) {
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
|
||||
} else {
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT;
|
||||
delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5);
|
||||
}
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT;
|
||||
delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5);
|
||||
delVdelBiasAcc_ += -dRij * deltaT;
|
||||
delVdelBiasOmega_ += temp;
|
||||
update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
|
||||
|
|
@ -135,10 +124,9 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
|
|||
//------------------------------------------------------------------------------
|
||||
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;
|
||||
Rot3 deltaRij = PreintegratedRotation::biascorrectedDeltaRij(
|
||||
biasIncr.gyroscope(), H ? &D_deltaRij_bias : 0);
|
||||
Rot3 deltaRij = biascorrectedDeltaRij(biasIncr.gyroscope());
|
||||
|
||||
Vector9 xi;
|
||||
Matrix3 D_dR_deltaRij;
|
||||
|
|
@ -147,9 +135,10 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
|
|||
+ delPdelBiasOmega_ * biasIncr.gyroscope();
|
||||
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_deltaRij * delRdelBiasOmega_;
|
||||
D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
|
||||
D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
|
||||
(*H) << D_dR_bias, D_dP_bias, D_dV_bias;
|
||||
|
|
@ -161,13 +150,23 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
|
|||
NavState PreintegrationBase::predict(const NavState& state_i,
|
||||
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
|
||||
OptionalJacobian<9, 6> H2) const {
|
||||
// correct for bias
|
||||
Matrix96 D_biasCorrected_bias;
|
||||
Vector9 biasCorrected = biasCorrectedDelta(bias_i,
|
||||
H2 ? &D_biasCorrected_bias : 0);
|
||||
|
||||
// integrate on tangent space
|
||||
Matrix9 D_delta_state, D_delta_biasCorrected;
|
||||
Vector9 xi = state_i.predictXi(biasCorrected, deltaTij_, p().gravity,
|
||||
Vector9 xi = state_i.integrateTangent(biasCorrected, deltaTij_,
|
||||
p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
|
||||
H2 ? &D_delta_biasCorrected : 0);
|
||||
|
||||
// Correct for gravity
|
||||
double dt = deltaTij_, dt2 = dt * dt;
|
||||
NavState::dP(xi) += 0.5 * p().b_gravity * dt2;
|
||||
NavState::dV(xi) += p().b_gravity * dt;
|
||||
|
||||
// Use retract to get back to NavState manifold
|
||||
Matrix9 D_predict_state, D_predict_delta;
|
||||
NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
|
||||
if (H1)
|
||||
|
|
@ -313,11 +312,11 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
|
|||
//------------------------------------------------------------------------------
|
||||
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
|
||||
const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
const Vector3& b_gravity, const Vector3& omegaCoriolis,
|
||||
const bool use2ndOrderCoriolis) {
|
||||
// NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
|
||||
boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
|
||||
q->gravity = gravity;
|
||||
q->b_gravity = b_gravity;
|
||||
q->omegaCoriolis = omegaCoriolis;
|
||||
q->use2ndOrderCoriolis = use2ndOrderCoriolis;
|
||||
p_ = q;
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
#include <gtsam/navigation/PreintegratedRotation.h>
|
||||
#include <gtsam/navigation/NavState.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -32,11 +33,16 @@ struct PoseVelocityBias {
|
|||
Pose3 pose;
|
||||
Vector3 velocity;
|
||||
imuBias::ConstantBias bias;
|
||||
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias)
|
||||
: pose(_pose), velocity(_velocity), bias(_bias) {}
|
||||
PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias)
|
||||
: pose(navState.pose()), velocity(navState.velocity()), bias(_bias) {}
|
||||
NavState navState() const { return NavState(pose,velocity);}
|
||||
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
|
||||
const imuBias::ConstantBias _bias) :
|
||||
pose(_pose), velocity(_velocity), bias(_bias) {
|
||||
}
|
||||
PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias) :
|
||||
pose(navState.pose()), velocity(navState.velocity()), bias(_bias) {
|
||||
}
|
||||
NavState navState() const {
|
||||
return NavState(pose, velocity);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -45,29 +51,41 @@ struct PoseVelocityBias {
|
|||
* It includes the definitions of the preintegrated variables and the methods
|
||||
* to access, print, and compare them.
|
||||
*/
|
||||
class PreintegrationBase : public PreintegratedRotation {
|
||||
class PreintegrationBase: public PreintegratedRotation {
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
/// Parameters for pre-integration:
|
||||
/// Usage: Create just a single Params and pass a shared pointer to the constructor
|
||||
struct Params : PreintegratedRotation::Params {
|
||||
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
|
||||
struct Params: PreintegratedRotation::Params {
|
||||
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
|
||||
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
|
||||
/// (to compensate errors in Euler integration)
|
||||
bool use2ndOrderIntegration; ///< Controls the order of integration
|
||||
/// (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
|
||||
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
|
||||
Vector3 gravity; ///< Gravity constant
|
||||
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
|
||||
Vector3 b_gravity; ///< Gravity constant in body frame
|
||||
|
||||
Params()
|
||||
: accelerometerCovariance(I_3x3),
|
||||
integrationCovariance(I_3x3),
|
||||
use2ndOrderIntegration(false),
|
||||
use2ndOrderCoriolis(false),
|
||||
gravity(0, 0, 9.8) {}
|
||||
/// The Params constructor insists on the user passing in gravity in the *body* frame.
|
||||
/// For convenience, two commonly used conventions are provided by named constructors below
|
||||
Params(const Vector3& b_gravity) :
|
||||
accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis(
|
||||
false), b_gravity(b_gravity) {
|
||||
}
|
||||
|
||||
// Default Params for Front-Right-Down convention: gravity points along positive Z-axis
|
||||
static boost::shared_ptr<Params> MakeSharedFRD(double g = 9.81) {
|
||||
return boost::make_shared<Params>(Vector3(0, 0, g));
|
||||
}
|
||||
|
||||
// Default Params for Front-Left-Up convention: gravity points along negative Z-axis
|
||||
static boost::shared_ptr<Params> MakeSharedFLU(double g = 9.81) {
|
||||
return boost::make_shared<Params>(Vector3(0, 0, -g));
|
||||
}
|
||||
|
||||
protected:
|
||||
/// Default constructor for serialization only: uninitialized!
|
||||
Params();
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
|
|
@ -75,29 +93,29 @@ class PreintegrationBase : public PreintegratedRotation {
|
|||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
|
||||
ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
|
||||
ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
|
||||
ar & BOOST_SERIALIZATION_NVP(use2ndOrderIntegration);
|
||||
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
|
||||
ar & BOOST_SERIALIZATION_NVP(gravity);
|
||||
ar & BOOST_SERIALIZATION_NVP(b_gravity);
|
||||
}
|
||||
};
|
||||
|
||||
protected:
|
||||
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)
|
||||
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 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
|
||||
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
|
||||
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity 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 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
|
||||
|
||||
/// Default constructor for serialization
|
||||
PreintegrationBase() {}
|
||||
PreintegrationBase() {
|
||||
}
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
/**
|
||||
* Constructor, initializes the variables in the base class
|
||||
|
|
@ -105,27 +123,45 @@ class PreintegrationBase : public PreintegratedRotation {
|
|||
* @param p Parameters, typically fixed in a single application
|
||||
*/
|
||||
PreintegrationBase(const boost::shared_ptr<const Params>& p,
|
||||
const imuBias::ConstantBias& biasHat)
|
||||
: PreintegratedRotation(p), biasHat_(biasHat) {
|
||||
const imuBias::ConstantBias& biasHat) :
|
||||
PreintegratedRotation(p), biasHat_(biasHat) {
|
||||
resetIntegration();
|
||||
}
|
||||
|
||||
/// Re-initialize PreintegratedMeasurements
|
||||
void resetIntegration();
|
||||
|
||||
const Params& p() const { return *boost::static_pointer_cast<const Params>(p_);}
|
||||
const Params& p() const {
|
||||
return *boost::static_pointer_cast<const Params>(p_);
|
||||
}
|
||||
|
||||
/// getters
|
||||
const imuBias::ConstantBias& biasHat() const { return biasHat_; }
|
||||
const Vector3& deltaPij() const { return deltaPij_; }
|
||||
const Vector3& deltaVij() const { return deltaVij_; }
|
||||
const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_; }
|
||||
const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_; }
|
||||
const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_; }
|
||||
const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_; }
|
||||
const imuBias::ConstantBias& biasHat() const {
|
||||
return biasHat_;
|
||||
}
|
||||
const Vector3& deltaPij() const {
|
||||
return deltaPij_;
|
||||
}
|
||||
const Vector3& deltaVij() const {
|
||||
return deltaVij_;
|
||||
}
|
||||
const Matrix3& delPdelBiasAcc() const {
|
||||
return delPdelBiasAcc_;
|
||||
}
|
||||
const Matrix3& delPdelBiasOmega() const {
|
||||
return delPdelBiasOmega_;
|
||||
}
|
||||
const Matrix3& delVdelBiasAcc() const {
|
||||
return delVdelBiasAcc_;
|
||||
}
|
||||
const Matrix3& delVdelBiasOmega() const {
|
||||
return delVdelBiasOmega_;
|
||||
}
|
||||
|
||||
// Exposed for MATLAB
|
||||
Vector6 biasHatVector() const { return biasHat_.vector(); }
|
||||
Vector6 biasHatVector() const {
|
||||
return biasHat_.vector();
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const std::string& s) const;
|
||||
|
|
@ -134,18 +170,16 @@ class PreintegrationBase : public PreintegratedRotation {
|
|||
bool equals(const PreintegrationBase& other, double tol) const;
|
||||
|
||||
/// Update preintegrated measurements
|
||||
void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR,
|
||||
const double deltaT, OptionalJacobian<9, 9> F);
|
||||
void updatePreintegratedMeasurements(const Vector3& correctedAcc,
|
||||
const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F);
|
||||
|
||||
/// Update Jacobians to be used during preintegration
|
||||
void updatePreintegratedJacobians(const Vector3& correctedAcc,
|
||||
const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
|
||||
double deltaT);
|
||||
const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT);
|
||||
|
||||
void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega,
|
||||
Vector3* correctedAcc,
|
||||
Vector3* correctedOmega);
|
||||
const Vector3& measuredOmega, Vector3* correctedAcc,
|
||||
Vector3* correctedOmega);
|
||||
|
||||
/// Given the estimate of the bias, return a NavState tangent vector
|
||||
/// summarizing the preintegrated IMU measurements so far
|
||||
|
|
@ -154,23 +188,23 @@ class PreintegrationBase : public PreintegratedRotation {
|
|||
|
||||
/// Predict state at time j
|
||||
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
|
||||
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = boost::none) const;
|
||||
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 =
|
||||
boost::none) const;
|
||||
|
||||
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
|
||||
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j,
|
||||
const Vector3& vel_j, const imuBias::ConstantBias& bias_i,
|
||||
OptionalJacobian<9, 6> H1 = boost::none,
|
||||
OptionalJacobian<9, 3> H2 = boost::none,
|
||||
OptionalJacobian<9, 6> H3 = boost::none,
|
||||
OptionalJacobian<9, 3> H4 = boost::none,
|
||||
OptionalJacobian<9, 6> H5 = boost::none) const;
|
||||
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 =
|
||||
boost::none, OptionalJacobian<9, 3> H2 = boost::none,
|
||||
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
|
||||
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
|
||||
|
||||
/// @deprecated predict
|
||||
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias_i, const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
const bool use2ndOrderCoriolis = false);
|
||||
const imuBias::ConstantBias& bias_i, const Vector3& b_gravity,
|
||||
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
|
||||
|
||||
private:
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
|
|
@ -187,4 +221,4 @@ class PreintegrationBase : public PreintegratedRotation {
|
|||
}
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -57,15 +57,11 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
|
|||
Vector updatePreintegratedPosVel(const Vector3 deltaPij_old,
|
||||
const Vector3& deltaVij_old, const Rot3& deltaRij_old,
|
||||
const Vector3& correctedAcc, const Vector3& correctedOmega,
|
||||
const double deltaT, const bool use2ndOrderIntegration_) {
|
||||
const double deltaT) {
|
||||
Matrix3 dRij = deltaRij_old.matrix();
|
||||
Vector3 temp = dRij * correctedAcc * deltaT;
|
||||
Vector3 deltaPij_new;
|
||||
if (!use2ndOrderIntegration_) {
|
||||
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
|
||||
} else {
|
||||
deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
|
||||
}
|
||||
deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
|
||||
Vector3 deltaVij_new = deltaVij_old + temp;
|
||||
|
||||
Vector result(6);
|
||||
|
|
@ -93,11 +89,9 @@ const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
|
|||
/* ************************************************************************* */
|
||||
PreintegratedImuMeasurements evaluatePreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas, const list<double>& deltaTs,
|
||||
const bool use2ndOrderIntegration = false) {
|
||||
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
|
||||
PreintegratedImuMeasurements result(bias, kMeasuredAccCovariance,
|
||||
kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
|
||||
use2ndOrderIntegration);
|
||||
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
|
||||
|
||||
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
|
||||
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
|
||||
|
|
@ -142,27 +136,22 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
|
|||
} // namespace
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace straight {
|
||||
// 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);
|
||||
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);
|
||||
|
||||
// Set up body pointing towards y axis, and start at 10,20,0 with zero velocity
|
||||
// TODO(frank): clean up Rot3 mess
|
||||
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));
|
||||
}
|
||||
|
||||
TEST(ImuFactor, StraightLineSecondOrder) {
|
||||
using namespace straight;
|
||||
// Set up body pointing towards y axis, and start at 10,20,0 with zero velocity
|
||||
// TODO(frank): clean up Rot3 mess
|
||||
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));
|
||||
|
||||
imuBias::ConstantBias biasHat, bias;
|
||||
boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
|
||||
PreintegratedImuMeasurements::Params::MakeSharedFRD();
|
||||
p->use2ndOrderIntegration = true;
|
||||
p->b_gravity = Vector3(0, 0, g); // FRD convention
|
||||
|
||||
PreintegratedImuMeasurements pim(p, biasHat);
|
||||
|
|
@ -186,39 +175,6 @@ TEST(ImuFactor, StraightLineSecondOrder) {
|
|||
EXPECT(assert_equal(expected, pim.predict(state1, bias)));
|
||||
}
|
||||
|
||||
TEST(ImuFactor, StraightLineFirstOrder) {
|
||||
using namespace straight;
|
||||
|
||||
imuBias::ConstantBias biasHat, bias;
|
||||
boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
|
||||
PreintegratedImuMeasurements::Params::MakeSharedFRD();
|
||||
p->use2ndOrderIntegration = false;
|
||||
p->b_gravity = Vector3(0, 0, g); // FRD convention
|
||||
|
||||
// We do a rough approximation:
|
||||
PreintegratedImuMeasurements pim(p, biasHat);
|
||||
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!
|
||||
const double approx = exact * 0.9; // approximation for dt split into 10 intervals
|
||||
EXPECT(assert_equal(Rot3(), pim.deltaRij()));
|
||||
EXPECT(assert_equal(Vector3(a * approx, 0, -g * approx), pim.deltaPij()));
|
||||
EXPECT(assert_equal(Vector3(a * dt, 0, -g * dt), pim.deltaVij())); // still correct
|
||||
|
||||
// Check bias-corrected delta: also in body frame
|
||||
Vector9 expectedBC;
|
||||
expectedBC << 0, 0, 0, a * approx, 0, -g * approx, a * dt, 0, -g * dt;
|
||||
EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(bias)));
|
||||
|
||||
// Check prediction, note we move along x in body, along y in nav
|
||||
// In this instance the position is just an approximation,
|
||||
// but gravity should be subtracted out exactly
|
||||
NavState expected(nRb, Point3(10, 20 + a * approx, 0), Velocity3(0, a * dt, 0));
|
||||
GTSAM_PRINT(pim);
|
||||
EXPECT(assert_equal(expected, pim.predict(state1, bias)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, PreintegratedMeasurements) {
|
||||
// Linearization point
|
||||
|
|
@ -236,11 +192,9 @@ TEST(ImuFactor, PreintegratedMeasurements) {
|
|||
Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0);
|
||||
double expectedDeltaT1(0.5);
|
||||
|
||||
bool use2ndOrderIntegration = true;
|
||||
// Actual preintegrated values
|
||||
PreintegratedImuMeasurements actual1(bias, kMeasuredAccCovariance,
|
||||
kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
|
||||
use2ndOrderIntegration);
|
||||
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
|
||||
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
EXPECT(
|
||||
|
|
@ -294,12 +248,11 @@ double deltaT = 1.0;
|
|||
TEST(ImuFactor, PreintegrationBaseMethods) {
|
||||
using namespace common;
|
||||
boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
|
||||
PreintegratedImuMeasurements::Params::MakeSharedFRD();
|
||||
PreintegratedImuMeasurements::Params::MakeSharedFRD();
|
||||
p->gyroscopeCovariance = kMeasuredOmegaCovariance;
|
||||
p->omegaCoriolis = Vector3(0.02, 0.03, 0.04);
|
||||
p->accelerometerCovariance = kMeasuredAccCovariance;
|
||||
p->integrationCovariance = kIntegrationErrorCovariance;
|
||||
p->use2ndOrderIntegration = false;
|
||||
p->use2ndOrderCoriolis = true;
|
||||
|
||||
PreintegratedImuMeasurements pim(p, bias);
|
||||
|
|
@ -332,10 +285,8 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
|
|||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, ErrorAndJacobians) {
|
||||
using namespace common;
|
||||
bool use2ndOrderIntegration = true;
|
||||
PreintegratedImuMeasurements pim(bias, kMeasuredAccCovariance,
|
||||
kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
|
||||
use2ndOrderIntegration);
|
||||
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
|
||||
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
EXPECT(assert_equal(state2, pim.predict(state1, bias), 1e-6));
|
||||
|
|
@ -631,11 +582,10 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
|
|||
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
|
||||
deltaTs.push_back(0.01);
|
||||
}
|
||||
bool use2ndOrderIntegration = false;
|
||||
// Actual preintegrated values
|
||||
PreintegratedImuMeasurements preintegrated =
|
||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
||||
deltaTs, use2ndOrderIntegration);
|
||||
deltaTs);
|
||||
|
||||
// so far we only created a nontrivial linearization point for the preintegrated measurements
|
||||
// Now we add a new measurement and ask for Jacobians
|
||||
|
|
@ -658,20 +608,17 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
|
|||
// Compute expected f_pos_vel wrt positions
|
||||
Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>(
|
||||
boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old,
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
|
||||
deltaPij_old);
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaPij_old);
|
||||
|
||||
// Compute expected f_pos_vel wrt velocities
|
||||
Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>(
|
||||
boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old,
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
|
||||
deltaVij_old);
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaVij_old);
|
||||
|
||||
// Compute expected f_pos_vel wrt angles
|
||||
Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>(
|
||||
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
|
||||
deltaRij_old);
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old);
|
||||
|
||||
Matrix FexpectedTop6(6, 9);
|
||||
FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
|
||||
|
|
@ -698,14 +645,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
|
|||
// Compute jacobian wrt acc noise
|
||||
Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>(
|
||||
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
|
||||
deltaRij_old, _1, newMeasuredOmega, newDeltaT,
|
||||
use2ndOrderIntegration), newMeasuredAcc);
|
||||
deltaRij_old, _1, newMeasuredOmega, newDeltaT), newMeasuredAcc);
|
||||
|
||||
// Compute expected F wrt gyro noise
|
||||
Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
|
||||
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
|
||||
deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration),
|
||||
newMeasuredOmega);
|
||||
deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega);
|
||||
Matrix GexpectedTop6(6, 9);
|
||||
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
|
||||
|
||||
|
|
@ -754,11 +699,10 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
|
|||
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
|
||||
deltaTs.push_back(0.01);
|
||||
}
|
||||
bool use2ndOrderIntegration = true;
|
||||
// Actual preintegrated values
|
||||
PreintegratedImuMeasurements preintegrated =
|
||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
||||
deltaTs, use2ndOrderIntegration);
|
||||
deltaTs);
|
||||
|
||||
// so far we only created a nontrivial linearization point for the preintegrated measurements
|
||||
// Now we add a new measurement and ask for Jacobians
|
||||
|
|
@ -781,20 +725,17 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
|
|||
// Compute expected f_pos_vel wrt positions
|
||||
Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>(
|
||||
boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old,
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
|
||||
deltaPij_old);
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaPij_old);
|
||||
|
||||
// Compute expected f_pos_vel wrt velocities
|
||||
Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>(
|
||||
boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old,
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
|
||||
deltaVij_old);
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaVij_old);
|
||||
|
||||
// Compute expected f_pos_vel wrt angles
|
||||
Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>(
|
||||
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
|
||||
deltaRij_old);
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old);
|
||||
|
||||
Matrix FexpectedTop6(6, 9);
|
||||
FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
|
||||
|
|
@ -821,14 +762,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
|
|||
// Compute jacobian wrt acc noise
|
||||
Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>(
|
||||
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
|
||||
deltaRij_old, _1, newMeasuredOmega, newDeltaT,
|
||||
use2ndOrderIntegration), newMeasuredAcc);
|
||||
deltaRij_old, _1, newMeasuredOmega, newDeltaT), newMeasuredAcc);
|
||||
|
||||
// Compute expected F wrt gyro noise
|
||||
Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
|
||||
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
|
||||
deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration),
|
||||
newMeasuredOmega);
|
||||
deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega);
|
||||
Matrix GexpectedTop6(6, 9);
|
||||
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
|
||||
|
||||
|
|
|
|||
|
|
@ -178,12 +178,12 @@ TEST(NavState, PredictXi) {
|
|||
xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
|
||||
double dt = 0.5;
|
||||
Matrix9 actualH1, actualH2;
|
||||
boost::function<Vector9(const NavState&, const Vector9&)> predictXi =
|
||||
boost::bind(&NavState::predictXi, _1, _2, dt, kGravity, kOmegaCoriolis,
|
||||
boost::function<Vector9(const NavState&, const Vector9&)> integrateTangent =
|
||||
boost::bind(&NavState::integrateTangent, _1, _2, dt, kGravity, kOmegaCoriolis,
|
||||
false, boost::none, boost::none);
|
||||
kState1.predictXi(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2);
|
||||
EXPECT(assert_equal(numericalDerivative21(predictXi, kState1, xi), actualH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(predictXi, kState1, xi), actualH2));
|
||||
kState1.integrateTangent(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2);
|
||||
EXPECT(assert_equal(numericalDerivative21(integrateTangent, kState1, xi), actualH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(integrateTangent, kState1, xi), actualH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue