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

Made functional correctMeasurementsByBiasAndSensorPose

release/4.3a0
krunalchande 2015-03-02 17:04:19 -05:00
parent 04cf6686b4
commit 0fafffb02e
3 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -78,7 +78,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// (i.e., we have to update jacobians and covariances before updating preintegrated measurements). // (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
Vector3 correctedAcc, correctedOmega; Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor);
const Vector3 integratedOmega = 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 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr

2
gtsam/navigation/ImuFactor.cpp
View File

@ -67,7 +67,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
Vector3 correctedAcc, correctedOmega; Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor);
const Vector3 integratedOmega = 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 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr

8
gtsam/navigation/PreintegrationBase.h
View File

@ -180,9 +180,10 @@ public:
update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT); update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT);
} }
void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, boost::tuple<Vector3, Vector3>
const Vector3& measuredOmega, Vector3& correctedAcc, correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor) { const Vector3& measuredOmega, boost::optional<const Pose3&> body_P_sensor) {
Vector3 correctedAcc, correctedOmega;
correctedAcc = biasHat_.correctAccelerometer(measuredAcc); correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
correctedOmega = biasHat_.correctGyroscope(measuredOmega); correctedOmega = biasHat_.correctGyroscope(measuredOmega);
@ -195,6 +196,7 @@ public:
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
// linear acceleration vector in the body frame // linear acceleration vector in the body frame
} }
return boost::make_tuple(correctedAcc, correctedOmega);
} }
/// Predict state at time j /// Predict state at time j