Fixed equals
dellaert
parent
73309d6fcf
commit
c20bacf025
|
|
@ -73,8 +73,8 @@ void PreintegratedImuMeasurements::integrateMeasurement(
|
|||
G << G1, Gi, G2;
|
||||
Matrix9 Cov;
|
||||
Cov << p().accelerometerCovariance / dt, Z_3x3, Z_3x3,
|
||||
Z_3x3, p().integrationCovariance * dt, Z_3x3,
|
||||
Z_3x3, Z_3x3, p().gyroscopeCovariance / dt;
|
||||
Z_3x3, p().integrationCovariance * dt, Z_3x3,
|
||||
Z_3x3, Z_3x3, p().gyroscopeCovariance / dt;
|
||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G * Cov * G.transpose();
|
||||
#else
|
||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose()
|
||||
|
|
@ -91,7 +91,7 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
|
|||
const Matrix3& measuredOmegaCovariance,
|
||||
const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) {
|
||||
if (!use2ndOrderIntegration)
|
||||
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
|
||||
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
|
||||
biasHat_ = biasHat;
|
||||
boost::shared_ptr<Params> p = Params::MakeSharedD();
|
||||
p->gyroscopeCovariance = measuredOmegaCovariance;
|
||||
|
|
@ -141,8 +141,9 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
|
|||
//------------------------------------------------------------------------------
|
||||
bool ImuFactor::equals(const NonlinearFactor& other, double tol) const {
|
||||
const This *e = dynamic_cast<const This*>(&other);
|
||||
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
|
||||
&& Base::equals(*e, tol);
|
||||
const bool base = Base::equals(*e, tol);
|
||||
const bool pim = _PIM_.equals(e->_PIM_, tol);
|
||||
return e != nullptr && base && pim;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -161,10 +162,10 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
|||
const PreintegratedImuMeasurements& pim, const Vector3& n_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) {
|
||||
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
|
||||
pose_j, vel_j, bias), _PIM_(pim) {
|
||||
boost::shared_ptr<PreintegratedImuMeasurements::Params> p = boost::make_shared<
|
||||
PreintegratedImuMeasurements::Params>(pim.p());
|
||||
PreintegratedImuMeasurements::Params>(pim.p());
|
||||
p->n_gravity = n_gravity;
|
||||
p->omegaCoriolis = omegaCoriolis;
|
||||
p->body_P_sensor = body_P_sensor;
|
||||
|
|
@ -185,4 +186,5 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
|||
#endif
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
} // namespace gtsam
|
||||
}
|
||||
// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -71,7 +71,9 @@ protected:
|
|||
///< (first-order propagation from *measurementCovariance*).
|
||||
|
||||
/// Default constructor for serialization
|
||||
PreintegratedImuMeasurements() {}
|
||||
PreintegratedImuMeasurements() {
|
||||
preintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
|
|
|
|||
|
|
@ -29,12 +29,26 @@ void PreintegratedRotation::Params::print(const string& s) const {
|
|||
cout << s << endl;
|
||||
cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
|
||||
if (omegaCoriolis)
|
||||
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")"
|
||||
<< endl;
|
||||
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl;
|
||||
if (body_P_sensor)
|
||||
body_P_sensor->print("body_P_sensor");
|
||||
}
|
||||
|
||||
bool PreintegratedRotation::Params::equals(
|
||||
const PreintegratedRotation::Params& other, double tol) const {
|
||||
if (body_P_sensor) {
|
||||
if (!other.body_P_sensor
|
||||
|| !assert_equal(*body_P_sensor, *other.body_P_sensor, tol))
|
||||
return false;
|
||||
}
|
||||
if (omegaCoriolis) {
|
||||
if (!other.omegaCoriolis
|
||||
|| !equal_with_abs_tol(*omegaCoriolis, *other.omegaCoriolis, tol))
|
||||
return false;
|
||||
}
|
||||
return equal_with_abs_tol(gyroscopeCovariance, other.gyroscopeCovariance, tol);
|
||||
}
|
||||
|
||||
void PreintegratedRotation::resetIntegration() {
|
||||
deltaTij_ = 0.0;
|
||||
deltaRij_ = Rot3();
|
||||
|
|
@ -49,8 +63,7 @@ void PreintegratedRotation::print(const string& s) const {
|
|||
|
||||
bool PreintegratedRotation::equals(const PreintegratedRotation& other,
|
||||
double tol) const {
|
||||
return this->matchesParamsWith(other)
|
||||
&& deltaRij_.equals(other.deltaRij_, tol)
|
||||
return p_->equals(*other.p_,tol) && deltaRij_.equals(other.deltaRij_, tol)
|
||||
&& fabs(deltaTij_ - other.deltaTij_) < tol
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
|
||||
}
|
||||
|
|
@ -76,11 +89,13 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
|
|||
return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
|
||||
}
|
||||
|
||||
void PreintegratedRotation::integrateMeasurement(
|
||||
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, OptionalJacobian<3, 3> F) {
|
||||
void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
|
||||
const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
|
||||
OptionalJacobian<3, 3> F) {
|
||||
Matrix3 D_incrR_integratedOmega;
|
||||
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, D_incrR_integratedOmega);
|
||||
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
|
||||
D_incrR_integratedOmega);
|
||||
|
||||
// If asked, pass first derivative as well
|
||||
if (optional_D_incrR_integratedOmega) {
|
||||
|
|
@ -93,7 +108,8 @@ void PreintegratedRotation::integrateMeasurement(
|
|||
|
||||
// Update Jacobian
|
||||
const Matrix3 incrRt = incrR.transpose();
|
||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT;
|
||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
|
||||
- D_incrR_integratedOmega * deltaT;
|
||||
}
|
||||
|
||||
Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
|
||||
|
|
|
|||
|
|
@ -43,6 +43,7 @@ class PreintegratedRotation {
|
|||
Params() : gyroscopeCovariance(I_3x3) {}
|
||||
|
||||
virtual void print(const std::string& s) const;
|
||||
virtual bool equals(const Params& other, double tol=1e-9) const;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
@ -53,7 +54,6 @@ class PreintegratedRotation {
|
|||
ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
|
||||
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
|
||||
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
|
||||
ar& BOOST_SERIALIZATION_NVP(body_P_sensor);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@ void PreintegrationBase::Params::print(const string& s) const {
|
|||
PreintegratedRotation::Params::print(s);
|
||||
cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
|
||||
<< endl;
|
||||
cout << "integrationCovariance:\n[\n" << accelerometerCovariance << "\n]"
|
||||
cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]"
|
||||
<< endl;
|
||||
if (omegaCoriolis && use2ndOrderCoriolis)
|
||||
cout << "Using 2nd-order Coriolis" << endl;
|
||||
|
|
@ -40,6 +40,18 @@ void PreintegrationBase::Params::print(const string& s) const {
|
|||
cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool PreintegrationBase::Params::equals(
|
||||
const PreintegratedRotation::Params& other, double tol) const {
|
||||
auto e = dynamic_cast<const PreintegrationBase::Params*>(&other);
|
||||
return e != nullptr && PreintegratedRotation::Params::equals(other, tol)
|
||||
&& use2ndOrderCoriolis == e->use2ndOrderCoriolis
|
||||
&& equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance,
|
||||
tol)
|
||||
&& equal_with_abs_tol(integrationCovariance, e->integrationCovariance,
|
||||
tol) && equal_with_abs_tol(n_gravity, e->n_gravity, tol);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void PreintegrationBase::resetIntegration() {
|
||||
deltaTij_ = 0.0;
|
||||
|
|
@ -64,8 +76,8 @@ void PreintegrationBase::print(const string& s) const {
|
|||
//------------------------------------------------------------------------------
|
||||
bool PreintegrationBase::equals(const PreintegrationBase& other,
|
||||
double tol) const {
|
||||
return this->matchesParamsWith(other)
|
||||
&& fabs(deltaTij_ - other.deltaTij_) < tol
|
||||
const bool params_match = p_->equals(*other.p_, tol);
|
||||
return params_match && fabs(deltaTij_ - other.deltaTij_) < tol
|
||||
&& deltaXij_.equals(other.deltaXij_, tol)
|
||||
&& biasHat_.equals(other.biasHat_, tol)
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
|
||||
|
|
@ -82,13 +94,13 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
|
|||
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega,
|
||||
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const {
|
||||
|
||||
// Correct for bias in the sensor frame
|
||||
// Correct for bias in the sensor frame
|
||||
Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
|
||||
Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega);
|
||||
|
||||
// Compensate for sensor-body displacement if needed: we express the quantities
|
||||
// (originally in the IMU frame) into the body frame
|
||||
// Equations below assume the "body" frame is the CG
|
||||
// Compensate for sensor-body displacement if needed: we express the quantities
|
||||
// (originally in the IMU frame) into the body frame
|
||||
// Equations below assume the "body" frame is the CG
|
||||
if (p().body_P_sensor) {
|
||||
// Correct omega to rotation rate vector in the body frame
|
||||
const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
|
||||
|
|
@ -98,9 +110,12 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
|
|||
j_correctedAcc = bRs * j_correctedAcc;
|
||||
|
||||
// Jacobians
|
||||
if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs;
|
||||
if (D_correctedAcc_measuredOmega) *D_correctedAcc_measuredOmega = Matrix3::Zero();
|
||||
if (D_correctedOmega_measuredOmega) *D_correctedOmega_measuredOmega = bRs;
|
||||
if (D_correctedAcc_measuredAcc)
|
||||
*D_correctedAcc_measuredAcc = bRs;
|
||||
if (D_correctedAcc_measuredOmega)
|
||||
*D_correctedAcc_measuredOmega = Matrix3::Zero();
|
||||
if (D_correctedOmega_measuredOmega)
|
||||
*D_correctedOmega_measuredOmega = bRs;
|
||||
|
||||
// Centrifugal acceleration
|
||||
const Vector3 b_arm = p().body_P_sensor->translation().vector();
|
||||
|
|
@ -120,7 +135,7 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
|
|||
}
|
||||
}
|
||||
|
||||
// Do update in one fell swoop
|
||||
// Do update in one fell swoop
|
||||
return make_pair(j_correctedAcc, j_correctedOmega);
|
||||
}
|
||||
|
||||
|
|
@ -135,22 +150,27 @@ NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
|
|||
Matrix3 D_correctedAcc_measuredAcc, //
|
||||
D_correctedAcc_measuredOmega, //
|
||||
D_correctedOmega_measuredOmega;
|
||||
bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor;
|
||||
bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega
|
||||
&& p().body_P_sensor;
|
||||
boost::tie(j_correctedAcc, j_correctedOmega) =
|
||||
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega,
|
||||
(needDerivs ? &D_correctedAcc_measuredAcc : 0),
|
||||
(needDerivs ? &D_correctedAcc_measuredOmega : 0),
|
||||
(needDerivs ? &D_correctedOmega_measuredOmega : 0));
|
||||
// Do update in one fell swoop
|
||||
// Do update in one fell swoop
|
||||
Matrix93 D_updated_correctedAcc, D_updated_correctedOmega;
|
||||
NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, D_updated_current,
|
||||
(needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc),
|
||||
(needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega));
|
||||
NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt,
|
||||
D_updated_current,
|
||||
(needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc),
|
||||
(needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega));
|
||||
if (needDerivs) {
|
||||
*D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc;
|
||||
*D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega;
|
||||
*D_updated_measuredAcc = D_updated_correctedAcc
|
||||
* D_correctedAcc_measuredAcc;
|
||||
*D_updated_measuredOmega = D_updated_correctedOmega
|
||||
* D_correctedOmega_measuredOmega;
|
||||
if (!p().body_P_sensor->translation().vector().isZero()) {
|
||||
*D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega;
|
||||
*D_updated_measuredOmega += D_updated_correctedAcc
|
||||
* D_correctedAcc_measuredOmega;
|
||||
}
|
||||
}
|
||||
return updated;
|
||||
|
|
@ -162,16 +182,16 @@ void PreintegrationBase::update(const Vector3& j_measuredAcc,
|
|||
Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
|
||||
Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) {
|
||||
|
||||
// Save current rotation for updating Jacobians
|
||||
// Save current rotation for updating Jacobians
|
||||
const Rot3 oldRij = deltaXij_.attitude();
|
||||
|
||||
// Do update
|
||||
// Do update
|
||||
deltaTij_ += dt;
|
||||
deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt,
|
||||
D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional
|
||||
|
||||
// Update Jacobians
|
||||
// TODO(frank): we are repeating some computation here: accessible in F ?
|
||||
// Update Jacobians
|
||||
// TODO(frank): we are repeating some computation here: accessible in F ?
|
||||
Vector3 j_correctedAcc, j_correctedOmega;
|
||||
boost::tie(j_correctedAcc, j_correctedOmega) =
|
||||
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
|
||||
|
|
@ -197,7 +217,7 @@ void PreintegrationBase::update(const Vector3& j_measuredAcc,
|
|||
//------------------------------------------------------------------------------
|
||||
Vector9 PreintegrationBase::biasCorrectedDelta(
|
||||
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
|
||||
// Correct deltaRij, derivative is delRdelBiasOmega_
|
||||
// Correct deltaRij, derivative is delRdelBiasOmega_
|
||||
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
|
||||
Matrix3 D_correctedRij_bias;
|
||||
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
|
||||
|
|
@ -208,8 +228,8 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
|
|||
|
||||
Vector9 xi;
|
||||
Matrix3 D_dR_correctedRij;
|
||||
// TODO(frank): could line below be simplified? It is equivalent to
|
||||
// LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
|
||||
// TODO(frank): could line below be simplified? It is equivalent to
|
||||
// LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
|
||||
NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
|
||||
NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
|
||||
+ delPdelBiasOmega_ * biasIncr.gyroscope();
|
||||
|
|
@ -230,18 +250,18 @@ 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
|
||||
// correct for bias
|
||||
Matrix96 D_biasCorrected_bias;
|
||||
Vector9 biasCorrected = biasCorrectedDelta(bias_i,
|
||||
H2 ? &D_biasCorrected_bias : 0);
|
||||
|
||||
// integrate on tangent space
|
||||
// integrate on tangent space
|
||||
Matrix9 D_delta_state, D_delta_biasCorrected;
|
||||
Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
|
||||
p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
|
||||
H2 ? &D_delta_biasCorrected : 0);
|
||||
|
||||
// Use retract to get back to NavState manifold
|
||||
// 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)
|
||||
|
|
@ -258,12 +278,12 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
|
|||
OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3,
|
||||
OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const {
|
||||
|
||||
// Note that derivative of constructors below is not identity for velocity, but
|
||||
// a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose()
|
||||
// Note that derivative of constructors below is not identity for velocity, but
|
||||
// a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose()
|
||||
NavState state_i(pose_i, vel_i);
|
||||
NavState state_j(pose_j, vel_j);
|
||||
|
||||
/// Predict state at time j
|
||||
/// Predict state at time j
|
||||
Matrix99 D_predict_state_i;
|
||||
Matrix96 D_predict_bias_i;
|
||||
NavState predictedState_j = predict(state_i, bias_i,
|
||||
|
|
@ -273,11 +293,11 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
|
|||
Vector9 error = state_j.localCoordinates(predictedState_j,
|
||||
H3 || H4 ? &D_error_state_j : 0, H1 || H2 || H5 ? &D_error_predict : 0);
|
||||
|
||||
// Separate out derivatives in terms of 5 arguments
|
||||
// Note that doing so requires special treatment of velocities, as when treated as
|
||||
// separate variables the retract applied will not be the semi-direct product in NavState
|
||||
// Instead, the velocities in nav are updated using a straight addition
|
||||
// This is difference is accounted for by the R().transpose calls below
|
||||
// Separate out derivatives in terms of 5 arguments
|
||||
// Note that doing so requires special treatment of velocities, as when treated as
|
||||
// separate variables the retract applied will not be the semi-direct product in NavState
|
||||
// Instead, the velocities in nav are updated using a straight addition
|
||||
// This is difference is accounted for by the R().transpose calls below
|
||||
if (H1)
|
||||
*H1 << D_error_predict * D_predict_state_i.leftCols<6>();
|
||||
if (H2)
|
||||
|
|
@ -300,7 +320,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
|
|||
const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
|
||||
const Vector3& n_gravity, const Vector3& omegaCoriolis,
|
||||
const bool use2ndOrderCoriolis) const {
|
||||
// NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
|
||||
// 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->n_gravity = n_gravity;
|
||||
q->omegaCoriolis = omegaCoriolis;
|
||||
|
|
@ -311,4 +331,5 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
|
|||
#endif
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
}/// namespace gtsam
|
||||
}
|
||||
/// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -83,6 +83,7 @@ public:
|
|||
}
|
||||
|
||||
void print(const std::string& s) const;
|
||||
bool equals(const PreintegratedRotation::Params& other, double tol) const;
|
||||
|
||||
protected:
|
||||
/// Default constructor for serialization only: uninitialized!
|
||||
|
|
@ -132,6 +133,7 @@ protected:
|
|||
|
||||
/// Default constructor for serialization
|
||||
PreintegrationBase() {
|
||||
resetIntegration();
|
||||
}
|
||||
|
||||
public:
|
||||
|
|
|
|||
|
|
@ -65,12 +65,12 @@ static const double kAccelerometerSigma = 0.1;
|
|||
static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
|
||||
auto p = PreintegratedImuMeasurements::Params::MakeSharedD(kGravity);
|
||||
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||
p->accelerometerCovariance = kAccelerometerSigma * kAccelerometerSigma * I_3x3;
|
||||
p->accelerometerCovariance = kAccelerometerSigma * kAccelerometerSigma
|
||||
* I_3x3;
|
||||
p->integrationCovariance = 0.0001 * I_3x3;
|
||||
return p;
|
||||
}
|
||||
|
||||
|
||||
// Auxiliary functions to test preintegrated Jacobians
|
||||
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -123,7 +123,7 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, Accelerating) {
|
||||
const double a = 0.2, v=50;
|
||||
const double a = 0.2, v = 50;
|
||||
|
||||
// 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
|
||||
|
|
@ -132,9 +132,9 @@ TEST(ImuFactor, Accelerating) {
|
|||
const Vector3 initial_velocity(v, 0, 0);
|
||||
|
||||
const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
|
||||
Vector3(a, 0, 0));
|
||||
Vector3(a, 0, 0));
|
||||
|
||||
const double T = 3.0; // seconds
|
||||
const double T = 3.0; // seconds
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
|
||||
PreintegratedImuMeasurements pim = runner.integrate(T);
|
||||
|
|
@ -144,17 +144,20 @@ TEST(ImuFactor, Accelerating) {
|
|||
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
|
||||
// Check G1 and G2 derivatives of pim.update
|
||||
Matrix93 aG1,aG2;
|
||||
boost::function<NavState(const Vector3&, const Vector3&)> f =
|
||||
boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T/10,
|
||||
boost::none, boost::none, boost::none);
|
||||
Matrix93 aG1, aG2;
|
||||
boost::function<NavState(const Vector3&, const Vector3&)> f = boost::bind(
|
||||
&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T / 10, boost::none,
|
||||
boost::none, boost::none);
|
||||
const Vector3 measuredAcc = runner.actualSpecificForce(T);
|
||||
const Vector3 measuredOmega = runner.actualAngularVelocity(T);
|
||||
pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, aG2);
|
||||
EXPECT(assert_equal(
|
||||
numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7));
|
||||
EXPECT(assert_equal(
|
||||
numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7));
|
||||
pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1,
|
||||
aG2);
|
||||
EXPECT(
|
||||
assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7),
|
||||
aG1, 1e-7));
|
||||
EXPECT(
|
||||
assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7),
|
||||
aG2, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -268,7 +271,8 @@ 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, kZeroBias)));
|
||||
assert_equal(expectedError,
|
||||
factor.evaluateError(x1, v1, x2, v2, kZeroBias)));
|
||||
|
||||
Values values;
|
||||
values.insert(X(1), x1);
|
||||
|
|
@ -284,16 +288,19 @@ TEST(ImuFactor, ErrorAndJacobians) {
|
|||
|
||||
// Actual Jacobians
|
||||
Matrix H1a, H2a, H3a, H4a, H5a;
|
||||
(void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, 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, kZeroBias), 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, kZeroBias), x2);
|
||||
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias),
|
||||
x2);
|
||||
EXPECT(assert_equal(H3Rot3, H3a.topRows(3)));
|
||||
|
||||
// Evaluate error with wrong values
|
||||
|
|
@ -386,7 +393,6 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
|
|||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
bool use2ndOrderCoriolis = true;
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
|
||||
|
||||
Values values;
|
||||
|
|
@ -533,7 +539,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Vector3 correctedAcc(const PreintegratedImuMeasurements& pim,
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega) {
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega) {
|
||||
return pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega).first;
|
||||
}
|
||||
|
||||
|
|
@ -544,15 +550,16 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
|||
|
||||
const Vector3 a = nRb * Vector3(0.2, 0.0, 0.0);
|
||||
const AcceleratingScenario scenario(nRb, p1, v1, a,
|
||||
Vector3(0, 0, M_PI / 10.0 + 0.3));
|
||||
Vector3(0, 0, M_PI / 10.0 + 0.3));
|
||||
|
||||
auto p = defaultParams();
|
||||
p->body_P_sensor =Pose3(Rot3::Expmap(Vector3(0, M_PI/2, 0)), Point3(0.1, 0.05, 0.01));
|
||||
p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(0, M_PI / 2, 0)),
|
||||
Point3(0.1, 0.05, 0.01));
|
||||
p->omegaCoriolis = kNonZeroOmegaCoriolis;
|
||||
|
||||
imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
|
||||
|
||||
const double T = 3.0; // seconds
|
||||
const double T = 3.0; // seconds
|
||||
ScenarioRunner runner(&scenario, p, T / 10);
|
||||
|
||||
// PreintegratedImuMeasurements pim = runner.integrate(T);
|
||||
|
|
@ -575,32 +582,34 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
|||
|
||||
// Check updatedDeltaXij derivatives
|
||||
Matrix3 D_correctedAcc_measuredOmega = Matrix3::Zero();
|
||||
pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, boost::none, D_correctedAcc_measuredOmega, boost::none);
|
||||
Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6);
|
||||
pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
|
||||
boost::none, D_correctedAcc_measuredOmega, boost::none);
|
||||
Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(
|
||||
boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6);
|
||||
EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5));
|
||||
|
||||
Matrix93 G1, G2;
|
||||
double dt = 0.1;
|
||||
NavState preint = pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2);
|
||||
NavState preint = pim.updatedDeltaXij(measuredAcc, measuredOmega, dt,
|
||||
boost::none, G1, G2);
|
||||
// Matrix9 preintCov = G1*((accNoiseVar2/dt).asDiagonal())*G1.transpose() + G2*((omegaNoiseVar2/dt).asDiagonal())*G2.transpose();
|
||||
|
||||
Matrix93 expectedG1 = numericalDerivative21<NavState, Vector3, Vector3>(
|
||||
boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2,
|
||||
dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega,
|
||||
1e-6);
|
||||
dt, boost::none, boost::none, boost::none), measuredAcc,
|
||||
measuredOmega, 1e-6);
|
||||
EXPECT(assert_equal(expectedG1, G1, 1e-5));
|
||||
|
||||
Matrix93 expectedG2 = numericalDerivative22<NavState, Vector3, Vector3>(
|
||||
boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2,
|
||||
dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega,
|
||||
1e-6);
|
||||
dt, boost::none, boost::none, boost::none), measuredAcc,
|
||||
measuredOmega, 1e-6);
|
||||
EXPECT(assert_equal(expectedG2, G2, 1e-5));
|
||||
|
||||
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
||||
// EXPECT(MonteCarlo(pim, NavState(x1, initial_velocity), bias, dt, body_P_sensor,
|
||||
// measuredAcc, measuredOmega, accNoiseVar2, omegaNoiseVar2, 100000));
|
||||
|
||||
|
||||
// integrate at least twice to get position information
|
||||
// otherwise factor cov noise from preint_cov is not positive definite
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
|
||||
|
|
@ -687,10 +696,9 @@ TEST(ImuFactor, PredictArbitrary) {
|
|||
const Vector3 v1(0, 0, 0);
|
||||
|
||||
const AcceleratingScenario scenario(x1.rotation(), x1.translation(), v1,
|
||||
Vector3(0.1, 0.2, 0),
|
||||
Vector3(M_PI / 10, M_PI / 10, M_PI / 10));
|
||||
Vector3(0.1, 0.2, 0), Vector3(M_PI / 10, M_PI / 10, M_PI / 10));
|
||||
|
||||
const double T = 3.0; // seconds
|
||||
const double T = 3.0; // seconds
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
//
|
||||
// PreintegratedImuMeasurements pim = runner.integrate(T);
|
||||
|
|
@ -707,7 +715,7 @@ TEST(ImuFactor, PredictArbitrary) {
|
|||
Vector3 measuredAcc = runner.actualSpecificForce(0);
|
||||
|
||||
auto p = defaultParams();
|
||||
p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise
|
||||
p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise
|
||||
PreintegratedImuMeasurements pim(p, biasHat);
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
|
||||
// EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega,
|
||||
|
|
@ -724,9 +732,9 @@ TEST(ImuFactor, PredictArbitrary) {
|
|||
NavState actual = pim.predict(NavState(x1, v1), bias);
|
||||
|
||||
// Regression test for Imu Refactor
|
||||
Rot3 expectedR( //
|
||||
+0.903715275, -0.250741668, 0.347026393, //
|
||||
+0.347026393, 0.903715275, -0.250741668, //
|
||||
Rot3 expectedR( //
|
||||
+0.903715275, -0.250741668, 0.347026393, //
|
||||
+0.347026393, 0.903715275, -0.250741668, //
|
||||
-0.250741668, 0.347026393, 0.903715275);
|
||||
Point3 expectedT(-0.516077031, 0.57842919, 0.0876478403);
|
||||
Vector3 expectedV(-1.62337767, 1.57954409, 0.343833571);
|
||||
|
|
@ -740,7 +748,7 @@ TEST(ImuFactor, bodyPSensorNoBias) {
|
|||
|
||||
// Rotate sensor (z-down) to body (same as navigation) i.e. z-up
|
||||
auto p = defaultParams();
|
||||
p->n_gravity = Vector3(0, 0, -kGravity); // z-up nav frame
|
||||
p->n_gravity = Vector3(0, 0, -kGravity); // z-up nav frame
|
||||
p->body_P_sensor = Pose3(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0));
|
||||
|
||||
// Measurements
|
||||
|
|
@ -836,8 +844,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
// Now add IMU factors and bias noise models
|
||||
Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0));
|
||||
for (int i = 1; i < numFactors; i++) {
|
||||
PreintegratedImuMeasurements pim =
|
||||
PreintegratedImuMeasurements(p, priorBias);
|
||||
PreintegratedImuMeasurements pim = PreintegratedImuMeasurements(p,
|
||||
priorBias);
|
||||
for (int j = 0; j < 200; ++j)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
|
|
@ -862,28 +870,31 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
/* ************************************************************************** */
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
|
||||
"gtsam_noiseModel_Constrained");
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal,
|
||||
"gtsam_noiseModel_Diagonal");
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian,
|
||||
"gtsam_noiseModel_Gaussian");
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
|
||||
"gtsam_noiseModel_Isotropic");
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
|
||||
|
||||
TEST(ImuFactor, serialization) {
|
||||
using namespace gtsam::serializationTestHelpers;
|
||||
|
||||
Vector3 n_gravity(0, 0, -9.81);
|
||||
Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3());
|
||||
Matrix3 accCov = 1e-7 * I_3x3;
|
||||
Matrix3 gyroCov = 1e-8 * I_3x3;
|
||||
Matrix3 integrationCov = 1e-9 * I_3x3;
|
||||
auto p = defaultParams();
|
||||
p->n_gravity = Vector3(0, 0, -9.81);
|
||||
p->body_P_sensor = Pose3(Rot3::ypr(0, 0, M_PI), Point3());
|
||||
p->accelerometerCovariance = 1e-7 * I_3x3;
|
||||
p->gyroscopeCovariance = 1e-8 * I_3x3;
|
||||
p->integrationCovariance = 1e-9 * I_3x3;
|
||||
double deltaT = 0.005;
|
||||
imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pim =
|
||||
ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov,
|
||||
integrationCov, true);
|
||||
PreintegratedImuMeasurements pim(p, priorBias);
|
||||
|
||||
// measurements are needed for non-inf noise model, otherwise will throw err when deserialize
|
||||
|
||||
|
|
@ -895,8 +906,7 @@ TEST(ImuFactor, serialization) {
|
|||
Vector3 measuredAcc(0, 0, -9.81);
|
||||
|
||||
for (int j = 0; j < 200; ++j)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT,
|
||||
body_P_sensor);
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue