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

Refactoring of integrateMeasurement to reduce copy/paste

release/4.3a0
dellaert 2015-07-29 15:49:03 -07:00
parent 75cc3020eb
commit 0b4919e099
6 changed files with 79 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/AHRSFactor.cpp
View File

@ -64,15 +64,17 @@ void PreintegratedAhrsMeasurements::integrateMeasurement(
// rotation vector describing rotation increment computed from the
// current rotation rate measurement
const Vector3 theta_incr = correctedOmega * deltaT;
Matrix3 D_Rincr_integratedOmega;
const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !!
Matrix3 D_incrR_integratedOmega;
const Rot3 incrR = Rot3::Expmap(theta_incr, D_incrR_integratedOmega); // expensive !!
// Update Jacobian
update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT;
// Update rotation and deltaTij.
Matrix3 Fr; // Jacobian of the update
updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr);
deltaRij_ = deltaRij_.compose(incrR, Fr);
deltaTij_ += deltaT;
// first order uncertainty propagation
// the deltaT allows to pass from continuous time noise to discrete time noise

25
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -55,33 +55,22 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
// (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
const Matrix3 dRij = deltaRij_.matrix(); // expensive when quaternion
Vector3 correctedAcc, correctedOmega;
boost::tie(correctedAcc, correctedOmega) =
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega);
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
const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
// Update preintegrated measurements.
Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx)
updatePreintegratedMeasurements(measuredAcc, measuredOmega, deltaT,
&D_incrR_integratedOmega, &F_9x9);
// 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. In this implementation, contrarily to [2] we
// consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
const Matrix3 dRij = deltaRij_.matrix(); // expensive when quaternion
// Update preintegrated measurements. TODO Frank moved from end of this function !!!
Matrix9 F_9x9;
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9);
// Single Jacobians to propagate covariance
Matrix3 H_vel_biasacc = -dRij * deltaT;
Matrix3 H_angles_biasomega = -D_Rincr_integratedOmega * deltaT;
Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * deltaT;
// overall Jacobian wrt preintegrated measurements (df/dx)
Eigen::Matrix<double,15,15> F;

25
gtsam/navigation/ImuFactor.cpp
View File

@ -63,24 +63,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
Vector3 correctedAcc, correctedOmega;
boost::tie(correctedAcc, correctedOmega) =
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega);
// rotation increment computed from the current rotation rate measurement
const Vector3 integratedOmega = correctedOmega * deltaT;
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
// rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega);
// Update Jacobians
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr,
deltaT);
const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test
// Update preintegrated measurements (also get Jacobian)
const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test
Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F);
updatePreintegratedMeasurements(measuredAcc, measuredOmega, deltaT,
&D_incrR_integratedOmega, &F);
// first order covariance propagation:
// as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF
@ -93,8 +82,8 @@ void PreintegratedImuMeasurements::integrateMeasurement(
preintMeasCov_ = F * preintMeasCov_ * F.transpose();
D_t_t(&preintMeasCov_) += p().integrationCovariance * deltaT;
D_v_v(&preintMeasCov_) += dRij * p().accelerometerCovariance * dRij.transpose() * deltaT;
D_R_R(&preintMeasCov_) += D_Rincr_integratedOmega * p().gyroscopeCovariance
* D_Rincr_integratedOmega.transpose() * deltaT;
D_R_R(&preintMeasCov_) += D_incrR_integratedOmega * p().gyroscopeCovariance
* D_incrR_integratedOmega.transpose() * deltaT;
Matrix3 F_pos_noiseacc;
F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT;
@ -113,7 +102,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
// This in only for testing & documentation, while the actual computation is done block-wise
// omegaNoise intNoise accNoise
(*G_test) <<
D_Rincr_integratedOmega * deltaT, Z_3x3, Z_3x3, // angle
D_incrR_integratedOmega * deltaT, Z_3x3, Z_3x3, // angle
Z_3x3, I_3x3 * deltaT, F_pos_noiseacc, // pos
Z_3x3, Z_3x3, dRij * deltaT; // vel
}

39
gtsam/navigation/PreintegratedRotation.h
View File

