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

Made PreintegratedRotation a base class of PreintegrationBase

release/4.3a0
dellaert 2014-12-05 12:17:08 +01:00
parent 507979c526
commit ac8e4d2536
4 changed files with 70 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -91,11 +91,13 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// 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
// consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3)
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
Rot3 Rot_j = deltaRij_ * Rincr; // Update preintegrated measurements. TODO Frank moved from end of this function !!!
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT);
const Vector3 theta_j = thetaRij(); // super-expensive parametrization of so(3)
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
// Single Jacobians to propagate covariance // Single Jacobians to propagate covariance
@ -105,10 +107,10 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
Matrix3 H_vel_pos = Z_3x3; Matrix3 H_vel_pos = Z_3x3;
Matrix3 H_vel_vel = I_3x3; Matrix3 H_vel_vel = I_3x3;
Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; Matrix3 H_vel_angles = - deltaRij() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative // analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); // Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT; Matrix3 H_vel_biasacc = - deltaRij() * deltaT;
Matrix3 H_angles_pos = Z_3x3; Matrix3 H_angles_pos = Z_3x3;
Matrix3 H_angles_vel = Z_3x3; Matrix3 H_angles_vel = Z_3x3;
@ -150,10 +152,6 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); G_measCov_Gt.block<3,3>(6,3) = block23.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
// Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------

15
gtsam/navigation/ImuFactor.cpp
View File

@ -86,11 +86,14 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// 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
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3)
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
Rot3 Rot_j = deltaRij_ * Rincr; // Update preintegrated measurements. TODO Frank moved from end of this function !!!
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) // Even though Luca says has to happen after ? Don't understand why.
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT);
const Vector3 theta_j = thetaRij(); // super-expensive parametrization of so(3)
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
Matrix H_pos_pos = I_3x3; Matrix H_pos_pos = I_3x3;
@ -99,7 +102,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
Matrix H_vel_pos = Z_3x3; Matrix H_vel_pos = Z_3x3;
Matrix H_vel_vel = I_3x3; Matrix H_vel_vel = I_3x3;
Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; Matrix H_vel_angles = - deltaRij() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative // analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); // Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
@ -130,10 +133,6 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
// //
// preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
// Update preintegrated measurements (this has to be done after the update of covariances and jacobians!)
/* ----------------------------------------------------------------------------------------------------------------------- */
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------

23
gtsam/navigation/ImuFactorBase.h
View File

@ -99,7 +99,7 @@ public:
const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, 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{ boost::optional<Matrix&> H3, boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const{
const double& deltaTij = preintegratedMeasurements_.deltaTij_; const double& deltaTij = preintegratedMeasurements_.deltaTij();
// We need the mistmatch w.r.t. the biases used for preintegration // We need the mistmatch w.r.t. the biases used for preintegration
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
@ -111,10 +111,11 @@ public:
// Jacobian computation // Jacobian computation
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); // Get Get so<3> version of bias corrected rotation
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) // 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 = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_biascorrected =
preintegratedMeasurements_.biascorrectedThetaRij(biasOmegaIncr, H5);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
@ -122,6 +123,7 @@ public:
const Rot3 deltaRij_biascorrected_corioliscorrected = const Rot3 deltaRij_biascorrected_corioliscorrected =
Rot3::Expmap( theta_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 Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); 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 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
@ -189,9 +191,8 @@ public:
Z_3x3; Z_3x3;
} }
if(H5) { if(H5) {
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); // H5 by this point already contains 3*3 biascorrectedThetaRij derivative
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
H5->resize(9,6); H5->resize(9,6);
(*H5) << (*H5) <<
// dfP/dBias // dfP/dBias
@ -228,7 +229,7 @@ public:
const PreintegrationBase& preintegratedMeasurements, const PreintegrationBase& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
const double& deltaTij = preintegratedMeasurements.deltaTij_; const double& deltaTij = preintegratedMeasurements.deltaTij();
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
@ -255,8 +256,10 @@ public:
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
} }
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); const Rot3 deltaRij_biascorrected = preintegratedMeasurements.biascorrectedDeltaRij(biasOmegaIncr);
// TODO Frank says comment below does not reflect what was in code
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term

96
gtsam/navigation/PreintegrationBase.h
View File

@ -21,19 +21,18 @@
#pragma once #pragma once
/* GTSAM includes */ #include <gtsam/navigation/PreintegratedRotation.h>
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
namespace gtsam { namespace gtsam {
/** /**
* PreintegrationBase is the base class for PreintegratedMeasurements * PreintegrationBase is the base class for PreintegratedMeasurements
* (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor).
* It includes the definitions of the preintegrated variables and the methods * It includes the definitions of the preintegrated variables and the methods
* to access, print, and compare them. * to access, print, and compare them.
*/ */
class PreintegrationBase { class PreintegrationBase : public PreintegratedRotation {
friend class ImuFactorBase; friend class ImuFactorBase;
friend class ImuFactor; friend class ImuFactor;
@ -45,14 +44,11 @@ protected:
Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
public: public:
@ -65,82 +61,90 @@ public:
PreintegrationBase(const imuBias::ConstantBias& bias, const bool use2ndOrderIntegration) : PreintegrationBase(const imuBias::ConstantBias& bias, const bool use2ndOrderIntegration) :
biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration), biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration),
deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
deltaRij_(Rot3()), deltaTij_(0.0),
delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3) {}
delRdelBiasOmega_(Z_3x3) {}
/// methods to access class variables
const Vector3& deltaPij() const {return deltaPij_;}
const Vector3& deltaVij() const {return deltaVij_;}
Vector biasHat() const { return biasHat_.vector();} // TODO expensive
const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;}
const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;}
const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;}
const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;}
/// Needed for testable /// Needed for testable
void print(const std::string& s) const { void print(const std::string& s) const {
std::cout << s << std::endl; PreintegratedRotation::print(s);
biasHat_.print(" biasHat");
std::cout << " deltaTij " << deltaTij_ << std::endl;
std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
deltaRij_.print(" deltaRij "); biasHat_.print(" biasHat");
} }
/// Needed for testable /// Needed for testable
bool equals(const PreintegrationBase& expected, double tol) const { bool equals(const PreintegrationBase& expected, double tol) const {
return biasHat_.equals(expected.biasHat_, tol) return PreintegratedRotation::equals(expected, tol)
&& biasHat_.equals(expected.biasHat_, tol)
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
&& deltaRij_.equals(expected.deltaRij_, tol)
&& fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol);
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
} }
/// Re-initialize PreintegratedMeasurements /// Re-initialize PreintegratedMeasurements
void resetIntegration(){ void resetIntegration(){
PreintegratedRotation::resetIntegration();
deltaPij_ = Vector3::Zero(); deltaPij_ = Vector3::Zero();
deltaVij_ = Vector3::Zero(); deltaVij_ = Vector3::Zero();
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delPdelBiasAcc_ = Z_3x3; delPdelBiasAcc_ = Z_3x3;
delPdelBiasOmega_ = Z_3x3; delPdelBiasOmega_ = Z_3x3;
delVdelBiasAcc_ = Z_3x3; delVdelBiasAcc_ = Z_3x3;
delVdelBiasOmega_ = Z_3x3; delVdelBiasOmega_ = Z_3x3;
delRdelBiasOmega_ = Z_3x3;
} }
/// Update preintegrated measurements /// Update preintegrated measurements
void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& Rincr, double deltaT){ void updatePreintegratedMeasurements(const Vector3& correctedAcc,
const Rot3& incrR, double deltaT) {
Matrix3 dRij = deltaRij(); // expensive
Vector3 temp = dRij * correctedAcc * deltaT;
if(!use2ndOrderIntegration_){ if(!use2ndOrderIntegration_){
deltaPij_ += deltaVij_ * deltaT; deltaPij_ += deltaVij_ * deltaT;
}else{ }else{
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * correctedAcc * deltaT*deltaT; deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
} }
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; deltaVij_ += temp;
deltaRij_ = deltaRij_ * Rincr; // TODO: we update rotation *after* the others. Is that correct?
deltaTij_ += deltaT; updateIntegratedRotationAndDeltaT(incrR,deltaT);
} }
/// Update Jacobians to be used during preintegration /// Update Jacobians to be used during preintegration
void updatePreintegratedJacobians(const Vector3& correctedAcc, void updatePreintegratedJacobians(const Vector3& correctedAcc,
const Matrix3& Jr_theta_incr, const Rot3& Rincr, double deltaT){ const Matrix3& Jr_theta_incr, const Rot3& incrR, double deltaT){
if(!use2ndOrderIntegration_){ Matrix3 dRij = deltaRij(); // expensive
Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega();
if (!use2ndOrderIntegration_) {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
}else{ } else {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() delPdelBiasOmega_ += deltaT*(delVdelBiasOmega_ + temp * 0.5);
* skewSymmetric(correctedAcc) * deltaT*deltaT * delRdelBiasOmega_;
} }
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; delVdelBiasAcc_ += -dRij * deltaT;
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; delVdelBiasOmega_ += temp;
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; // TODO: we update rotation *after* the others. Is that correct?
update_delRdelBiasOmega(Jr_theta_incr,incrR,deltaT);
} }
void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, const Vector3& measuredOmega, void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
Vector3& correctedAcc, Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor){ const Vector3& measuredOmega, Vector3& correctedAcc,
Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor) {
correctedAcc = biasHat_.correctAccelerometer(measuredAcc); correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
correctedOmega = biasHat_.correctGyroscope(measuredOmega); correctedOmega = biasHat_.correctGyroscope(measuredOmega);
// Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame // Then compensate for sensor-body displacement: we express the quantities
// (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
@ -150,18 +154,6 @@ public:
} }
} }
/// methods to access class variables
Matrix deltaRij() const {return deltaRij_.matrix();}
double deltaTij() const{return deltaTij_;}
Vector deltaPij() const {return deltaPij_;}
Vector deltaVij() const {return deltaVij_;}
Vector biasHat() const { return biasHat_.vector();}
Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones) // 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, static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
@ -189,16 +181,14 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
ar & BOOST_SERIALIZATION_NVP(biasHat_); ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(deltaPij_); ar & BOOST_SERIALIZATION_NVP(deltaPij_);
ar & BOOST_SERIALIZATION_NVP(deltaVij_); ar & BOOST_SERIALIZATION_NVP(deltaVij_);
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
} }
}; };