Unit test that fails! Bias estimated is opposite in direction for test bodyPSensorWithBias!
Not equal: expected: .biasAcc [0 0 0] expected: .biasGyro [ 0 0.01 0] actual: .biasAcc [-0.00012 3.6e-16 0.00015] actual: .biasGyro [-3.7e-18 -0.01 -3.6e-18]release/4.3a0
krunalchande
parent
b2dcf35e77
commit
77f69146f6
|
|
@ -922,6 +922,141 @@ TEST(ImuFactor, PredictRotation) {
|
||||||
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity)));
|
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity)));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ImuFactor, bodyPSensorNoBias) {
|
||||||
|
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
|
||||||
|
|
||||||
|
// Measurements
|
||||||
|
Vector3 n_gravity; n_gravity << 0, 0, -9.81; // z-up nav frame
|
||||||
|
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
|
||||||
|
// Sensor frame is z-down
|
||||||
|
// Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
|
||||||
|
Vector3 s_omegaMeas_ns; s_omegaMeas_ns << 0, 0.1, M_PI/10;
|
||||||
|
// Acc measurement is acceleration of sensor in the sensor frame, when stationary, table exerts an equal and opposite force
|
||||||
|
// w.r.t gravity
|
||||||
|
Vector3 s_accMeas; s_accMeas << 0,0,-9.81;
|
||||||
|
double dt = 0.001;
|
||||||
|
Pose3 b_P_s(Rot3::ypr(0,0,M_PI), Point3(0,0,0)); // Rotate sensor (z-down) to body (same as navigation) i.e. z-up
|
||||||
|
|
||||||
|
Matrix I6x6(6,6);
|
||||||
|
I6x6 = Matrix::Identity(6,6);
|
||||||
|
|
||||||
|
ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(),
|
||||||
|
Matrix3::Zero(), Matrix3::Zero(), true);
|
||||||
|
|
||||||
|
for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, b_P_s);
|
||||||
|
|
||||||
|
// Create factor
|
||||||
|
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis);
|
||||||
|
|
||||||
|
// Predict
|
||||||
|
Pose3 x1;
|
||||||
|
Vector3 v1(0, 0.0, 0.0);
|
||||||
|
PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, n_gravity, omegaCoriolis);
|
||||||
|
Pose3 expectedPose(Rot3().ypr(-M_PI/10, 0, 0), Point3(0, 0, 0));
|
||||||
|
Vector3 expectedVelocity; expectedVelocity<<0,0,0;
|
||||||
|
EXPECT(assert_equal(expectedPose, poseVelocity.pose));
|
||||||
|
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>
|
||||||
|
#include <gtsam/nonlinear/Marginals.h>
|
||||||
|
|
||||||
|
TEST(ImuFactor, bodyPSensorWithBias) {
|
||||||
|
int numFactors = 80;
|
||||||
|
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);
|
||||||
|
|
||||||
|
// Measurements
|
||||||
|
Vector3 n_gravity; n_gravity << 0, 0, -9.81;
|
||||||
|
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
|
||||||
|
|
||||||
|
// Sensor frame is z-down
|
||||||
|
// Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
|
||||||
|
Vector3 measuredOmega; measuredOmega << 0, 0.01, 0.0;
|
||||||
|
// Acc measurement is acceleration of sensor in the sensor frame, when stationary, table exerts an equal and opposite force
|
||||||
|
// w.r.t gravity
|
||||||
|
Vector3 measuredAcc; measuredAcc << 0,0,-9.81;
|
||||||
|
|
||||||
|
Pose3 bPs(Rot3::ypr(0,0,M_PI), Point3());
|
||||||
|
|
||||||
|
Matrix3 accCov = 1e-4 * I_3x3;
|
||||||
|
Matrix3 gyroCov = 1e-6 * I_3x3;
|
||||||
|
Matrix3 integrationCov = 1e-8 * I_3x3;
|
||||||
|
double deltaT = 0.005;
|
||||||
|
|
||||||
|
// 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.1, 0.1, 0.1).finished());
|
||||||
|
Vector6 priorNoiseBiasSigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5e-1, 0.5e-1, 0.5e-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);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
Values values;
|
||||||
|
NonlinearFactorGraph graph;
|
||||||
|
|
||||||
|
PriorFactor<Pose3> priorPose(X(0), Pose3(), priorNoisePose);
|
||||||
|
graph.add(priorPose);
|
||||||
|
values.insert(X(0), Pose3());
|
||||||
|
|
||||||
|
PriorFactor<Vector3> priorVel(V(0), zeroVel, priorNoiseVel);
|
||||||
|
graph.add(priorVel);
|
||||||
|
values.insert(V(0), zeroVel);
|
||||||
|
|
||||||
|
PriorFactor<imuBias::ConstantBias> priorBias(B(0), zeroBias, priorNoiseBias);
|
||||||
|
graph.add(priorBias);
|
||||||
|
values.insert(B(0), zeroBias);
|
||||||
|
|
||||||
|
for (int i = 1; i < numFactors; i++) {
|
||||||
|
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<200; ++j) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT, bPs);
|
||||||
|
|
||||||
|
// Create factor
|
||||||
|
ImuFactor factor(X(i-1), V(i-1), X(i), V(i), B(i-1), pre_int_data, n_gravity, omegaCoriolis);
|
||||||
|
graph.add(factor);
|
||||||
|
graph.add(BetweenFactor<imuBias::ConstantBias>(B(i-1), B(i), zeroBias, biasNoiseModel));
|
||||||
|
|
||||||
|
values.insert(X(i), Pose3());
|
||||||
|
values.insert(V(i), zeroVel);
|
||||||
|
values.insert(B(i), zeroBias);
|
||||||
|
}
|
||||||
|
Values results = LevenbergMarquardtOptimizer(graph, values).optimize();
|
||||||
|
cout.precision(2);
|
||||||
|
Marginals marginals(graph, results);
|
||||||
|
imuBias::ConstantBias biasActual = results.at<imuBias::ConstantBias>(B(numFactors-1));
|
||||||
|
|
||||||
|
imuBias::ConstantBias biasExpected(Vector3(0,0,0), Vector3(0, 0.01, 0.0));
|
||||||
|
EXPECT(assert_equal(biasExpected, biasActual, 1e-3));
|
||||||
|
|
||||||
|
// results.print("Results: \n");
|
||||||
|
//
|
||||||
|
// for (int i = 0; i < numFactors; i++) {
|
||||||
|
// imuBias::ConstantBias currentBias = results.at<imuBias::ConstantBias>(B(i));
|
||||||
|
// cout << "currentBiasEstimate: " << currentBias.vector().transpose() << endl;
|
||||||
|
// }
|
||||||
|
// for (int i = 0; i < numFactors; i++) {
|
||||||
|
// Matrix biasCov = marginals.marginalCovariance(B(i));
|
||||||
|
// cout << "b" << i << " cov: " << biasCov.diagonal().transpose() << endl;
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// for (int i = 0; i < numFactors; i++) {
|
||||||
|
// Pose3 currentPose = results.at<Pose3>(X(i));
|
||||||
|
// cout << "currentYPREstimate: " << currentPose.rotation().ypr().transpose()*180/M_PI << endl;
|
||||||
|
// }
|
||||||
|
// for (int i = 0; i < numFactors; i++)
|
||||||
|
// cout << "x" << i << " covariance: " << marginals.marginalCovariance(X(i)).diagonal().transpose() << endl;
|
||||||
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue