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

applied (to some extend) the naming convention proposed by Frank

release/4.3a0
Luca 2014-12-09 16:59:30 -05:00
parent b593a6a2d5
commit 1e8402231c
5 changed files with 55 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
gtsam/navigation/AHRSFactor.cpp
View File

@ -83,11 +83,11 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
// rotation vector describing rotation increment computed from the
// current rotation rate measurement
const Vector3 theta_incr = correctedOmega * deltaT;
Matrix3 Jr_theta_incr;
const Rot3 incrR = Rot3::Expmap(theta_incr, Jr_theta_incr); // expensive !!
Matrix3 D_Rincr_integratedOmega;
const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !!
// Update Jacobian
update_delRdelBiasOmega(Jr_theta_incr, incrR, deltaT);
update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
// Update rotation and deltaTij.
Matrix3 Fr; // Jacobian of the update
@ -172,44 +172,44 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3) const {
// Do bias correction, if (H3) will contain 3*3 derivative used below
const Vector3 theta_biascorrected = _PIM_.predict(bias, H3);
const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3);
// Coriolis term
const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_);
const Matrix3 coriolisHat = skewSymmetric(coriolis);
const Vector3 theta_corrected = theta_biascorrected - coriolis;
const Vector3 correctedOmega = biascorrectedOmega - coriolis;
// Prediction
const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected);
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
// Get error between actual and prediction
const Rot3 actualRij = Ri.between(Rj);
const Rot3 fRhat = deltaRij_corrected.between(actualRij);
Vector3 fR = Rot3::Logmap(fRhat);
const Rot3 fRrot = correctedDeltaRij.between(actualRij);
Vector3 fR = Rot3::Logmap(fRrot);
// Terms common to derivatives
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected);
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(fR);
const Matrix3 D_cDeltaRij_cOmega = Rot3::rightJacobianExpMapSO3(correctedOmega);
const Matrix3 D_fR_fRrot = Rot3::rightJacobianExpMapSO3inverse(fR);
if (H1) {
// dfR/dRi
H1->resize(3, 3);
Matrix3 Jtheta = -Jr_theta_bcc * coriolisHat;
Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat;
(*H1)
<< Jrinv_fRhat * (-actualRij.transpose() - fRhat.transpose() * Jtheta);
<< D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis);
}
if (H2) {
// dfR/dPosej
H2->resize(3, 3);
(*H2) << Jrinv_fRhat * Matrix3::Identity();
(*H2) << D_fR_fRrot * Matrix3::Identity();
}
if (H3) {
// dfR/dBias, note H3 contains derivative of predict
const Matrix3 JbiasOmega = Jr_theta_bcc * (*H3);
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H3);
H3->resize(3, 3);
(*H3) << Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega);
(*H3) << D_fR_fRrot * (-fRrot.transpose() * JbiasOmega);
}
Vector error(3);
@ -222,16 +222,16 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) {
const Vector3 theta_biascorrected = preintegratedMeasurements.predict(bias);
const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias);
// Coriolis term
const Vector3 coriolis = //
preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis);
const Vector3 theta_corrected = theta_biascorrected - coriolis;
const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected);
const Vector3 correctedOmega = biascorrectedOmega - coriolis;
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
return rot_i.compose(deltaRij_corrected);
return rot_i.compose(correctedDeltaRij);
}
} //namespace gtsam

10
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -80,13 +80,13 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
Matrix3 Jr_theta_incr; // Right jacobian computed at theta_incr
const Rot3 Rincr = Rot3::Expmap(theta_incr, Jr_theta_incr); // rotation increment computed from the current rotation rate measurement
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, Jr_theta_incr, Rincr, deltaT);
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
// 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
@ -99,7 +99,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// Single Jacobians to propagate covariance
Matrix3 H_vel_biasacc = - R_i * deltaT;
Matrix3 H_angles_biasomega =- Jr_theta_incr * deltaT;
Matrix3 H_angles_biasomega =- D_Rincr_integratedOmega * deltaT;
// overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(15,15);

10
gtsam/navigation/ImuFactor.cpp
View File

@ -73,12 +73,12 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
Matrix3 Jr_theta_incr; // Right jacobian computed at theta_incr
const Rot3 Rincr = Rot3::Expmap(theta_incr, Jr_theta_incr); // rotation increment computed from the current rotation rate measurement
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, Jr_theta_incr, Rincr, deltaT);
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
// Update preintegrated measurements (also get Jacobian)
const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test
@ -106,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, Jr_theta_incr * deltaT; // angle
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
// Propagation with no approximation:
// preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose();
}

8
gtsam/navigation/PreintegratedRotation.h
View File

