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

Inlined Logmap and derivatives, removed from PreintegratedRotation

release/4.3a0
dellaert 2015-07-19 00:54:25 -07:00
parent 814b8c22bf
commit 3c59168406
3 changed files with 11 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/navigation/AHRSFactor.cpp
View File

@ -83,7 +83,11 @@ void PreintegratedAhrsMeasurements::integrateMeasurement(
Vector3 PreintegratedAhrsMeasurements::predict(const Vector3& bias,
OptionalJacobian<3,3> H) const {
const Vector3 biasOmegaIncr = bias - biasHat_;
return biascorrectedThetaRij(biasOmegaIncr, H);
const Rot3 biascorrected = biascorrectedDeltaRij(biasOmegaIncr, H);
Matrix3 D_omega_biascorrected;
const Vector3 omega = Rot3::Logmap(biascorrected, H ? &D_omega_biascorrected : 0);
if (H) (*H) = D_omega_biascorrected * (*H);
return omega;
}
//------------------------------------------------------------------------------
Vector PreintegratedAhrsMeasurements::DeltaAngles(

17
gtsam/navigation/PreintegratedRotation.h
View File

@ -135,23 +135,6 @@ class PreintegratedRotation {
return deltaRij_biascorrected;
}
/// Get so<3> version of bias corrected rotation, with optional Jacobian
// Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) )
Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr,
OptionalJacobian<3, 3> H = boost::none) const {
// First, we correct deltaRij using the biasOmegaIncr, rotated
Matrix3 D_biascorrected_biasOmegaIncr;
const Rot3 biascorrected = biascorrectedDeltaRij(biasOmegaIncr,
H ? &D_biascorrected_biasOmegaIncr : 0);
// This was done via an expmap, now we go *back* to so<3>
Matrix3 D_omega_biascorrected;
const Vector3 omega = Rot3::Logmap(biascorrected, H ? &D_omega_biascorrected : 0);
if (H) (*H) = D_omega_biascorrected * D_biascorrected_biasOmegaIncr;
return omega;
}
/// Integrate coriolis correction in body frame rot_i
Vector3 integrateCoriolis(const Rot3& rot_i) const {
if (!p_->omegaCoriolis) return Vector3::Zero();

12
gtsam/navigation/PreintegrationBase.cpp
View File

@ -193,7 +193,9 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
// Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasIncr.gyroscope());
Matrix3 D_biascorrected_biasIncr;
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(
biasIncr.gyroscope(), H5 ? &D_biascorrected_biasIncr : 0);
const Vector3 deltaPij_biascorrected = biascorrectedDeltaPij(biasIncr);
const Vector3 deltaVij_biascorrected = biascorrectedDeltaVij(biasIncr);
PoseVelocityBias predictedState_j =
@ -212,10 +214,8 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
// 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
// TODO(frank): biascorrectedThetaRij calculates deltaRij_biascorrected, which we already have
Matrix3 D_cThetaRij_biasOmegaIncr;
const Vector3 biascorrectedOmega = biascorrectedThetaRij(biasIncr.gyroscope(),
H5 ? &D_cThetaRij_biasOmegaIncr : 0);
Matrix3 D_omega_biascorrected;
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, H5 ? &D_omega_biascorrected : 0);
// Coriolis term, NOTE inconsistent with AHRS, where coriolisHat is *after* integration
// TODO(frank): move derivatives to predict and do coriolis branching there
@ -272,7 +272,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
}
if (H5) {
// H5 by this point already contains 3*3 biascorrectedThetaRij derivative
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr;
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_omega_biascorrected * D_biascorrected_biasIncr;
(*H5) <<
Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega), // dfR/dBias
-delPdelBiasAcc(), -delPdelBiasOmega(), // dfP/dBias