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

included methods in the base class to reduce redundancy between ImuFactor and CombinedImuFactor

release/4.3a0
Luca 2014-12-03 18:58:20 -05:00
parent c4b62929bf
commit 218af7c889
3 changed files with 50 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -76,18 +76,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
// (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).
// First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty Vector3 correctedAcc, correctedOmega;
Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);
Vector3 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
}
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
@ -95,17 +85,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// Update Jacobians // Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){ updatePreintegratedJacobians(correctedAcc, Jr_theta_incr, Rincr, deltaT);
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
}else{
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
* skewSymmetric(correctedAcc) * deltaT*deltaT * delRdelBiasOmega_;
}
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
// can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
@ -173,14 +153,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// Update preintegrated measurements // Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){ updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT);
deltaPij_ += deltaVij_ * deltaT;
}else{
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * correctedAcc * deltaT*deltaT;
}
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
deltaRij_ = deltaRij_ * Rincr;
deltaTij_ += deltaT;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------

35
gtsam/navigation/ImuFactor.cpp
View File

@ -72,18 +72,8 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// NOTE: order is important here because each update uses old values (i.e., we have to update // NOTE: order is important here because each update uses old values (i.e., we have to update
// jacobians and covariances before updating preintegrated measurements). // jacobians and covariances before updating preintegrated measurements).
// First we compensate the measurements for the bias Vector3 correctedAcc, correctedOmega;
Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);
Vector3 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
}
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
@ -91,17 +81,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// Update Jacobians // Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){ updatePreintegratedJacobians(correctedAcc, Jr_theta_incr, Rincr, deltaT);
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
}else{
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
* skewSymmetric(correctedAcc) * deltaT*deltaT * delRdelBiasOmega_;
}
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
// Update preintegrated measurements covariance // Update preintegrated measurements covariance
// as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
@ -153,14 +133,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// Update preintegrated measurements (this has to be done after the update of covariances and jacobians!) // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!)
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){ updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT);
deltaPij_ += deltaVij_ * deltaT;
}else{
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * correctedAcc * deltaT*deltaT;
}
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
deltaRij_ = deltaRij_ * Rincr;
deltaTij_ += deltaT;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------

42
gtsam/navigation/PreintegrationBase.h
View File

@ -105,6 +105,48 @@ public:
delRdelBiasOmega_ = Z_3x3; delRdelBiasOmega_ = Z_3x3;
} }
/// Update preintegrated measurements
void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& Rincr, double deltaT){
if(!use2ndOrderIntegration_){
deltaPij_ += deltaVij_ * deltaT;
}else{
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * correctedAcc * deltaT*deltaT;
}
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
deltaRij_ = deltaRij_ * Rincr;
deltaTij_ += deltaT;
}
/// Update Jacobians to be used during preintegration
void updatePreintegratedJacobians(const Vector3& correctedAcc, const Matrix3& Jr_theta_incr, const Rot3& Rincr, double deltaT){
if(!use2ndOrderIntegration_){
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
}else{
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
* skewSymmetric(correctedAcc) * deltaT*deltaT * delRdelBiasOmega_;
}
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
}
void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, const Vector3& measuredOmega,
Vector3& correctedAcc, Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor){
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
}
}
/// methods to access class variables /// methods to access class variables
Matrix deltaRij() const {return deltaRij_.matrix();} Matrix deltaRij() const {return deltaRij_.matrix();}
double deltaTij() const{return deltaTij_;} double deltaTij() const{return deltaTij_;}