Merge branch 'fix/imuFactor_BodyPSensor' into feature/cleanup_ImuFactor
Conflicts: gtsam/navigation/CombinedImuFactor.cpp gtsam/navigation/ImuFactor.cpp gtsam/navigation/PreintegrationBase.cpp gtsam/navigation/PreintegrationBase.h gtsam/navigation/tests/testImuFactor.cpprelease/4.3a0
dellaert
commit
d5d8000926
|
|
@ -59,8 +59,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
|
|||
// (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
|
||||
|
||||
Vector3 correctedAcc, correctedOmega;
|
||||
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
|
||||
&correctedAcc, &correctedOmega);
|
||||
boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor);
|
||||
|
||||
const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
|
||||
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
|
||||
|
|
|
|||
|
|
@ -54,8 +54,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
|
|||
OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
|
||||
|
||||
Vector3 correctedAcc, correctedOmega;
|
||||
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
|
||||
&correctedAcc, &correctedOmega);
|
||||
boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor);
|
||||
|
||||
// rotation increment computed from the current rotation rate measurement
|
||||
const Vector3 integratedOmega = correctedOmega * deltaT;
|
||||
|
|
|
|||
|
|
@ -102,23 +102,30 @@ void PreintegrationBase::updatePreintegratedJacobians(
|
|||
update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
|
||||
}
|
||||
|
||||
void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
|
||||
std::pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega,
|
||||
Vector3* correctedAcc, Vector3* correctedOmega) {
|
||||
*correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
|
||||
*correctedOmega = biasHat_.correctGyroscope(measuredOmega);
|
||||
boost::optional<const Pose3&> body_P_sensor) const {
|
||||
// Correct for bias in the sensor frame
|
||||
Vector3 s_correctedAcc, s_correctedOmega;
|
||||
s_correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
|
||||
s_correctedOmega = biasHat_.correctGyroscope(measuredOmega);
|
||||
|
||||
// Then compensate for sensor-body displacement: we express the quantities
|
||||
// Compensate for sensor-body displacement if needed: we express the quantities
|
||||
// (originally in the IMU frame) into the body frame
|
||||
if (p().body_P_sensor) {
|
||||
Matrix3 body_R_sensor = p().body_P_sensor->rotation().matrix();
|
||||
*correctedOmega = body_R_sensor * (*correctedOmega); // rotation rate vector in the body frame
|
||||
Matrix3 body_omega_body__cross = skewSymmetric(*correctedOmega);
|
||||
*correctedAcc = body_R_sensor * (*correctedAcc)
|
||||
- body_omega_body__cross * body_omega_body__cross
|
||||
* p().body_P_sensor->translation().vector();
|
||||
// linear acceleration vector in the body frame
|
||||
}
|
||||
// Equations below assume the "body" frame is the CG
|
||||
if (body_P_sensor) {
|
||||
Matrix3 bRs = body_P_sensor->rotation().matrix();
|
||||
Vector3 b_correctedOmega = bRs * s_correctedOmega; // rotation rate vector in the body frame
|
||||
Matrix3 body_omega_body__cross = skewSymmetric(b_correctedOmega);
|
||||
Vector3 b_arm = body_P_sensor->translation().vector();
|
||||
Vector3 b_velocity_bs = b_correctedOmega.cross(b_arm); // magnitude: omega * arm
|
||||
// Subtract out the the centripetal acceleration from the measured one
|
||||
// to get linear acceleration vector in the body frame:
|
||||
Vector3 b_correctedAcc = bRs * s_correctedAcc
|
||||
- b_correctedOmega.cross(b_velocity_bs);
|
||||
return std::make_pair(b_correctedAcc, b_correctedOmega);
|
||||
} else
|
||||
return std::make_pair(correctedAcc, s_correctedOmega);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -177,9 +177,9 @@ public:
|
|||
void updatePreintegratedJacobians(const Vector3& correctedAcc,
|
||||
const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT);
|
||||
|
||||
void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, Vector3* correctedAcc,
|
||||
Vector3* correctedOmega);
|
||||
std::pair<Vector3, Vector3>
|
||||
correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega) const;
|
||||
|
||||
/// Given the estimate of the bias, return a NavState tangent vector
|
||||
/// summarizing the preintegrated IMU measurements so far
|
||||
|
|
|
|||
|
|
@ -958,6 +958,141 @@ TEST(ImuFactor, PredictArbitrary) {
|
|||
EXPECT(assert_equal(Vector(expectedV), v2, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, bodyPSensorNoBias) {
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
|
||||
|
||||
// Measurements
|
||||
Vector3 n_gravity(0, 0, -9.81); // z-up nav frame
|
||||
Vector3 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(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(0, 0, -9.81);
|
||||
double dt = 0.001;
|
||||
|
||||
// Rotate sensor (z-down) to body (same as navigation) i.e. z-up
|
||||
Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0));
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true);
|
||||
|
||||
for (int i = 0; i < 1000; ++i)
|
||||
pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, body_P_sensor);
|
||||
|
||||
// Create factor
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, n_gravity, omegaCoriolis);
|
||||
|
||||
// Predict
|
||||
Pose3 x1;
|
||||
Vector3 v1(0, 0, 0);
|
||||
PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity,
|
||||
omegaCoriolis);
|
||||
|
||||
Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0));
|
||||
EXPECT(assert_equal(expectedPose, poseVelocity.pose));
|
||||
|
||||
Vector3 expectedVelocity(0, 0, 0);
|
||||
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) {
|
||||
using noiseModel::Diagonal;
|
||||
typedef imuBias::ConstantBias Bias;
|
||||
|
||||
int numFactors = 80;
|
||||
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 = Diagonal::Sigmas(noiseBetweenBiasSigma);
|
||||
|
||||
// Measurements
|
||||
Vector3 n_gravity(0, 0, -9.81);
|
||||
Vector3 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(0, 0.01, 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(0, 0, -9.81);
|
||||
|
||||
Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3());
|
||||
|
||||
Matrix3 accCov = 1e-7 * I_3x3;
|
||||
Matrix3 gyroCov = 1e-8 * I_3x3;
|
||||
Matrix3 integrationCov = 1e-9 * 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 = Diagonal::Sigmas(priorNoisePoseSigmas);
|
||||
SharedDiagonal priorNoiseVel = Diagonal::Sigmas(priorNoiseVelSigmas);
|
||||
SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
|
||||
Vector3 zeroVel(0, 0, 0);
|
||||
|
||||
// Create a factor graph with priors on initial pose, vlocity and bias
|
||||
NonlinearFactorGraph graph;
|
||||
Values values;
|
||||
|
||||
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);
|
||||
|
||||
// The key to this test is that we specify the bias, in the sensor frame, as known a priori
|
||||
// We also create factors below that encode our assumption that this bias is constant over time
|
||||
// In theory, after optimization, we should recover that same bias estimate
|
||||
Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
|
||||
PriorFactor<Bias> priorBiasFactor(B(0), priorBias, priorNoiseBias);
|
||||
graph.add(priorBiasFactor);
|
||||
values.insert(B(0), priorBias);
|
||||
|
||||
// Now add IMU factors and bias noise models
|
||||
Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0));
|
||||
for (int i = 1; i < numFactors; i++) {
|
||||
ImuFactor::PreintegratedMeasurements pim =
|
||||
ImuFactor::PreintegratedMeasurements(zeroBias, accCov, gyroCov,
|
||||
integrationCov, true);
|
||||
for (int j = 0; j < 200; ++j)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT,
|
||||
body_P_sensor);
|
||||
|
||||
// Create factors
|
||||
graph.add(
|
||||
ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim, n_gravity,
|
||||
omegaCoriolis));
|
||||
graph.add(BetweenFactor<Bias>(B(i - 1), B(i), zeroBias, biasNoiseModel));
|
||||
|
||||
values.insert(X(i), Pose3());
|
||||
values.insert(V(i), zeroVel);
|
||||
values.insert(B(i), priorBias);
|
||||
}
|
||||
|
||||
// Finally, optimize, and get bias at last time step
|
||||
Values results = LevenbergMarquardtOptimizer(graph, values).optimize();
|
||||
Bias biasActual = results.at<Bias>(B(numFactors - 1));
|
||||
|
||||
// And compare it with expected value (our prior)
|
||||
Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0));
|
||||
EXPECT(assert_equal(biasExpected, biasActual, 1e-3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue