Fixed equals
dellaert
parent
73309d6fcf
commit
c20bacf025
|
|
@ -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;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -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)
|
||||
|
|
@ -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();
|
||||
|
|
@ -135,7 +150,8 @@ 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),
|
||||
|
|
@ -143,14 +159,18 @@ NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
|
|||
(needDerivs ? &D_correctedOmega_measuredOmega : 0));
|
||||
// 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,
|
||||
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;
|
||||
|
|
@ -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_
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -145,16 +145,19 @@ TEST(ImuFactor, Accelerating) {
|
|||
|
||||
// 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);
|
||||
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;
|
||||
|
|
@ -547,7 +553,8 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
|
|||
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));
|
||||
|
|
@ -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,8 +696,7 @@ 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
|
||||
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||
|
|
@ -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