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

10
gtsam/navigation/CombinedImuFactor.cpp
View File

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

10
gtsam/navigation/ImuFactor.cpp
View File

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

8
gtsam/navigation/PreintegratedRotation.h
View File

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

44
gtsam/navigation/PreintegrationBase.h
View File

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