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

using fixed-size matrix/vector when possible

release/4.3a0
Luca 2014-12-11 11:28:43 -05:00
parent c4fafd9268
commit af04b834b9
4 changed files with 15 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -94,7 +94,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
/* ----------------------------------------------------------------------------------------------------------------------- */
const Matrix3 R_i = deltaRij(); // store this
// Update preintegrated measurements. TODO Frank moved from end of this function !!!
Matrix F_9x9;
Matrix9 F_9x9;
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9);
// Single Jacobians to propagate covariance

4
gtsam/navigation/ImuFactor.cpp
View File

@ -82,7 +82,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// Update preintegrated measurements (also get Jacobian)
const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test
Matrix F; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F);
// first order covariance propagation:
@ -96,13 +96,11 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// F_test and G_test are used for testing purposes and are not needed by the factor
if(F_test){
// This in only for testing
F_test->resize(9,9);
(*F_test) << F;
}
if(G_test){
// Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
// This in only for testing & documentation
G_test->resize(9,9);
// intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos
Z_3x3, R_i * deltaT, Z_3x3, // vel

2
gtsam/navigation/PreintegratedRotation.h
View File

@ -97,7 +97,7 @@ public:
/// Get so<3> version of bias corrected rotation, with optional Jacobian
Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr,
boost::optional<Matrix&> H) const {
OptionalJacobian<3, 3> H = boost::none) const {
// First, we correct deltaRij using the biasOmegaIncr, rotated
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);

19
gtsam/navigation/PreintegrationBase.h
View File

@ -77,7 +77,7 @@ public:
const Vector3& deltaPij() const {return deltaPij_;}
const Vector3& deltaVij() const {return deltaVij_;}
const imuBias::ConstantBias& biasHat() const { return biasHat_;}
Vector biasHatVector() const { return biasHat_.vector();} // expensive
Vector6 biasHatVector() const { return biasHat_.vector();} // expensive
const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;}
const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;}
const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;}
@ -228,13 +228,15 @@ public:
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
//------------------------------------------------------------------------------
Vector computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
boost::optional<Matrix&> H5) const {
OptionalJacobian<9, 6> H1 = boost::none,
OptionalJacobian<9, 3> H2 = boost::none,
OptionalJacobian<9, 6> H3 = boost::none,
OptionalJacobian<9, 3> H4 = boost::none,
OptionalJacobian<9, 6> H5 = boost::none) const {
// We need the mismatch w.r.t. the biases used for preintegration
// const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary
@ -264,7 +266,8 @@ 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 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5);
Matrix3 H5temp;
Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &H5temp : 0);
// Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis);
@ -348,7 +351,7 @@ public:
}
if(H5) {
// H5 by this point already contains 3*3 biascorrectedThetaRij derivative
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H5);
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * H5temp;
H5->resize(9,6);
(*H5) <<
// dfP/dBias
@ -358,7 +361,7 @@ public:
// dfR/dBias
Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega);
}
Vector r(9); r << fp, fv, fR;
Vector9 r; r << fp, fv, fR;
return r;
}