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

Refactored arm correction but there is still a difference in regression values. Did I introduce a bug?

release/4.3a0
dellaert 2015-08-04 08:31:47 -07:00
parent 3ae998d31d
commit 9af69254b2
3 changed files with 36 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/navigation/ImuFactor.cpp
View File

@ -103,7 +103,8 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
const Pose3& body_P_sensor) {
p_->body_P_sensor = body_P_sensor;
// modify parameters to accommodate deprecated method:-(
p_->body_P_sensor.reset(body_P_sensor);
integrateMeasurement(measuredAcc, measuredOmega, deltaT);
}

40
gtsam/navigation/PreintegrationBase.cpp
View File

@ -61,9 +61,8 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
}
//------------------------------------------------------------------------------
NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
const Vector3& j_measuredOmega, const double dt, OptionalJacobian<9, 9> F,
OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const {
std::pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega) const {
// Correct for bias in the sensor frame
Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
@ -73,17 +72,34 @@ NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
// (originally in the IMU frame) into the body frame
// Equations below assume the "body" frame is the CG
if (p().body_P_sensor) {
// Correct omega: slight duplication as this is also done in integrateMeasurement below
Matrix3 bRs = p().body_P_sensor->rotation().matrix();
j_correctedOmega = bRs * j_correctedOmega; // rotation rate vector in the body frame
// Correct omega to rotation rate vector in the body frame
const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
j_correctedOmega = bRs * j_correctedOmega;
// Correct acceleration
Vector3 b_arm = p().body_P_sensor->translation().vector();
Vector3 b_velocity_bs = j_correctedOmega.cross(b_arm); // magnitude: omega * arm
j_correctedAcc = bRs * j_correctedAcc;
const Vector3 b_arm = p().body_P_sensor->translation().vector();
if (!b_arm.isZero()) {
// Subtract out the the centripetal acceleration from the measured one
// to get linear acceleration vector in the body frame:
j_correctedAcc = bRs * j_correctedAcc - j_correctedOmega.cross(b_velocity_bs);
const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega);
const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
j_correctedAcc -= body_Omega_body * b_velocity_bs;
}
}
// Do update in one fell swoop
return make_pair(j_correctedAcc, j_correctedOmega);
}
//------------------------------------------------------------------------------
NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
const Vector3& j_measuredOmega, const double dt, OptionalJacobian<9, 9> F,
OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const {
Vector3 j_correctedAcc, j_correctedOmega;
boost::tie(j_correctedAcc, j_correctedOmega) =
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
// Do update in one fell swoop
return deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, F, G1, G2);
@ -103,9 +119,9 @@ void PreintegrationBase::update(
// Update Jacobians
// TODO(frank): we are repeating some computation here: accessible in F ?
// Correct for bias in the sensor frame
Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega);
Vector3 j_correctedAcc, j_correctedOmega;
boost::tie(j_correctedAcc, j_correctedOmega) =
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
Matrix3 D_acc_R;
oldRij.rotate(j_correctedAcc, D_acc_R);

4
gtsam/navigation/PreintegrationBase.h
View File

@ -191,6 +191,10 @@ public:
/// check equality
bool equals(const PreintegrationBase& other, double tol) const;
/// Subtract estimate and correct for sensor pose
std::pair<Vector3, Vector3> correctMeasurementsByBiasAndSensorPose(
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega) const;
/// Calculate the updated preintegrated measurement, does not modify
/// It takes measured quantities in the j frame
NavState updatedDeltaXij(const Vector3& j_measuredAcc,