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

Fixed compilation errors

release/4.3a0
dellaert 2015-07-25 10:02:38 +02:00
parent 7407b605b9
commit 0310eb4e7b
2 changed files with 11 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/navigation/PreintegrationBase.cpp
View File

@ -126,9 +126,10 @@ void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAc
update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
} }
void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( std::pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3& correctedAcc, const Vector3& measuredAcc, const Vector3& measuredOmega,
Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor) { boost::optional<const Pose3&> body_P_sensor) const {
Vector3 correctedAcc, correctedOmega;
correctedAcc = biasHat_.correctAccelerometer(measuredAcc); correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
correctedOmega = biasHat_.correctGyroscope(measuredOmega); correctedOmega = biasHat_.correctGyroscope(measuredOmega);
@ -136,12 +137,14 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
// (originally in the IMU frame) into the body frame // (originally in the IMU frame) into the body frame
if (body_P_sensor) { if (body_P_sensor) {
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
correctedAcc = body_R_sensor * correctedAcc correctedAcc = body_R_sensor * correctedAcc
- body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - 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 std::make_pair(correctedAcc, correctedOmega);
} }
/// Predict state at time j /// Predict state at time j

20
gtsam/navigation/PreintegrationBase.h
View File

@ -133,24 +133,10 @@ class PreintegrationBase : public PreintegratedRotation {
const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
double deltaT); double deltaT);
boost::tuple<Vector3, Vector3> std::pair<Vector3, Vector3>
correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
const Vector3& measuredOmega, boost::optional<const Pose3&> body_P_sensor) { const Vector3& measuredOmega,
Vector3 correctedAcc, correctedOmega; boost::optional<const Pose3&> body_P_sensor) const;
correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
correctedOmega = biasHat_.correctGyroscope(measuredOmega);
// Then compensate for sensor-body displacement: we express the quantities
// (originally in the IMU frame) into the body frame
if(body_P_sensor){
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
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 /// Predict state at time j
PoseVelocityBias predict( PoseVelocityBias predict(