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) {
if (!matchesParamsWith(pim12))
throw std::domain_error(
"Cannot merge preintegrated measurements with different params");
"Cannot merge pre-integrated measurements with different params");
if (params()->body_P_sensor)
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& t12 = pim12.deltaTij();
@ -382,9 +382,9 @@ void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
Vector9 zeta12 = pim12.preintegrated();
// TODO(frank): adjust zeta12 due to bias difference
// const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat();
// zeta12 += pim12.delPdelBiasAcc() * bias_incr_for_12.accelerometer() +
// pim12.delPdelBiasOmega() * bias_incr_for_12.gyroscope();
const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat();
zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope()
+ pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer();
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_,
const Scenario& scenario,
const imuBias::ConstantBias& bias01,
const imuBias::ConstantBias& bias12, double tol) {
const Bias& bias01,
const Bias& bias12, double tol) {
// Test merge by creating a 01, 12, and 02 PreintegratedRotation,
// then checking the merge of 01-12 matches 02.
PreintegratedImuMeasurements pim01(p_, bias01);
@ -831,40 +831,41 @@ struct ImuFactorMergeTest {
}
void TestScenarios(TestResult& result_, const std::string& name_,
const imuBias::ConstantBias& bias01,
const imuBias::ConstantBias& bias12, double tol) {
const Bias& bias01,
const Bias& bias12, double tol) {
for (auto scenario : {forward_, loop_})
TestScenario(result_, name_, scenario, bias01, bias12, tol);
}
};
/* ************************************************************************* */
// Test case with identical biases where there is no approximation so we expect
// an exact answer.
// Test case with zero biases
TEST(ImuFactor, MergeZeroBias) {
ImuFactorMergeTest mergeTest;
// TODO(frank): not too happy with large tolerance (needed for loop case)
mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-3);
mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4);
}
//// Test case with different biases where we expect there to be some variation.
//TEST(ImuFactor, MergeChangingBias) {
// ImuFactorMergeTest mergeTest;
// mergeTest.TestScenarios(
// result_, name_, imuBias::ConstantBias(Vector3(0.03, -0.02, 0.01),
// Vector3(-0.01, 0.02, -0.03)),
// imuBias::ConstantBias(Vector3(0.01, 0.02, 0.03),
// Vector3(0.03, -0.02, 0.01)),
// 0.4);
//}
//
//// Test case with non-zero coriolis
//TEST(ImuFactor, MergeWithCoriolis) {
// ImuFactorMergeTest mergeTest;
// mergeTest.p_->omegaCoriolis.reset(Vector3(0.1, 0.2, -0.1));
// mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias,
// 1e-6);
//}
// Test case with identical biases: we expect an exact answer.
TEST(ImuFactor, MergeConstantBias) {
ImuFactorMergeTest mergeTest;
Bias bias(Vector3(0.03, -0.02, 0.01), Vector3(-0.01, 0.02, -0.03));
mergeTest.TestScenarios(result_, name_, bias, bias, 1e-4);
}
// Test case with different biases where we expect there to be some variation.
TEST(ImuFactor, MergeChangingBias) {
ImuFactorMergeTest mergeTest;
mergeTest.TestScenarios(result_, name_,
Bias(Vector3(0.03, -0.02, 0.01), Vector3(-0.01, 0.02, -0.03)),
Bias(Vector3(0.01, 0.02, 0.03), Vector3(0.03, -0.02, 0.01)), 1e-1);
}
// Test case with non-zero coriolis
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() {