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

Test not needed for the purposes of the P.R

release/4.3a0
krunalchande 2015-03-08 15:35:01 -04:00
parent 5d739ea904
commit 736fce27db
1 changed files with 0 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -922,79 +922,6 @@ TEST(ImuFactor, PredictRotation) {
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity)));
}
/* ************************************************************************* */
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
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<Pose3> (X(0), Pose3(), priorNoisePose));
graph.add(PriorFactor<Vector3>(V(0), zeroVel, priorNoiseVel));
graph.add(PriorFactor<imuBias::ConstantBias>(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<imuBias::ConstantBias>(B(i-1), B(i), zeroBias, biasNoiseModel));
if (i == 30) graph.add(PriorFactor<Pose3>(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<imuBias::ConstantBias>(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);}
/* ************************************************************************* */