@ -108,21 +108,34 @@ class PreintegratedRotation {
/// @}
/// Update preintegrated measurements
void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
OptionalJacobian<3, 3> H = boost::none) {
deltaRij_ = deltaRij_.compose(incrR, H, boost::none);
deltaTij_ += deltaT;
}
void integrateMeasurement(const Vector3& measuredOmega,
const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
Matrix3* Fr) {
/**
* Update Jacobians to be used during preintegration
* TODO: explain arguments
*/
void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega,
const Rot3& incrR, double deltaT) {
// First we compensate the measurements for the bias
Vector3 correctedOmega = measuredOmega - biasHat;
// Then compensate for sensor-body displacement: 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();
// rotation rate vector in the body frame
correctedOmega = body_R_sensor * correctedOmega;
}
// rotation vector describing rotation increment computed from the
// current rotation rate measurement
const Vector3 theta_incr = correctedOmega * deltaT;
const Rot3 incrR = Rot3::Expmap(theta_incr, D_incrR_integratedOmega); // expensive !!
// Update deltaTij and rotation
deltaTij_ += deltaT;
deltaRij_ = deltaRij_.compose(incrR, Fr);
// Update Jacobian
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT;
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- *D_incrR_integratedOmega * deltaT;
}
/// Return a bias corrected version of the integrated rotation, with optional Jacobian

54
gtsam/navigation/PreintegrationBase.cpp
View File

@ -60,40 +60,46 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
/// Update preintegrated measurements
void PreintegrationBase::updatePreintegratedMeasurements(
const Vector3& correctedAcc, const Rot3& incrR, const double deltaT,
OptionalJacobian<9, 9> F) {
const Vector3& measuredAcc, const Vector3& measuredOmega,
const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* F) {
Matrix3 D_Rij_incrR;
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
// (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
Vector3 correctedAcc, correctedOmega;
boost::tie(correctedAcc, correctedOmega) =
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega);
// rotation vector describing rotation increment computed from the current rotation rate measurement
const Vector3 integratedOmega = correctedOmega * deltaT;
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // rotation increment computed from the current rotation rate measurement
// Calculate acceleration in *current* i frame, i.e., before rotation update below
Matrix3 D_acc_R;
const Vector3 i_acc = deltaRij_.rotate(correctedAcc, F? &D_acc_R : 0);
Matrix3 F_angles_angles;
updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0);
const Matrix3 dRij = deltaRij_.matrix(); // expensive
const Vector3 i_acc = deltaRij_.rotate(correctedAcc, F ? &D_acc_R : 0);
double dt22 = 0.5 * deltaT * deltaT;
deltaTij_ += deltaT;
deltaRij_ = deltaRij_.compose(incrR, F ? &D_Rij_incrR : 0);
deltaPij_ += dt22 * i_acc + deltaT * deltaVij_;
deltaVij_ += deltaT * i_acc;
if (F) {
*F << // angle pos vel
F_angles_angles, Z_3x3, Z_3x3, // angle
dt22 * D_acc_R, I_3x3, I_3x3 * deltaT, // pos
deltaT * D_acc_R, Z_3x3, I_3x3; // vel
}
}
*F << // angle pos vel
D_Rij_incrR, Z_3x3, Z_3x3, // angle
dt22 * D_acc_R, I_3x3, I_3x3 * deltaT, // pos
deltaT * D_acc_R, Z_3x3, I_3x3; // vel
/// Update Jacobians to be used during preintegration
void PreintegrationBase::updatePreintegratedJacobians(
const Vector3& correctedAcc, const Matrix3& D_Rincr_integratedOmega,
const Rot3& incrR, double deltaT) {
const Matrix3 dRij = deltaRij_.matrix(); // expensive
const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT
* delRdelBiasOmega_;
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT;
delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5);
const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * delRdelBiasOmega_;
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - dt22 * dRij;
delPdelBiasOmega_ += deltaT * delVdelBiasOmega_ + dt22 * temp;
delVdelBiasAcc_ += -dRij * deltaT;
delVdelBiasOmega_ += temp;
update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
delVdelBiasOmega_ += temp * deltaT;
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- *D_incrR_integratedOmega * deltaT;
}
std::pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(

9
gtsam/navigation/PreintegrationBase.h
View File

@ -170,12 +170,9 @@ public:
bool equals(const PreintegrationBase& other, double tol) const;
/// Update preintegrated measurements
void updatePreintegratedMeasurements(const Vector3& correctedAcc,
const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F);
/// Update Jacobians to be used during preintegration
void updatePreintegratedJacobians(const Vector3& correctedAcc,
const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT);
void updatePreintegratedMeasurements(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double deltaT,
Matrix3* D_incrR_integratedOmega, Matrix9* F);
std::pair<Vector3, Vector3>
correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,