@ -84,10 +84,10 @@ public:
* Update Jacobians to be used during preintegration
* TODO: explain arguments
*/
void update_delRdelBiasOmega(const Matrix3& Jr_theta_incr, const Rot3& incrR,
void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
double deltaT) {
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT;
}
/// Return a bias corrected version of the integrated rotation - expensive
@ -104,11 +104,11 @@ public:
if (H) {
Matrix3 Jrinv_theta_bc;
// This was done via an expmap, now we go *back* to so<3>
const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc);
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc);
const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
return theta_biascorrected;
return biascorrectedOmega;
}
//else
return Rot3::Logmap(deltaRij_biascorrected);

44
gtsam/navigation/PreintegrationBase.h
View File

@ -144,7 +144,7 @@ public:
/// Update Jacobians to be used during preintegration
void updatePreintegratedJacobians(const Vector3& correctedAcc,
const Matrix3& Jr_theta_incr, const Rot3& incrR, double deltaT){
const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT){
Matrix3 dRij = deltaRij(); // expensive
Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega();
if (!use2ndOrderIntegration_) {
@ -156,7 +156,7 @@ public:
}
delVdelBiasAcc_ += -dRij * deltaT;
delVdelBiasOmega_ += temp;
update_delRdelBiasOmega(Jr_theta_incr,incrR,deltaT);
update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT);
}
void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
@ -211,12 +211,12 @@ public:
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
// deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_corrected = theta_biascorrected -
Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
Vector3 correctedOmega = biascorrectedOmega -
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
const Rot3 deltaRij_corrected =
Rot3::Expmap( theta_corrected );
const Rot3 Rot_j = Rot_i.compose( deltaRij_corrected );
const Rot3 correctedDeltaRij =
Rot3::Expmap( correctedOmega );
const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij );
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
@ -246,31 +246,31 @@ public:
// Get Get so<3> version of bias corrected rotation
// If H5 is asked for, we will need the Jacobian, which we store in H5
// H5 will then be corrected below to take into account the Coriolis effect
Vector3 theta_biascorrected = biascorrectedThetaRij(biasOmegaIncr, H5);
Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5);
// Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis);
const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
Vector3 theta_corrected = theta_biascorrected - coriolis;
Vector3 correctedOmega = biascorrectedOmega - coriolis;
Rot3 deltaRij_corrected, fRhat;
Rot3 correctedDeltaRij, fRrot;
Vector3 fR;
// Accessory matrix, used to build the jacobians
Matrix3 Jr_theta_bcc, Jtheta, Jrinv_fRhat;
Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot;
// This is done to save computation: we ask for the jacobians only when they are needed
if(H1 || H2 || H3 || H4 || H5){
deltaRij_corrected = Rot3::Expmap( theta_corrected, Jr_theta_bcc);
correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega);
// Residual rotation error
fRhat = deltaRij_corrected.between(Ri.between(Rj));
fR = Rot3::Logmap(fRhat, Jrinv_fRhat);
Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis);
fRrot = correctedDeltaRij.between(Ri.between(Rj));
fR = Rot3::Logmap(fRrot, D_fR_fRrot);
D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis);
}else{
deltaRij_corrected = Rot3::Expmap( theta_corrected);
correctedDeltaRij = Rot3::Expmap( correctedOmega);
// Residual rotation error
fRhat = deltaRij_corrected.between(Ri.between(Rj));
fR = Rot3::Logmap(fRhat);
fRrot = correctedDeltaRij.between(Ri.between(Rj));
fR = Rot3::Logmap(fRrot);
}
if(H1) {
@ -298,7 +298,7 @@ public:
// dfV/dPi
dfVdPi,
// dfR/dRi
Jrinv_fRhat * (- Rj.between(Ri).matrix() - fRhat.inverse().matrix() * Jtheta),
D_fR_fRrot * (- Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis),
// dfR/dPi
Z_3x3;
}
@ -322,7 +322,7 @@ public:
// dfV/dPosej
Matrix::Zero(3,6),
// dfR/dPosej
Jrinv_fRhat * ( I_3x3 ), Z_3x3;
D_fR_fRrot * ( I_3x3 ), Z_3x3;
}
if(H4) {
H4->resize(9,3);
@ -336,7 +336,7 @@ public:
}
if(H5) {
// H5 by this point already contains 3*3 biascorrectedThetaRij derivative
const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5);
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H5);
H5->resize(9,6);
(*H5) <<
// dfP/dBias
@ -347,7 +347,7 @@ public:
- Ri.matrix() * delVdelBiasOmega(),
// dfR/dBias
Matrix::Zero(3,3),
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega);
}
// Evaluate residual error, according to [3]