added comments, made more elegant error evaluation for CombinedImuFactor
Luca
parent
30810e2917
commit
c1d63b77ff
|
|
@ -206,21 +206,17 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
|
|||
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
|
||||
boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {
|
||||
|
||||
// Bias evolution model: random walk
|
||||
const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer();
|
||||
const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope();
|
||||
|
||||
// error wrt preintegrated position, velocity, rotation
|
||||
Vector r_pvR(9);
|
||||
|
||||
// if we need the jacobians
|
||||
if(H1 || H2 || H3 || H4 || H5 || H6){
|
||||
Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR; // pvR = mnemonic: position (p), velocity (v), rotation (R)
|
||||
Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R)
|
||||
|
||||
// include errors wrt preintegrated measurements
|
||||
r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i,
|
||||
// error wrt preintegrated measurements
|
||||
Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i,
|
||||
H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR);
|
||||
|
||||
// error wrt bias evolution model (random walk)
|
||||
Vector6 fbias = bias_j.between(bias_i, Hbias_j, Hbias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr]
|
||||
|
||||
if(H1) {
|
||||
H1->resize(15,6);
|
||||
H1->block<9,6>(0,0) = H1_pvR;
|
||||
|
|
@ -249,22 +245,25 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
|
|||
H5->resize(15,6);
|
||||
H5->block<9,6>(0,0) = H5_pvR;
|
||||
// adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i]
|
||||
H5->block<6,6>(0,9) = - Matrix::Identity(6,6);
|
||||
H5->block<6,6>(0,9) = Hbias_i;
|
||||
}
|
||||
if(H6) {
|
||||
H6->resize(15,6);
|
||||
H6->block<9,6>(0,0) = Matrix::Zero(6,6);
|
||||
// adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
|
||||
H6->block<6,6>(0,9) = Matrix::Identity(6,6);
|
||||
H6->block<6,6>(0,9) = Hbias_j;
|
||||
}
|
||||
Vector r(15); r << r_pvR, fbiasAcc, fbiasOmega; // vector of size 15
|
||||
Vector r(15); r << r_pvR, fbias; // vector of size 15
|
||||
return r;
|
||||
}
|
||||
// else, only compute the error vector:
|
||||
// Evaluate residual error, including the model for bias evolution
|
||||
r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i,
|
||||
// error wrt preintegrated measurements
|
||||
Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i,
|
||||
boost::none, boost::none, boost::none, boost::none, boost::none);
|
||||
Vector r(15); r << r_pvR, fbiasAcc, fbiasOmega; // vector of size 15
|
||||
// error wrt bias evolution model (random walk)
|
||||
Vector6 fbias = bias_j.between(bias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr]
|
||||
// overall error
|
||||
Vector r(15); r << r_pvR, fbias; // vector of size 15
|
||||
return r;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -52,15 +52,24 @@ protected:
|
|||
|
||||
public:
|
||||
|
||||
/** Default constructor - only use for serialization */
|
||||
ImuFactorBase() :
|
||||
gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)),
|
||||
body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {}
|
||||
|
||||
/**
|
||||
* Default constructor, stores basic quantities required by the Imu factors
|
||||
* @param gravity Gravity vector expressed in the global frame
|
||||
* @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
|
||||
* @param body_P_sensor Optional pose of the sensor frame in the body frame
|
||||
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
|
||||
*/
|
||||
ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) :
|
||||
gravity_(gravity), omegaCoriolis_(omegaCoriolis),
|
||||
body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {}
|
||||
|
||||
/// Methods to access class variables
|
||||
const Vector3& gravity() const { return gravity_; }
|
||||
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
|
||||
|
||||
|
|
@ -91,6 +100,7 @@ public:
|
|||
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const{
|
||||
|
||||
const double& deltaTij = preintegratedMeasurements_.deltaTij_;
|
||||
// We need the mistmatch w.r.t. the biases used for preintegration
|
||||
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
|
||||
|
||||
|
|
@ -100,7 +110,7 @@ public:
|
|||
const Vector3 pos_i = pose_i.translation().vector();
|
||||
const Vector3 pos_j = pose_j.translation().vector();
|
||||
|
||||
// We compute factor's Jacobians
|
||||
// Jacobian computation
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
|
|
@ -114,11 +124,8 @@ public:
|
|||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
|
||||
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
|
||||
|
||||
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
|
||||
|
||||
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
|
||||
|
||||
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
|
||||
|
||||
if(H1) {
|
||||
|
|
@ -134,7 +141,6 @@ public:
|
|||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Z_3x3;
|
||||
}
|
||||
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
|
||||
|
|
@ -151,7 +157,6 @@ public:
|
|||
// dfR/dPi
|
||||
Z_3x3;
|
||||
}
|
||||
|
||||
if(H2) {
|
||||
H2->resize(9,3);
|
||||
(*H2) <<
|
||||
|
|
@ -164,7 +169,6 @@ public:
|
|||
// dfR/dVi
|
||||
Z_3x3;
|
||||
}
|
||||
|
||||
if(H3) {
|
||||
H3->resize(9,6);
|
||||
(*H3) <<
|
||||
|
|
@ -175,7 +179,6 @@ public:
|
|||
// dfR/dPosej
|
||||
Jrinv_fRhat * ( I_3x3 ), Z_3x3;
|
||||
}
|
||||
|
||||
if(H4) {
|
||||
H4->resize(9,3);
|
||||
(*H4) <<
|
||||
|
|
@ -186,7 +189,6 @@ public:
|
|||
// dfR/dVj
|
||||
Z_3x3;
|
||||
}
|
||||
|
||||
if(H5) {
|
||||
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
|
||||
|
|
|
|||
Loading…
Reference in New Issue