diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 4b55e0ee3..0b672608d 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -922,79 +922,6 @@ TEST(ImuFactor, PredictRotation) { EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); } -/* ************************************************************************* */ -#include -#include -#include -#include - -TEST(ImuFactor, CheckBiasCorrection) { - int numFactors = 2; - int numSamplesPreint = 1; - double g = 9.81; - // Measurements. Body frame and nav frame are both z-up - Vector3 measuredOmega; measuredOmega << 0, 0.3, 0.0; - Vector3 measuredAcc; measuredAcc << 0, 0, g; - Vector3 gravity; gravity << 0, 0, -g; - - // Set up noise and other test params - imuBias::ConstantBias zeroBias(Vector3(0, 0, 0), Vector3(0.0, 0, 0)); // Biases (acc, rot) - Vector6 noiseBetweenBiasSigma; noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6, 3.0e-6, 3.0e-6); - SharedDiagonal biasNoiseModel = noiseModel::Diagonal::Sigmas(noiseBetweenBiasSigma); - Matrix3 accCov = 1e-4 * I_3x3; - Matrix3 gyroCov = 1e-6 * I_3x3; - Matrix3 integrationCov = 1e-8 * I_3x3; - double deltaT = 0.005; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - - // Specify noise values on priors - Vector6 priorNoisePoseSigmas((Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished()); - Vector3 priorNoiseVelSigmas((Vector(3) << 0.5, 0.5, 0.5).finished()); - Vector6 priorNoiseBiasSigmas((Vector(6) << 1, 1, 1, 1, 1, 1).finished()); - SharedDiagonal priorNoisePose = noiseModel::Diagonal::Sigmas(priorNoisePoseSigmas); - SharedDiagonal priorNoiseVel = noiseModel::Diagonal::Sigmas(priorNoiseVelSigmas); - SharedDiagonal priorNoiseBias = noiseModel::Diagonal::Sigmas(priorNoiseBiasSigmas); - Vector3 zeroVel(0, 0.0, 0.0); - - // Instantiate graph and values - Values values; - NonlinearFactorGraph graph; - - // Add prior factor and values - graph.add(PriorFactor (X(0), Pose3(), priorNoisePose)); - graph.add(PriorFactor(V(0), zeroVel, priorNoiseVel)); - graph.add(PriorFactor(B(0), zeroBias, priorNoiseBias)); - values.insert(X(0), Pose3()); - values.insert(V(0), zeroVel); - values.insert(B(0), zeroBias); - - for (int i = 1; i < numFactors; i++) { - // Preintegrate measurements - ImuFactor::PreintegratedMeasurements pre_int_data = ImuFactor::PreintegratedMeasurements(imuBias::ConstantBias(Vector3(0, 0.0, 0.0), - Vector3(0.0, 0.0, 0.0)), accCov, gyroCov, integrationCov, true); - for (int j = 0; j< numSamplesPreint; ++j) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - // Create and add factor - ImuFactor factor(X(i-1), V(i-1), X(i), V(i), B(i-1), pre_int_data, gravity, omegaCoriolis); - graph.add(factor); - graph.add(BetweenFactor(B(i-1), B(i), zeroBias, biasNoiseModel)); - - if (i == 30) graph.add(PriorFactor(X(i), Pose3(), priorNoisePose)); - - // Add values - values.insert(X(i), Pose3()); - values.insert(V(i), zeroVel); - values.insert(B(i), zeroBias); - } - // Solve graph and find estimated bias - Values results = LevenbergMarquardtOptimizer(graph, values).optimize(); - imuBias::ConstantBias biasActual = results.at(B(numFactors-1)); -// -// // set expected bias -// imuBias::ConstantBias biasExpected(Vector3(0,0,0), Vector3(0, 0.3, 0.0)); -// EXPECT(assert_equal(biasExpected, biasActual, 1e-2)); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */