Moved two very large methods from ImuFactorBase to PreintegrationBase
dellaert
parent
8bfe4d75fb
commit
aa93475b3d
|
|
@ -209,7 +209,9 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
|
|||
Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R)
|
||||
|
||||
// error wrt preintegrated measurements
|
||||
Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i,
|
||||
Vector r_pvR(9);
|
||||
r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
|
||||
gravity_, omegaCoriolis_, use2ndOrderCoriolis_, //
|
||||
H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR);
|
||||
|
||||
// error wrt bias evolution model (random walk)
|
||||
|
|
@ -256,7 +258,9 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
|
|||
}
|
||||
// else, only compute the error vector:
|
||||
// error wrt preintegrated measurements
|
||||
Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i,
|
||||
Vector r_pvR(9);
|
||||
r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
|
||||
gravity_, omegaCoriolis_, use2ndOrderCoriolis_, //
|
||||
boost::none, boost::none, boost::none, boost::none, boost::none);
|
||||
// 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]
|
||||
|
|
|
|||
|
|
@ -212,14 +212,6 @@ public:
|
|||
boost::optional<Matrix&> H5 = boost::none,
|
||||
boost::optional<Matrix&> H6 = boost::none) const;
|
||||
|
||||
/// predicted states from IMU
|
||||
static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias_i,
|
||||
const PreintegrationBase& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){
|
||||
return ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis);
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -148,7 +148,8 @@ ImuFactor::ImuFactor(
|
|||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
boost::optional<const Pose3&> body_P_sensor,
|
||||
const bool use2ndOrderCoriolis) :
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_),
|
||||
pose_i, vel_i, pose_j, vel_j, bias),
|
||||
ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
|
||||
_PIM_(preintegratedMeasurements) {}
|
||||
|
||||
|
|
@ -180,13 +181,14 @@ bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const {
|
|||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias_i,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
|
||||
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
|
||||
boost::optional<Matrix&> H5) const{
|
||||
Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3,
|
||||
boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const {
|
||||
|
||||
return ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5);
|
||||
return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
|
||||
gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5);
|
||||
}
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -197,13 +197,6 @@ public:
|
|||
boost::optional<Matrix&> H4 = boost::none,
|
||||
boost::optional<Matrix&> H5 = boost::none) const;
|
||||
|
||||
/// predicted states from IMU
|
||||
static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias_i, const PreintegrationBase& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){
|
||||
return ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis);
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -27,20 +27,6 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Struct to hold all state variables of returned by Predict function
|
||||
*/
|
||||
struct PoseVelocityBias {
|
||||
Pose3 pose;
|
||||
Vector3 velocity;
|
||||
imuBias::ConstantBias bias;
|
||||
|
||||
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
|
||||
const imuBias::ConstantBias _bias) :
|
||||
pose(_pose), velocity(_velocity), bias(_bias) {
|
||||
}
|
||||
};
|
||||
|
||||
class ImuFactorBase {
|
||||
|
||||
protected:
|
||||
|
|
@ -93,184 +79,6 @@ public:
|
|||
(body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_)));
|
||||
}
|
||||
|
||||
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
|
||||
//------------------------------------------------------------------------------
|
||||
Vector computeErrorAndJacobians(const PreintegrationBase& _PIM, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
|
||||
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const{
|
||||
|
||||
const double& deltaTij = _PIM.deltaTij();
|
||||
// We need the mistmatch w.r.t. the biases used for preintegration
|
||||
const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.biasHat().gyroscope();
|
||||
|
||||
// we give some shorter name to rotations and translations
|
||||
const Rot3& Rot_i = pose_i.rotation();
|
||||
const Rot3& Rot_j = pose_j.rotation();
|
||||
const Vector3& pos_j = pose_j.translation().vector();
|
||||
|
||||
// Jacobian computation
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
// Get Get so<3> version of bias corrected rotation
|
||||
// If H5 is asked for, we will need the Jacobian, which we store in H5
|
||||
// H5 will then be corrected below to take into account the Coriolis effect
|
||||
Vector3 theta_biascorrected =
|
||||
_PIM.biascorrectedThetaRij(biasOmegaIncr, H5);
|
||||
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
|
||||
|
||||
const Rot3 deltaRij_biascorrected_corioliscorrected =
|
||||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
|
||||
// TODO: these are not always needed
|
||||
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) {
|
||||
H1->resize(9,6);
|
||||
|
||||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis_){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Z_3x3;
|
||||
}
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(_PIM.deltaPij()
|
||||
+ _PIM.delPdelBiasOmega() * biasOmegaIncr + _PIM.delPdelBiasAcc() * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(_PIM.deltaVij()
|
||||
+ _PIM.delVdelBiasOmega() * biasOmegaIncr + _PIM.delVdelBiasAcc() * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Z_3x3;
|
||||
}
|
||||
if(H2) {
|
||||
H2->resize(9,3);
|
||||
(*H2) <<
|
||||
// dfP/dVi
|
||||
- I_3x3 * deltaTij
|
||||
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
// dfV/dVi
|
||||
- I_3x3
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
|
||||
// dfR/dVi
|
||||
Z_3x3;
|
||||
}
|
||||
if(H3) {
|
||||
H3->resize(9,6);
|
||||
(*H3) <<
|
||||
// dfP/dPosej
|
||||
Z_3x3, Rot_j.matrix(),
|
||||
// dfV/dPosej
|
||||
Matrix::Zero(3,6),
|
||||
// dfR/dPosej
|
||||
Jrinv_fRhat * ( I_3x3 ), Z_3x3;
|
||||
}
|
||||
if(H4) {
|
||||
H4->resize(9,3);
|
||||
(*H4) <<
|
||||
// dfP/dVj
|
||||
Z_3x3,
|
||||
// dfV/dVj
|
||||
I_3x3,
|
||||
// dfR/dVj
|
||||
Z_3x3;
|
||||
}
|
||||
if(H5) {
|
||||
// H5 by this point already contains 3*3 biascorrectedThetaRij derivative
|
||||
const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5);
|
||||
H5->resize(9,6);
|
||||
(*H5) <<
|
||||
// dfP/dBias
|
||||
- Rot_i.matrix() * _PIM.delPdelBiasAcc(),
|
||||
- Rot_i.matrix() * _PIM.delPdelBiasOmega(),
|
||||
// dfV/dBias
|
||||
- Rot_i.matrix() * _PIM.delVdelBiasAcc(),
|
||||
- Rot_i.matrix() * _PIM.delVdelBiasOmega(),
|
||||
// dfR/dBias
|
||||
Matrix::Zero(3,3),
|
||||
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
|
||||
}
|
||||
|
||||
// Evaluate residual error, according to [3]
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
PoseVelocityBias predictedState_j = ImuFactorBase::predict(pose_i, vel_i, bias_i, _PIM,
|
||||
gravity_, omegaCoriolis_, use2ndOrderCoriolis_);
|
||||
|
||||
const Vector3 fp = pos_j - predictedState_j.pose.translation().vector();
|
||||
|
||||
const Vector3 fv = vel_j - predictedState_j.velocity;
|
||||
|
||||
// This is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j)
|
||||
const Vector3 fR = Rot3::Logmap(fRhat);
|
||||
|
||||
Vector r(9); r << fp, fv, fR;
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Predict state at time j
|
||||
//------------------------------------------------------------------------------
|
||||
static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias_i,
|
||||
const PreintegrationBase& _PIM,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
|
||||
|
||||
const double& deltaTij = _PIM.deltaTij();
|
||||
const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.biasHat().gyroscope();
|
||||
|
||||
const Rot3& Rot_i = pose_i.rotation();
|
||||
const Vector3& pos_i = pose_i.translation().vector();
|
||||
|
||||
// Predict state at time j
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (_PIM.deltaPij()
|
||||
+ _PIM.delPdelBiasAcc() * biasAccIncr
|
||||
+ _PIM.delPdelBiasOmega() * biasOmegaIncr)
|
||||
+ vel_i * deltaTij
|
||||
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ 0.5 * gravity * deltaTij*deltaTij;
|
||||
|
||||
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (_PIM.deltaVij()
|
||||
+ _PIM.delVdelBiasAcc() * biasAccIncr
|
||||
+ _PIM.delVdelBiasOmega() * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
const Rot3 deltaRij_biascorrected = _PIM.biascorrectedDeltaRij(biasOmegaIncr);
|
||||
// TODO Frank says comment below does not reflect what was in code
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
|
||||
const Rot3 deltaRij_biascorrected_corioliscorrected =
|
||||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
|
||||
|
||||
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
|
||||
return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -26,6 +26,20 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Struct to hold all state variables of returned by Predict function
|
||||
*/
|
||||
struct PoseVelocityBias {
|
||||
Pose3 pose;
|
||||
Vector3 velocity;
|
||||
imuBias::ConstantBias bias;
|
||||
|
||||
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
|
||||
const imuBias::ConstantBias _bias) :
|
||||
pose(_pose), velocity(_velocity), bias(_bias) {
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* PreintegrationBase is the base class for PreintegratedMeasurements
|
||||
* (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor).
|
||||
|
|
@ -150,6 +164,185 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
/// Predict state at time j
|
||||
//------------------------------------------------------------------------------
|
||||
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias_i, const Vector3& gravity,
|
||||
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const {
|
||||
|
||||
const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
|
||||
|
||||
const Rot3& Rot_i = pose_i.rotation();
|
||||
const Vector3& pos_i = pose_i.translation().vector();
|
||||
|
||||
// Predict state at time j
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (deltaPij()
|
||||
+ delPdelBiasAcc() * biasAccIncr
|
||||
+ delPdelBiasOmega() * biasOmegaIncr)
|
||||
+ vel_i * deltaTij()
|
||||
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ 0.5 * gravity * deltaTij()*deltaTij();
|
||||
|
||||
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij()
|
||||
+ delVdelBiasAcc() * biasAccIncr
|
||||
+ delVdelBiasOmega() * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij() // Coriolis term
|
||||
+ gravity * deltaTij());
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij()*deltaTij(); // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij(); // 2nd order term for velocity
|
||||
}
|
||||
|
||||
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
|
||||
// TODO Frank says comment below does not reflect what was in code
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
|
||||
const Rot3 deltaRij_biascorrected_corioliscorrected =
|
||||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
|
||||
|
||||
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
|
||||
return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
|
||||
}
|
||||
|
||||
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
|
||||
//------------------------------------------------------------------------------
|
||||
Vector computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias_i, const Vector3& gravity,
|
||||
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
|
||||
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
|
||||
boost::optional<Matrix&> H5) const {
|
||||
|
||||
// We need the mistmatch w.r.t. the biases used for preintegration
|
||||
const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
|
||||
|
||||
// we give some shorter name to rotations and translations
|
||||
const Rot3& Rot_i = pose_i.rotation();
|
||||
const Rot3& Rot_j = pose_j.rotation();
|
||||
const Vector3& pos_j = pose_j.translation().vector();
|
||||
|
||||
// Jacobian computation
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
// Get Get so<3> version of bias corrected rotation
|
||||
// If H5 is asked for, we will need the Jacobian, which we store in H5
|
||||
// H5 will then be corrected below to take into account the Coriolis effect
|
||||
Vector3 theta_biascorrected =
|
||||
biascorrectedThetaRij(biasOmegaIncr, H5);
|
||||
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
|
||||
|
||||
const Rot3 deltaRij_biascorrected_corioliscorrected =
|
||||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
|
||||
// TODO: these are not always needed
|
||||
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) {
|
||||
H1->resize(9,6);
|
||||
|
||||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * Rot_i.matrix() * deltaTij()*deltaTij();
|
||||
dfVdPi = skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * Rot_i.matrix() * deltaTij();
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Z_3x3;
|
||||
}
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(deltaPij()
|
||||
+ delPdelBiasOmega() * biasOmegaIncr + delPdelBiasAcc() * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(deltaVij()
|
||||
+ delVdelBiasOmega() * biasOmegaIncr + delVdelBiasAcc() * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Z_3x3;
|
||||
}
|
||||
if(H2) {
|
||||
H2->resize(9,3);
|
||||
(*H2) <<
|
||||
// dfP/dVi
|
||||
- I_3x3 * deltaTij()
|
||||
+ skewSymmetric(omegaCoriolis) * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
// dfV/dVi
|
||||
- I_3x3
|
||||
+ 2 * skewSymmetric(omegaCoriolis) * deltaTij(), // Coriolis term
|
||||
// dfR/dVi
|
||||
Z_3x3;
|
||||
}
|
||||
if(H3) {
|
||||
H3->resize(9,6);
|
||||
(*H3) <<
|
||||
// dfP/dPosej
|
||||
Z_3x3, Rot_j.matrix(),
|
||||
// dfV/dPosej
|
||||
Matrix::Zero(3,6),
|
||||
// dfR/dPosej
|
||||
Jrinv_fRhat * ( I_3x3 ), Z_3x3;
|
||||
}
|
||||
if(H4) {
|
||||
H4->resize(9,3);
|
||||
(*H4) <<
|
||||
// dfP/dVj
|
||||
Z_3x3,
|
||||
// dfV/dVj
|
||||
I_3x3,
|
||||
// dfR/dVj
|
||||
Z_3x3;
|
||||
}
|
||||
if(H5) {
|
||||
// H5 by this point already contains 3*3 biascorrectedThetaRij derivative
|
||||
const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5);
|
||||
H5->resize(9,6);
|
||||
(*H5) <<
|
||||
// dfP/dBias
|
||||
- Rot_i.matrix() * delPdelBiasAcc(),
|
||||
- Rot_i.matrix() * delPdelBiasOmega(),
|
||||
// dfV/dBias
|
||||
- Rot_i.matrix() * delVdelBiasAcc(),
|
||||
- Rot_i.matrix() * delVdelBiasOmega(),
|
||||
// dfR/dBias
|
||||
Matrix::Zero(3,3),
|
||||
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
|
||||
}
|
||||
|
||||
// Evaluate residual error, according to [3]
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
|
||||
omegaCoriolis, use2ndOrderCoriolis);
|
||||
|
||||
const Vector3 fp = pos_j - predictedState_j.pose.translation().vector();
|
||||
|
||||
const Vector3 fv = vel_j - predictedState_j.velocity;
|
||||
|
||||
// This is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j)
|
||||
const Vector3 fR = Rot3::Logmap(fRhat);
|
||||
|
||||
Vector r(9); r << fp, fv, fR;
|
||||
return r;
|
||||
}
|
||||
|
||||
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
|
||||
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
|
||||
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
|
||||
|
|
|
|||
|
|
@ -290,7 +290,7 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){
|
|||
// Predict
|
||||
Pose3 x1;
|
||||
Vector3 v1(0, 0.0, 0.0);
|
||||
PoseVelocityBias poseVelocityBias = Combinedfactor.predict(x1, v1, bias, Combined_pre_int_data, gravity, omegaCoriolis);
|
||||
PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
|
||||
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
|
||||
Vector3 expectedVelocity; expectedVelocity<<0,1,0;
|
||||
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
|
||||
|
|
@ -319,7 +319,7 @@ TEST(CombinedImuFactor, PredictRotation) {
|
|||
// Predict
|
||||
Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0));
|
||||
Vector3 v(0,0,0);
|
||||
PoseVelocityBias poseVelocityBias = Combinedfactor.predict(x,v,bias, Combined_pre_int_data, gravity, omegaCoriolis);
|
||||
PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis);
|
||||
Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0));
|
||||
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -561,7 +561,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){
|
|||
// Predict
|
||||
Pose3 x1;
|
||||
Vector3 v1(0, 0.0, 0.0);
|
||||
PoseVelocityBias poseVelocity = factor.predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
|
||||
PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
|
||||
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
|
||||
Vector3 expectedVelocity; expectedVelocity<<0,1,0;
|
||||
EXPECT(assert_equal(expectedPose, poseVelocity.pose));
|
||||
|
|
@ -593,7 +593,7 @@ TEST(ImuFactor, PredictRotation) {
|
|||
// Predict
|
||||
Pose3 x1;
|
||||
Vector3 v1(0, 0.0, 0.0);
|
||||
PoseVelocityBias poseVelocity = factor.predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
|
||||
PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
|
||||
Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0));
|
||||
Vector3 expectedVelocity; expectedVelocity<<0,0,0;
|
||||
EXPECT(assert_equal(expectedPose, poseVelocity.pose));
|
||||
|
|
|
|||
Loading…
Reference in New Issue