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

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)

release/4.3a0
Luca 2014-12-08 18:41:39 -05:00
parent 02f92e4e04
commit dc13912ce2
5 changed files with 120 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam/navigation/AHRSFactor.cpp
View File

@ -69,7 +69,6 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredOmega, double deltaT, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor) { 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 // First we compensate the measurements for the bias
Vector3 correctedOmega = measuredOmega - biasHat_; Vector3 correctedOmega = measuredOmega - biasHat_;
@ -84,42 +83,20 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
// rotation vector describing rotation increment computed from the // rotation vector describing rotation increment computed from the
// current rotation rate measurement // current rotation rate measurement
const Vector3 theta_incr = correctedOmega * deltaT; const Vector3 theta_incr = correctedOmega * deltaT;
Matrix3 Jr_theta_incr;
// rotation increment computed from the current rotation rate measurement const Rot3 incrR = Rot3::Expmap(theta_incr, Jr_theta_incr); // expensive !!
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);
// Update Jacobian // Update Jacobian
update_delRdelBiasOmega(Jr_theta_incr, incrR, deltaT); update_delRdelBiasOmega(Jr_theta_incr, incrR, deltaT);
// Update preintegrated measurements covariance // Update rotation and deltaTij.
const Vector3 theta_i = thetaRij(); // super-expensive, Parameterization of so(3) Matrix3 Fr; // Jacobian of the update
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr);
// 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;
// first order uncertainty propagation // first order uncertainty propagation
// the deltaT allows to pass from continuous time noise to discrete time noise // 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; + measurementCovariance_ * deltaT;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------

39
gtsam/navigation/ImuFactor.cpp
View File

@ -70,9 +70,6 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
boost::optional<const Pose3&> body_P_sensor, boost::optional<const Pose3&> body_P_sensor,
boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) { 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; Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); 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 const Rot3 Rincr = Rot3::Expmap(theta_incr, Jr_theta_incr); // rotation increment computed from the current rotation rate measurement
// Update Jacobians // Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
updatePreintegratedJacobians(correctedAcc, Jr_theta_incr, Rincr, deltaT); 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 // 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 // the deltaT allows to pass from continuous time noise to discrete time noise
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
// Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * 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){ if(F_test){
// This in only for testing // This in only for testing
F_test->resize(9,9); F_test->resize(9,9);
@ -129,7 +106,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// intNoise accNoise omegaNoise // intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos
Z_3x3, R_i * deltaT, Z_3x3, // vel 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: // Propagation with no approximation:
// preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose(); // preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose();
} }

7
gtsam/navigation/PreintegratedRotation.h
View File

@ -74,9 +74,14 @@ public:
} }
/// Update preintegrated measurements /// 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; deltaRij_ = deltaRij_ * incrR;
deltaTij_ += deltaT; deltaTij_ += deltaT;
if(H){
H->resize(3,3);
*H = incrR.transpose();
}
} }
/** /**

15
gtsam/navigation/PreintegrationBase.h
View File

@ -116,7 +116,8 @@ public:
/// Update preintegrated measurements /// Update preintegrated measurements
void updatePreintegratedMeasurements(const Vector3& correctedAcc, 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 Matrix3 dRij = deltaRij(); // expensive
Vector3 temp = dRij * correctedAcc * deltaT; Vector3 temp = dRij * correctedAcc * deltaT;
if(!use2ndOrderIntegration_){ if(!use2ndOrderIntegration_){
@ -125,7 +126,17 @@ public:
deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
} }
deltaVij_ += temp; 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 /// Update Jacobians to be used during preintegration

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

@ -37,6 +37,8 @@ using symbol_shorthand::B;
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
// Auxiliary functions to test evaluate error in ImuFactor
/* ************************************************************************* */
Vector callEvaluateError(const ImuFactor& factor, Vector callEvaluateError(const ImuFactor& factor,
const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias){ 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) ) ; 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 // Auxiliary functions to test Jacobians F and G used for
Vector updatePreintegratedMeasurementsTest( // covariance propagation during preintegration
const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Vector3& logDeltaRij_old, /* ************************************************************************* */
Vector updatePreintegratedPosVel(
const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration_) { const bool use2ndOrderIntegration_) {
Rot3 deltaRij_old = Rot3::Expmap(logDeltaRij_old);
Matrix3 dRij = deltaRij_old.matrix(); Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * correctedAcc * deltaT; Vector3 temp = dRij * correctedAcc * deltaT;
Vector3 deltaPij_new; Vector3 deltaPij_new;
@ -65,12 +68,20 @@ Vector updatePreintegratedMeasurementsTest(
deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
} }
Vector3 deltaVij_new = deltaVij_old + temp; Vector3 deltaVij_new = deltaVij_old + temp;
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT);
Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); Vector result(6); result << deltaPij_new, deltaVij_new;
Vector result(9); result << deltaPij_new, deltaVij_new, logDeltaRij_new;
return result; 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( ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs, const list<Vector3>& measuredAccs,
@ -356,6 +367,24 @@ TEST( ImuFactor, PartialDerivativeLogmap )
EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); 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 ) TEST( ImuFactor, fistOrderExponential )
{ {
@ -469,7 +498,6 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0);
const double newDeltaT = 0.01; 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 Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
@ -480,44 +508,67 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
bool use2ndOrderIntegration = false; bool use2ndOrderIntegration = false;
// Compute expected F wrt positions //////////////////////////////////////////////////////////////////////////////////////////////
Matrix df_dpos = // COMPUTE NUMERICAL DERIVATIVES FOR F
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, //////////////////////////////////////////////////////////////////////////////////////////////
_1, deltaVij_old, logDeltaRij_old, // 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); newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
// Compute expected F wrt velocities // Compute expected f_pos_vel wrt velocities
Matrix df_dvel = Matrix dfpv_dvel =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, _1, logDeltaRij_old, deltaPij_old, _1, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
// Compute expected F wrt angles // Compute expected f_pos_vel wrt angles
Matrix df_dangle = Matrix dfpv_dangle =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, _1, deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), logDeltaRij_old); newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
Matrix Fexpected(9,9); Matrix FexpectedTop6(6,9);
FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
Fexpected << df_dpos, df_dvel, df_dangle; EXPECT(assert_equal(FexpectedTop6, Factual.topRows(6)));
EXPECT(assert_equal(Fexpected, Factual));
// Compute expected G wrt integration noise // Compute expected f_rot wrt angles
Matrix df_dintNoise(9,3); Matrix dfr_dangle =
df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3; numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedRot,
_1, newMeasuredOmega, newDeltaT), deltaRij_old);
// Compute expected F wrt acc noise Matrix FexpectedBottom3(3,9);
Matrix df_daccNoise = FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, EXPECT(assert_equal(FexpectedBottom3, Factual.bottomRows(3)));
deltaPij_old, deltaVij_old, logDeltaRij_old,
//////////////////////////////////////////////////////////////////////////////////////////////
// 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); _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
// Compute expected F wrt gyro noise // Compute expected F wrt gyro noise
Matrix df_domegaNoise = Matrix dgpv_domegaNoise =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, logDeltaRij_old, deltaPij_old, deltaVij_old, deltaRij_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); 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; // Compute expected f_rot wrt gyro noise
EXPECT(assert_equal(Gexpected, Gactual)); 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> //#include <gtsam/linear/GaussianFactorGraph.h>