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

Test changing bias and non-zero coriolis

release/4.3a0
dellaert 2016-02-01 14:53:33 -08:00
parent f5380283a1
commit 1556c25464
2 changed files with 32 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/PreintegrationBase.cpp
View File

@ -368,11 +368,11 @@ void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
Matrix9* H2) { Matrix9* H2) {
if (!matchesParamsWith(pim12)) if (!matchesParamsWith(pim12))
throw std::domain_error( throw std::domain_error(
"Cannot merge preintegrated measurements with different params"); "Cannot merge pre-integrated measurements with different params");
if (params()->body_P_sensor) if (params()->body_P_sensor)
throw std::domain_error( throw std::domain_error(
"Cannot merge preintegrated measurements with sensor pose yet"); "Cannot merge pre-integrated measurements with sensor pose yet");
const double& t01 = deltaTij(); const double& t01 = deltaTij();
const double& t12 = pim12.deltaTij(); const double& t12 = pim12.deltaTij();
@ -382,9 +382,9 @@ void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
Vector9 zeta12 = pim12.preintegrated(); Vector9 zeta12 = pim12.preintegrated();
// TODO(frank): adjust zeta12 due to bias difference // TODO(frank): adjust zeta12 due to bias difference
// const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat(); const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat();
// zeta12 += pim12.delPdelBiasAcc() * bias_incr_for_12.accelerometer() + zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope()
// pim12.delPdelBiasOmega() * bias_incr_for_12.gyroscope(); + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer();
preintegrated_ << PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2); preintegrated_ << PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2);

53
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -790,8 +790,8 @@ struct ImuFactorMergeTest {
int TestScenario(TestResult& result_, const std::string& name_, int TestScenario(TestResult& result_, const std::string& name_,
const Scenario& scenario, const Scenario& scenario,
const imuBias::ConstantBias& bias01, const Bias& bias01,
const imuBias::ConstantBias& bias12, double tol) { const Bias& bias12, double tol) {
// Test merge by creating a 01, 12, and 02 PreintegratedRotation, // Test merge by creating a 01, 12, and 02 PreintegratedRotation,
// then checking the merge of 01-12 matches 02. // then checking the merge of 01-12 matches 02.
PreintegratedImuMeasurements pim01(p_, bias01); PreintegratedImuMeasurements pim01(p_, bias01);
@ -831,40 +831,41 @@ struct ImuFactorMergeTest {
} }
void TestScenarios(TestResult& result_, const std::string& name_, void TestScenarios(TestResult& result_, const std::string& name_,
const imuBias::ConstantBias& bias01, const Bias& bias01,
const imuBias::ConstantBias& bias12, double tol) { const Bias& bias12, double tol) {
for (auto scenario : {forward_, loop_}) for (auto scenario : {forward_, loop_})
TestScenario(result_, name_, scenario, bias01, bias12, tol); TestScenario(result_, name_, scenario, bias01, bias12, tol);
} }
}; };
/* ************************************************************************* */ /* ************************************************************************* */
// Test case with identical biases where there is no approximation so we expect // Test case with zero biases
// an exact answer.
TEST(ImuFactor, MergeZeroBias) { TEST(ImuFactor, MergeZeroBias) {
ImuFactorMergeTest mergeTest; ImuFactorMergeTest mergeTest;
// TODO(frank): not too happy with large tolerance (needed for loop case) mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4);
mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-3);
} }
//// Test case with different biases where we expect there to be some variation. // Test case with identical biases: we expect an exact answer.
//TEST(ImuFactor, MergeChangingBias) { TEST(ImuFactor, MergeConstantBias) {
// ImuFactorMergeTest mergeTest; ImuFactorMergeTest mergeTest;
// mergeTest.TestScenarios( Bias bias(Vector3(0.03, -0.02, 0.01), Vector3(-0.01, 0.02, -0.03));
// result_, name_, imuBias::ConstantBias(Vector3(0.03, -0.02, 0.01), mergeTest.TestScenarios(result_, name_, bias, bias, 1e-4);
// Vector3(-0.01, 0.02, -0.03)), }
// imuBias::ConstantBias(Vector3(0.01, 0.02, 0.03),
// Vector3(0.03, -0.02, 0.01)), // Test case with different biases where we expect there to be some variation.
// 0.4); TEST(ImuFactor, MergeChangingBias) {
//} ImuFactorMergeTest mergeTest;
// mergeTest.TestScenarios(result_, name_,
//// Test case with non-zero coriolis Bias(Vector3(0.03, -0.02, 0.01), Vector3(-0.01, 0.02, -0.03)),
//TEST(ImuFactor, MergeWithCoriolis) { Bias(Vector3(0.01, 0.02, 0.03), Vector3(0.03, -0.02, 0.01)), 1e-1);
// ImuFactorMergeTest mergeTest; }
// mergeTest.p_->omegaCoriolis.reset(Vector3(0.1, 0.2, -0.1));
// mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, // Test case with non-zero coriolis
// 1e-6); TEST(ImuFactor, MergeWithCoriolis) {
//} ImuFactorMergeTest mergeTest;
mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1);
mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4);
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {