moved jacobian computation to updateMeasurement functions, and fixed noise propagation. Luca&Christian: insight is that preintegration noise acts on rotations as R * expmap(noise), while before it was expmap( logmap(R) + noise)
Luca
parent
02f92e4e04
commit
dc13912ce2
|
|
@ -69,7 +69,6 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
|
|||
const Vector3& measuredOmega, double deltaT,
|
||||
boost::optional<const Pose3&> body_P_sensor) {
|
||||
|
||||
// NOTE: order is important here because each update uses old values.
|
||||
// First we compensate the measurements for the bias
|
||||
Vector3 correctedOmega = measuredOmega - biasHat_;
|
||||
|
||||
|
|
@ -84,42 +83,20 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
|
|||
// rotation vector describing rotation increment computed from the
|
||||
// current rotation rate measurement
|
||||
const Vector3 theta_incr = correctedOmega * deltaT;
|
||||
|
||||
// rotation increment computed from the current rotation rate measurement
|
||||
const Rot3 incrR = Rot3::Expmap(theta_incr); // expensive !!
|
||||
const Matrix3 incrRt = incrR.transpose();
|
||||
|
||||
// Right Jacobian computed at theta_incr
|
||||
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr);
|
||||
Matrix3 Jr_theta_incr;
|
||||
const Rot3 incrR = Rot3::Expmap(theta_incr, Jr_theta_incr); // expensive !!
|
||||
|
||||
// Update Jacobian
|
||||
update_delRdelBiasOmega(Jr_theta_incr, incrR, deltaT);
|
||||
|
||||
// Update preintegrated measurements covariance
|
||||
const Vector3 theta_i = thetaRij(); // super-expensive, Parameterization of so(3)
|
||||
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i);
|
||||
|
||||
// Update rotation and deltaTij. TODO Frank moved from end of this function !!!
|
||||
updateIntegratedRotationAndDeltaT(incrR, deltaT);
|
||||
|
||||
const Vector3 theta_j = thetaRij(); // super-expensive, , Parameterization of so(3)
|
||||
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
|
||||
|
||||
// Update preintegrated measurements covariance: as in [2] we consider a first
|
||||
// order propagation that can be seen as a prediction phase in an EKF framework
|
||||
Matrix3 H_angles_angles = Jrinv_theta_j * incrRt * Jr_theta_i;
|
||||
// analytic expression corresponding to the following numerical derivative
|
||||
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>
|
||||
// (boost::bind(&DeltaAngles, correctedOmega, deltaT, _1), thetaij);
|
||||
|
||||
// overall Jacobian wrpt preintegrated measurements (df/dx)
|
||||
const Matrix3& F = H_angles_angles;
|
||||
// Update rotation and deltaTij.
|
||||
Matrix3 Fr; // Jacobian of the update
|
||||
updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr);
|
||||
|
||||
// first order uncertainty propagation
|
||||
// the deltaT allows to pass from continuous time noise to discrete time noise
|
||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose()
|
||||
preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose()
|
||||
+ measurementCovariance_ * deltaT;
|
||||
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -70,9 +70,6 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
|
|||
boost::optional<const Pose3&> body_P_sensor,
|
||||
boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
|
||||
|
||||
// NOTE: order is important here because each update uses old values (i.e., we have to update
|
||||
// jacobians and covariances before updating preintegrated measurements).
|
||||
|
||||
Vector3 correctedAcc, correctedOmega;
|
||||
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);
|
||||
|
||||
|
|
@ -81,42 +78,22 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
|
|||
const Rot3 Rincr = Rot3::Expmap(theta_incr, Jr_theta_incr); // rotation increment computed from the current rotation rate measurement
|
||||
|
||||
// Update Jacobians
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
updatePreintegratedJacobians(correctedAcc, Jr_theta_incr, Rincr, deltaT);
|
||||
|
||||
// Update preintegrated measurements covariance
|
||||
// Update preintegrated measurements (also get Jacobian)
|
||||
const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test
|
||||
Matrix F; // overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F);
|
||||
|
||||
// first order covariance propagation:
|
||||
// as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
// Matrix3 Jr_theta_i;
|
||||
// const Vector3 theta_i = thetaRij(Jr_theta_i); // super-expensive parametrization of so(3)
|
||||
// const Matrix3 R_i = deltaRij();
|
||||
|
||||
const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3)
|
||||
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
|
||||
const Matrix3 R_i = deltaRij();
|
||||
|
||||
// Update preintegrated measurements
|
||||
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT);
|
||||
|
||||
Matrix3 Jrinv_theta_j; thetaRij(Jrinv_theta_j); // computation of rightJacobianInverse
|
||||
|
||||
Matrix H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
|
||||
Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
|
||||
|
||||
// overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
Matrix F(9,9);
|
||||
// pos vel angle
|
||||
F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos
|
||||
Z_3x3, I_3x3, H_vel_angles, // vel
|
||||
Z_3x3, Z_3x3, H_angles_angles;// angle
|
||||
|
||||
// first order uncertainty propagation:
|
||||
// the deltaT allows to pass from continuous time noise to discrete time noise
|
||||
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
|
||||
// Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
|
||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
|
||||
|
||||
// F_test and Gout are used for testing purposes and are not needed by the factor
|
||||
// F_test and G_test are used for testing purposes and are not needed by the factor
|
||||
if(F_test){
|
||||
// This in only for testing
|
||||
F_test->resize(9,9);
|
||||
|
|
@ -129,7 +106,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
|
|||
// intNoise accNoise omegaNoise
|
||||
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos
|
||||
Z_3x3, R_i * deltaT, Z_3x3, // vel
|
||||
Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;// angle
|
||||
Z_3x3, Z_3x3, Jr_theta_incr * deltaT; // angle
|
||||
// Propagation with no approximation:
|
||||
// preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -74,9 +74,14 @@ public:
|
|||
}
|
||||
|
||||
/// Update preintegrated measurements
|
||||
void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT){
|
||||
void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
|
||||
boost::optional<Matrix3&> H = boost::none){
|
||||
deltaRij_ = deltaRij_ * incrR;
|
||||
deltaTij_ += deltaT;
|
||||
if(H){
|
||||
H->resize(3,3);
|
||||
*H = incrR.transpose();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -116,7 +116,8 @@ public:
|
|||
|
||||
/// Update preintegrated measurements
|
||||
void updatePreintegratedMeasurements(const Vector3& correctedAcc,
|
||||
const Rot3& incrR, const double deltaT) {
|
||||
const Rot3& incrR, const double deltaT, boost::optional<Matrix&> F = boost::none) {
|
||||
|
||||
Matrix3 dRij = deltaRij(); // expensive
|
||||
Vector3 temp = dRij * correctedAcc * deltaT;
|
||||
if(!use2ndOrderIntegration_){
|
||||
|
|
@ -125,7 +126,17 @@ public:
|
|||
deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
|
||||
}
|
||||
deltaVij_ += temp;
|
||||
updateIntegratedRotationAndDeltaT(incrR,deltaT);
|
||||
Matrix3 F_angles_angles;
|
||||
const Matrix3 R_i = deltaRij();
|
||||
updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles);
|
||||
Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
|
||||
if(F){
|
||||
F->resize(9,9);
|
||||
// pos vel angle
|
||||
*F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos
|
||||
Z_3x3, I_3x3, F_vel_angles, // vel
|
||||
Z_3x3, Z_3x3, F_angles_angles;// angle
|
||||
}
|
||||
}
|
||||
|
||||
/// Update Jacobians to be used during preintegration
|
||||
|
|
|
|||
|
|
@ -37,6 +37,8 @@ using symbol_shorthand::B;
|
|||
|
||||
/* ************************************************************************* */
|
||||
namespace {
|
||||
// Auxiliary functions to test evaluate error in ImuFactor
|
||||
/* ************************************************************************* */
|
||||
Vector callEvaluateError(const ImuFactor& factor,
|
||||
const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias){
|
||||
|
|
@ -49,13 +51,14 @@ Rot3 evaluateRotationError(const ImuFactor& factor,
|
|||
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
|
||||
}
|
||||
|
||||
// Correspond to updatePreintegratedMeasurements, but has a different syntax to test numerical derivatives
|
||||
Vector updatePreintegratedMeasurementsTest(
|
||||
const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Vector3& logDeltaRij_old,
|
||||
// Auxiliary functions to test Jacobians F and G used for
|
||||
// covariance propagation during preintegration
|
||||
/* ************************************************************************* */
|
||||
Vector updatePreintegratedPosVel(
|
||||
const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
|
||||
const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT,
|
||||
const bool use2ndOrderIntegration_) {
|
||||
|
||||
Rot3 deltaRij_old = Rot3::Expmap(logDeltaRij_old);
|
||||
Matrix3 dRij = deltaRij_old.matrix();
|
||||
Vector3 temp = dRij * correctedAcc * deltaT;
|
||||
Vector3 deltaPij_new;
|
||||
|
|
@ -65,12 +68,20 @@ Vector updatePreintegratedMeasurementsTest(
|
|||
deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
|
||||
}
|
||||
Vector3 deltaVij_new = deltaVij_old + temp;
|
||||
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT);
|
||||
Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new);
|
||||
Vector result(9); result << deltaPij_new, deltaVij_new, logDeltaRij_new;
|
||||
|
||||
Vector result(6); result << deltaPij_new, deltaVij_new;
|
||||
return result;
|
||||
}
|
||||
|
||||
Rot3 updatePreintegratedRot(const Rot3& deltaRij_old,
|
||||
const Vector3& correctedOmega, const double deltaT) {
|
||||
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT);
|
||||
return deltaRij_new;
|
||||
}
|
||||
|
||||
// Auxiliary functions to test preintegrated Jacobians
|
||||
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
|
||||
/* ************************************************************************* */
|
||||
ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias,
|
||||
const list<Vector3>& measuredAccs,
|
||||
|
|
@ -356,6 +367,24 @@ TEST( ImuFactor, PartialDerivativeLogmap )
|
|||
EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
|
||||
}
|
||||
|
||||
Rot3 constRot = Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0);
|
||||
Rot3 testRot(const Rot3& Rk){
|
||||
return Rk * constRot;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuFactor, understandRot )
|
||||
{
|
||||
Rot3 Rbar = Rot3::RzRyRx( M_PI, M_PI/6.0, -M_PI/4.0 );
|
||||
|
||||
Matrix Jexpected = numericalDerivative11<Rot3,Rot3>(boost::bind(
|
||||
&testRot, _1), Rbar);
|
||||
|
||||
Matrix3 Jactual = constRot.transpose();
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuFactor, fistOrderExponential )
|
||||
{
|
||||
|
|
@ -469,7 +498,6 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
|
|||
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
|
||||
const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0);
|
||||
const double newDeltaT = 0.01;
|
||||
const Vector3 logDeltaRij_old = preintegrated.thetaRij(); // before adding new measurement
|
||||
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
|
||||
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
|
||||
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
|
||||
|
|
@ -480,44 +508,67 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
|
|||
|
||||
bool use2ndOrderIntegration = false;
|
||||
|
||||
// Compute expected F wrt positions
|
||||
Matrix df_dpos =
|
||||
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
|
||||
_1, deltaVij_old, logDeltaRij_old,
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// COMPUTE NUMERICAL DERIVATIVES FOR F
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// Compute expected f_pos_vel wrt positions
|
||||
Matrix dfpv_dpos =
|
||||
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
|
||||
_1, deltaVij_old, deltaRij_old,
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
|
||||
// Compute expected F wrt velocities
|
||||
Matrix df_dvel =
|
||||
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
|
||||
deltaPij_old, _1, logDeltaRij_old,
|
||||
// Compute expected f_pos_vel wrt velocities
|
||||
Matrix dfpv_dvel =
|
||||
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
|
||||
deltaPij_old, _1, deltaRij_old,
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
|
||||
// Compute expected F wrt angles
|
||||
Matrix df_dangle =
|
||||
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
|
||||
// Compute expected f_pos_vel wrt angles
|
||||
Matrix dfpv_dangle =
|
||||
numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedPosVel,
|
||||
deltaPij_old, deltaVij_old, _1,
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), logDeltaRij_old);
|
||||
Matrix Fexpected(9,9);
|
||||
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
|
||||
Matrix FexpectedTop6(6,9);
|
||||
FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
|
||||
|
||||
Fexpected << df_dpos, df_dvel, df_dangle;
|
||||
EXPECT(assert_equal(Fexpected, Factual));
|
||||
EXPECT(assert_equal(FexpectedTop6, Factual.topRows(6)));
|
||||
|
||||
// Compute expected G wrt integration noise
|
||||
Matrix df_dintNoise(9,3);
|
||||
df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3;
|
||||
// Compute expected f_rot wrt angles
|
||||
Matrix dfr_dangle =
|
||||
numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedRot,
|
||||
_1, newMeasuredOmega, newDeltaT), deltaRij_old);
|
||||
|
||||
// Compute expected F wrt acc noise
|
||||
Matrix df_daccNoise =
|
||||
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
|
||||
deltaPij_old, deltaVij_old, logDeltaRij_old,
|
||||
Matrix FexpectedBottom3(3,9);
|
||||
FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
|
||||
EXPECT(assert_equal(FexpectedBottom3, Factual.bottomRows(3)));
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// COMPUTE NUMERICAL DERIVATIVES FOR G
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// Compute jacobian wrt integration noise
|
||||
Matrix dgpv_dintNoise(6,3);
|
||||
dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3;
|
||||
|
||||
// Compute jacobian wrt acc noise
|
||||
Matrix dgpv_daccNoise =
|
||||
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
|
||||
deltaPij_old, deltaVij_old, deltaRij_old,
|
||||
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
|
||||
// Compute expected F wrt gyro noise
|
||||
Matrix df_domegaNoise =
|
||||
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
|
||||
deltaPij_old, deltaVij_old, logDeltaRij_old,
|
||||
Matrix dgpv_domegaNoise =
|
||||
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
|
||||
deltaPij_old, deltaVij_old, deltaRij_old,
|
||||
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
|
||||
Matrix Gexpected(9,9);
|
||||
Matrix GexpectedTop6(6,9);
|
||||
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
|
||||
EXPECT(assert_equal(GexpectedTop6, Gactual.topRows(6)));
|
||||
|
||||
Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise;
|
||||
EXPECT(assert_equal(Gexpected, Gactual));
|
||||
// Compute expected f_rot wrt gyro noise
|
||||
Matrix dgr_dangle =
|
||||
numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedRot,
|
||||
deltaRij_old, _1, newDeltaT), newMeasuredOmega);
|
||||
|
||||
Matrix GexpectedBottom3(3,9);
|
||||
GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
|
||||
EXPECT(assert_equal(GexpectedBottom3, Gactual.bottomRows(3)));
|
||||
}
|
||||
|
||||
//#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
|
|
|
|||
Loading…
Reference in New Issue