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).
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
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) {
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
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);
}
void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
const Vector3& measuredOmega, Vector3& correctedAcc,
Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor) {
boost::tuple<Vector3, Vector3>
correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
const Vector3& measuredOmega, boost::optional<const Pose3&> body_P_sensor) {
Vector3 correctedAcc, correctedOmega;
correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
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();
// linear acceleration vector in the body frame
}
return boost::make_tuple(correctedAcc, correctedOmega);
}
/// Predict state at time j