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

getting rid of rightJacobianSO3 (not completed yet)

release/4.3a0
Luca 2014-12-08 13:15:51 -05:00
parent 422db08c69
commit f991c1a398
3 changed files with 31 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/navigation/ImuFactor.cpp
View File

@ -77,8 +77,8 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
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 Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
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
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
@ -87,15 +87,18 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// 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 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 R_i = deltaRij();
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
const Matrix3 R_i = deltaRij();
// Update preintegrated measurements
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT);
const Vector3 theta_j = thetaRij(); // super-expensive parametrization of so(3)
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
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;

15
gtsam/navigation/PreintegratedRotation.h
View File

@ -49,7 +49,7 @@ public:
/// methods to access class variables
Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive
Vector3 thetaRij() const {return Rot3::Logmap(deltaRij_);} // super-expensive
Vector3 thetaRij(boost::optional<Matrix3&> H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive
const double& deltaTij() const{return deltaTij_;}
const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;}
@ -99,17 +99,18 @@ public:
boost::optional<Matrix&> H) const {
// First, we correct deltaRij using the biasOmegaIncr, rotated
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
// This was done via an expmap, now we go *back* to so<3>
const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
if (H) {
// We then do a very expensive Jacobian calculation. TODO Right Duy ?
const Matrix3 Jrinv_theta_bc = //
Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
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 Matrix3 Jr_JbiasOmegaIncr = //
Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
return theta_biascorrected;
}
return theta_biascorrected;
//else
return Rot3::Logmap(deltaRij_biascorrected);
}
/// Integrate coriolis correction in body frame rot_i

25
gtsam/navigation/PreintegrationBase.h
View File

@ -239,18 +239,24 @@ public:
const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
Vector3 theta_corrected = theta_biascorrected - coriolis;
// Prediction
const Rot3 deltaRij_corrected = Rot3::Expmap( theta_corrected );
// Residual rotation error
const Rot3 fRhat = deltaRij_corrected.between(Ri.between(Rj));
Rot3 deltaRij_corrected, fRhat;
Vector3 fR;
// Accessory matrix, used to build the jacobians
Matrix3 Jr_theta_bcc, Jtheta, Jrinv_fRhat;
if(H1 || H3 || H5){
Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected);
// 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);
// Residual rotation error
fRhat = deltaRij_corrected.between(Ri.between(Rj));
fR = Rot3::Logmap(fRhat, Jrinv_fRhat);
Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis);
}else{
deltaRij_corrected = Rot3::Expmap( theta_corrected);
// Residual rotation error
fRhat = deltaRij_corrected.between(Ri.between(Rj));
fR = Rot3::Logmap(fRhat);
}
if(H1) {
@ -339,8 +345,7 @@ public:
const Vector3 fv = vel_j - predictedState_j.velocity;
// This is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j)
const Vector3 fR = Rot3::Logmap(fRhat);
// fR was computes earlier. Note: it is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j)
Vector r(9); r << fp, fv, fR;
return r;