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

Complete refactor with a shared parameter to fixed parameters. Tests still use old-style and all pass, because of hacky backwards compatible functions.

release/4.3a0
Frank Dellaert 2015-07-18 18:30:42 -07:00
parent 45e99f05b6
commit 0df1e345a3
13 changed files with 762 additions and 844 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
gtsam/navigation/AHRSFactor.cpp
View File

@ -27,52 +27,36 @@ namespace gtsam {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// Inner class PreintegratedMeasurements // Inner class PreintegratedMeasurements
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( void PreintegratedAhrsMeasurements::print(const string& s) const {
const Vector3& bias, const Matrix3& measuredOmegaCovariance) :
PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias)
{
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
PreintegratedRotation(I_3x3), biasHat_(Vector3())
{
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void AHRSFactor::PreintegratedMeasurements::print(const string& s) const {
PreintegratedRotation::print(s); PreintegratedRotation::print(s);
cout << "biasHat [" << biasHat_.transpose() << "]" << endl; cout << "biasHat [" << biasHat_.transpose() << "]" << endl;
cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl; cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool AHRSFactor::PreintegratedMeasurements::equals( bool PreintegratedAhrsMeasurements::equals(
const PreintegratedMeasurements& other, double tol) const { const PreintegratedAhrsMeasurements& other, double tol) const {
return PreintegratedRotation::equals(other, tol) return PreintegratedRotation::equals(other, tol) &&
&& equal_with_abs_tol(biasHat_, other.biasHat_, tol); equal_with_abs_tol(biasHat_, other.biasHat_, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void AHRSFactor::PreintegratedMeasurements::resetIntegration() { void PreintegratedAhrsMeasurements::resetIntegration() {
PreintegratedRotation::resetIntegration(); PreintegratedRotation::resetIntegration();
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( void PreintegratedAhrsMeasurements::integrateMeasurement(
const Vector3& measuredOmega, double deltaT, const Vector3& measuredOmega, double deltaT) {
boost::optional<const Pose3&> body_P_sensor) {
// First we compensate the measurements for the bias // First we compensate the measurements for the bias
Vector3 correctedOmega = measuredOmega - biasHat_; Vector3 correctedOmega = measuredOmega - biasHat_;
// Then compensate for sensor-body displacement: we express the quantities // Then compensate for sensor-body displacement: we express the quantities
// (originally in the IMU frame) into the body frame // (originally in the IMU frame) into the body frame
if (body_P_sensor) { if (p().body_P_sensor) {
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); Matrix3 body_R_sensor = p().body_P_sensor->rotation().matrix();
// rotation rate vector in the body frame // rotation rate vector in the body frame
correctedOmega = body_R_sensor * correctedOmega; correctedOmega = body_R_sensor * correctedOmega;
} }
@ -92,18 +76,17 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
// first order uncertainty propagation // first order uncertainty propagation
// the deltaT allows to pass from continuous time noise to discrete time noise // the deltaT allows to pass from continuous time noise to discrete time noise
preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + p().gyroscopeCovariance * deltaT;
+ gyroscopeCovariance() * deltaT;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, Vector3 PreintegratedAhrsMeasurements::predict(const Vector3& bias,
boost::optional<Matrix&> H) const { OptionalJacobian<3,3> H) const {
const Vector3 biasOmegaIncr = bias - biasHat_; const Vector3 biasOmegaIncr = bias - biasHat_;
return biascorrectedThetaRij(biasOmegaIncr, H); return biascorrectedThetaRij(biasOmegaIncr, H);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( Vector PreintegratedAhrsMeasurements::DeltaAngles(
const Vector& msr_gyro_t, const double msr_dt, const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles) { const Vector3& delta_angles) {
@ -121,22 +104,16 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles(
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// AHRSFactor methods // AHRSFactor methods
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
AHRSFactor::AHRSFactor() : AHRSFactor::AHRSFactor(
_PIM_(Vector3(), Z_3x3) { Key rot_i, Key rot_j, Key bias,
} const PreintegratedAhrsMeasurements& preintegratedMeasurements)
: Base(noiseModel::Gaussian::Covariance(
preintegratedMeasurements.preintMeasCov_),
rot_i, rot_j, bias),
_PIM_(preintegratedMeasurements) {}
AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) :
Base(
noiseModel::Gaussian::Covariance(
preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_(
preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
body_P_sensor) {
}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const {
//------------------------------------------------------------------------------
return boost::static_pointer_cast<gtsam::NonlinearFactor>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
@ -147,20 +124,13 @@ void AHRSFactor::print(const string& s,
cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ",";
_PIM_.print(" preintegrated measurements:"); _PIM_.print(" preintegrated measurements:");
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
noiseModel_->print(" noise model: "); noiseModel_->print(" noise model: ");
if (body_P_sensor_)
body_P_sensor_->print(" sensor pose in body frame: ");
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
const This *e = dynamic_cast<const This*>(&other); const This *e = dynamic_cast<const This*>(&other);
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_)
|| (body_P_sensor_ && e->body_P_sensor_
&& body_P_sensor_->equals(*e->body_P_sensor_)));
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -172,7 +142,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3); const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3);
// Coriolis term // Coriolis term
const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_); const Vector3 coriolis = _PIM_.integrateCoriolis(Ri);
const Matrix3 coriolisHat = skewSymmetric(coriolis); const Matrix3 coriolisHat = skewSymmetric(coriolis);
const Vector3 correctedOmega = biascorrectedOmega - coriolis; const Vector3 correctedOmega = biascorrectedOmega - coriolis;
@ -215,15 +185,13 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, Rot3 AHRSFactor::Predict(
const PreintegratedMeasurements preintegratedMeasurements, const Rot3& rot_i, const Vector3& bias,
const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) { const PreintegratedAhrsMeasurements preintegratedMeasurements) {
const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias); const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias);
// Coriolis term // Coriolis term
const Vector3 coriolis = // const Vector3 coriolis = preintegratedMeasurements.integrateCoriolis(rot_i);
preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis);
const Vector3 correctedOmega = biascorrectedOmega - coriolis; const Vector3 correctedOmega = biascorrectedOmega - coriolis;
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
@ -231,4 +199,31 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
return rot_i.compose(correctedDeltaRij); return rot_i.compose(correctedDeltaRij);
} }
} //namespace gtsam //------------------------------------------------------------------------------
AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& pim,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, bias),
_PIM_(pim) {
boost::shared_ptr<PreintegratedMeasurements::Params> p =
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
p->body_P_sensor = body_P_sensor;
_PIM_.p_ = p;
}
//------------------------------------------------------------------------------
Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements pim,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor) {
boost::shared_ptr<PreintegratedMeasurements::Params> p =
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
PreintegratedMeasurements newPim = pim;
newPim.p_ = p;
return Predict(rot_i, bias, newPim);
}
} // namespace gtsam

175
gtsam/navigation/AHRSFactor.h
View File

@ -26,91 +26,92 @@
namespace gtsam { namespace gtsam {
class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> { /**
public: * PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope
* measurements (rotation rates) and the corresponding covariance matrix.
* Can be built incrementally so as to avoid costly integration at time of factor construction.
*/
class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation {
protected:
Vector3 biasHat_; ///< Angular rate bias values used during preintegration.
Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
/// Default constructor, only for serialization
PreintegratedAhrsMeasurements() {}
friend class AHRSFactor;
public:
/** /**
* CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope * Default constructor, initialize with no measurements
* measurements (rotation rates) and the corresponding covariance matrix. * @param bias Current estimate of acceleration and rotation rate biases
* The measurements are then used to build the Preintegrated AHRS factor.
* Can be built incrementally so as to avoid costly integration at time of
* factor construction.
*/ */
class GTSAM_EXPORT PreintegratedMeasurements : public PreintegratedRotation { PreintegratedAhrsMeasurements(const boost::shared_ptr<const Params>& p, const Vector3& biasHat)
: PreintegratedRotation(p), biasHat_(biasHat) {
resetIntegration();
}
friend class AHRSFactor; const Params& p() const { return *boost::static_pointer_cast<const Params>(p_);}
const Vector3& biasHat() const { return biasHat_; }
const Matrix3& preintMeasCov() const { return preintMeasCov_; }
protected: /// print
Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer void print(const std::string& s = "Preintegrated Measurements: ") const;
Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
public: /// equals
bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const;
/// Default constructor /// Reset inetgrated quantities to zero
PreintegratedMeasurements(); void resetIntegration();
/** /**
* Default constructor, initialize with no measurements * Add a single Gyroscope measurement to the preintegration.
* @param bias Current estimate of acceleration and rotation rate biases * @param measureOmedga Measured angular velocity (in body frame)
* @param measuredOmegaCovariance Covariance matrix of measured angular rate * @param deltaT Time step
*/ */
PreintegratedMeasurements(const Vector3& bias, void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
const Matrix3& measuredOmegaCovariance);
Vector3 biasHat() const { /// Predict bias-corrected incremental rotation
return biasHat_; /// TODO: The matrix Hbias is the derivative of predict? Unit-test?
} Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const;
const Matrix3& preintMeasCov() const {
return preintMeasCov_;
}
/// print // This function is only used for test purposes
void print(const std::string& s = "Preintegrated Measurements: ") const; // (compare numerical derivatives wrt analytic ones)
static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles);
/// equals /// @deprecated constructor
bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const; PreintegratedAhrsMeasurements(const Vector3& biasHat,
const Matrix3& measuredOmegaCovariance)
/// TODO: Document : PreintegratedRotation(boost::make_shared<Params>()),
void resetIntegration(); biasHat_(biasHat) {
resetIntegration();
/** }
* Add a single Gyroscope measurement to the preintegration.
* @param measureOmedga Measured angular velocity (in body frame)
* @param deltaT Time step
* @param body_P_sensor Optional sensor frame
*/
void integrateMeasurement(const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none);
/// Predict bias-corrected incremental rotation
/// TODO: The matrix Hbias is the derivative of predict? Unit-test?
Vector3 predict(const Vector3& bias, boost::optional<Matrix&> H =
boost::none) const;
// This function is only used for test purposes
// (compare numerical derivatives wrt analytic ones)
static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
}
};
private: private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
}
};
class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
typedef AHRSFactor This; typedef AHRSFactor This;
typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base; typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base;
PreintegratedMeasurements _PIM_; PreintegratedAhrsMeasurements _PIM_;
Vector3 gravity_;
Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect /** Default constructor - only use for serialization */
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame AHRSFactor() {}
public: public:
@ -121,22 +122,15 @@ public:
typedef boost::shared_ptr<AHRSFactor> shared_ptr; typedef boost::shared_ptr<AHRSFactor> shared_ptr;
#endif #endif
/** Default constructor - only use for serialization */
AHRSFactor();
/** /**
* Constructor * Constructor
* @param rot_i previous rot key * @param rot_i previous rot key
* @param rot_j current rot key * @param rot_j current rot key
* @param bias previous bias key * @param bias previous bias key
* @param preintegratedMeasurements preintegrated measurements * @param preintegratedMeasurements preintegrated measurements
* @param omegaCoriolis rotation rate of the inertial frame
* @param body_P_sensor Optional pose of the sensor frame in the body frame
*/ */
AHRSFactor(Key rot_i, Key rot_j, Key bias, AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements, const PreintegratedAhrsMeasurements& preintegratedMeasurements);
const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none);
virtual ~AHRSFactor() { virtual ~AHRSFactor() {
} }
@ -152,14 +146,10 @@ public:
virtual bool equals(const NonlinearFactor&, double tol = 1e-9) const; virtual bool equals(const NonlinearFactor&, double tol = 1e-9) const;
/// Access the preintegrated measurements. /// Access the preintegrated measurements.
const PreintegratedMeasurements& preintegratedMeasurements() const { const PreintegratedAhrsMeasurements& preintegratedMeasurements() const {
return _PIM_; return _PIM_;
} }
const Vector3& omegaCoriolis() const {
return omegaCoriolis_;
}
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/// vector of errors /// vector of errors
@ -169,10 +159,25 @@ public:
boost::none) const; boost::none) const;
/// predicted states from IMU /// predicted states from IMU
/// TODO(frank): relationship with PIM predict ??
static Rot3 Predict(
const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements preintegratedMeasurements);
/// @deprecated name
typedef PreintegratedAhrsMeasurements PreintegratedMeasurements;
/// @deprecated constructor
AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
/// @deprecated static function
static Rot3 predict(const Rot3& rot_i, const Vector3& bias, static Rot3 predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements preintegratedMeasurements, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& omegaCoriolis, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none); const boost::optional<Pose3>& body_P_sensor = boost::none);
private: private:
@ -184,13 +189,9 @@ private:
& boost::serialization::make_nvp("NoiseModelFactor3", & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
} }
}; };
// AHRSFactor // AHRSFactor
typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements;
} //namespace gtsam } //namespace gtsam

153
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -29,53 +29,30 @@ namespace gtsam {
using namespace std; using namespace std;
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// Inner class CombinedPreintegratedMeasurements // Inner class PreintegratedCombinedMeasurements
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements( void PreintegratedCombinedMeasurements::print(
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit,
const bool use2ndOrderIntegration) :
PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
integrationErrorCovariance, use2ndOrderIntegration), biasAccCovariance_(
biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), biasAccOmegaInit_(
biasAccOmegaInit) {
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::print(
const string& s) const { const string& s) const {
PreintegrationBase::print(s); PreintegrationBase::print(s);
cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl;
cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl;
cout << " biasAccOmegaInit [ " << biasAccOmegaInit_ << " ]" << endl;
cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl; cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals( bool PreintegratedCombinedMeasurements::equals(
const CombinedPreintegratedMeasurements& expected, double tol) const { const PreintegratedCombinedMeasurements& other, double tol) const {
return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, return PreintegrationBase::equals(other, tol) &&
tol) equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
&& equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_,
tol)
&& equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol)
&& equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
&& PreintegrationBase::equals(expected, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration() { void PreintegratedCombinedMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration(); PreintegrationBase::resetIntegration();
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( void PreintegratedCombinedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor,
boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) { boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
// 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.
@ -83,7 +60,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
Vector3 correctedAcc, correctedOmega; Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
correctedAcc, correctedOmega, body_P_sensor); &correctedAcc, &correctedOmega);
const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
@ -91,20 +68,19 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// Update Jacobians // Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
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
// 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 Matrix3 R_i = deltaRij(); // store this const Matrix3 dRij = deltaRij().matrix(); // expensive when quaternion
// Update preintegrated measurements. TODO Frank moved from end of this function !!! // Update preintegrated measurements. TODO Frank moved from end of this function !!!
Matrix9 F_9x9; Matrix9 F_9x9;
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9); updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9);
// Single Jacobians to propagate covariance // Single Jacobians to propagate covariance
Matrix3 H_vel_biasacc = -R_i * deltaT; Matrix3 H_vel_biasacc = -dRij * deltaT;
Matrix3 H_angles_biasomega = -D_Rincr_integratedOmega * deltaT; Matrix3 H_angles_biasomega = -D_Rincr_integratedOmega * deltaT;
// overall Jacobian wrt preintegrated measurements (df/dx) // overall Jacobian wrt preintegrated measurements (df/dx)
@ -127,18 +103,18 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
G_measCov_Gt.setZero(15, 15); G_measCov_Gt.setZero(15, 15);
// BLOCK DIAGONAL TERMS // BLOCK DIAGONAL TERMS
G_measCov_Gt.block<3, 3>(0, 0) = deltaT * integrationCovariance(); G_measCov_Gt.block<3, 3>(0, 0) = deltaT * p().integrationCovariance;
G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc) G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc)
* (accelerometerCovariance() + biasAccOmegaInit_.block<3, 3>(0, 0)) * (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0))
* (H_vel_biasacc.transpose()); * (H_vel_biasacc.transpose());
G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega) G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega)
* (gyroscopeCovariance() + biasAccOmegaInit_.block<3, 3>(3, 3)) * (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3))
* (H_angles_biasomega.transpose()); * (H_angles_biasomega.transpose());
G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * biasAccCovariance_; G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * p().biasAccCovariance;
G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * biasOmegaCovariance_; G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS // OFF BLOCK DIAGONAL TERMS
Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3, 3>(3, 0) Matrix3 block23 = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0)
* H_angles_biasomega.transpose(); * H_angles_biasomega.transpose();
G_measCov_Gt.block<3, 3>(3, 6) = block23; G_measCov_Gt.block<3, 3>(3, 6) = block23;
G_measCov_Gt.block<3, 3>(6, 3) = block23.transpose(); G_measCov_Gt.block<3, 3>(6, 3) = block23.transpose();
@ -162,26 +138,35 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
} }
} }
//------------------------------------------------------------------------------
PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit,
const bool use2ndOrderIntegration) {
biasHat_ = biasHat;
boost::shared_ptr<Params> p = boost::make_shared<Params>();
p->gyroscopeCovariance = measuredOmegaCovariance;
p->accelerometerCovariance = measuredAccCovariance;
p->integrationCovariance = integrationErrorCovariance;
p->biasAccCovariance = biasAccCovariance;
p->biasOmegaCovariance = biasOmegaCovariance;
p->biasAccOmegaInit = biasAccOmegaInit;
p->use2ndOrderIntegration = use2ndOrderIntegration;
p_ = p;
resetIntegration();
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// CombinedImuFactor methods // CombinedImuFactor methods
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor() : CombinedImuFactor::CombinedImuFactor(
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
Z_3x3, Z_6x6) { const PreintegratedCombinedMeasurements& pim)
} : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j),
//------------------------------------------------------------------------------ _PIM_(pim) {}
CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j,
Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
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_i, bias_j), ImuFactorBase(gravity, omegaCoriolis,
body_P_sensor, use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) {
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
@ -196,17 +181,14 @@ void CombinedImuFactor::print(const string& s,
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ","
<< keyFormatter(this->key6()) << ")\n"; << keyFormatter(this->key6()) << ")\n";
ImuFactorBase::print("");
_PIM_.print(" preintegrated measurements:"); _PIM_.print(" preintegrated measurements:");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool CombinedImuFactor::equals(const NonlinearFactor& expected, bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const {
double tol) const { const This* e = dynamic_cast<const This*>(&other);
const This *e = dynamic_cast<const This*>(&expected); return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
&& ImuFactorBase::equals(*e, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -226,8 +208,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
Matrix93 D_r_vel_i, D_r_vel_j; Matrix93 D_r_vel_i, D_r_vel_j;
// error wrt preintegrated measurements // error wrt preintegrated measurements
Vector9 r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, Vector9 r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
bias_i, gravity_, omegaCoriolis_, use2ndOrderCoriolis_,
H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0); H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);
@ -275,4 +256,42 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
return r; return r;
} }
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor(
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& pim, const Vector3& gravity,
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
const bool use2ndOrderCoriolis)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j),
_PIM_(pim) {
boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
p->gravity = gravity;
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
_PIM_.p_ = p;
}
//------------------------------------------------------------------------------
void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i,
const CombinedPreintegratedMeasurements& pim,
const Vector3& gravity,
const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
// NOTE(frank): hack below only for backward compatibility
boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
p->gravity = gravity;
p->omegaCoriolis = omegaCoriolis;
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
CombinedPreintegratedMeasurements newPim = pim;
newPim.p_ = p;
PoseVelocityBias pvb = newPim.predict(pose_i, vel_i, bias_i);
pose_j = pvb.pose;
vel_j = pvb.velocity;
}
} /// namespace gtsam } /// namespace gtsam

257
gtsam/navigation/CombinedImuFactor.h
View File

@ -24,13 +24,113 @@
/* GTSAM includes */ /* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/PreintegrationBase.h> #include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/navigation/ImuFactorBase.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/debug.h>
namespace gtsam { namespace gtsam {
/** /**
* * PreintegratedCombinedMeasurements integrates the IMU measurements
* (rotation rates and accelerations) and the corresponding covariance matrix.
* The measurements are then used to build the CombinedImuFactor. Integration
* is done incrementally (ideally, one integrates the measurement as soon as
* it is received from the IMU) so as to avoid costly integration at time of
* factor construction.
*/
class PreintegratedCombinedMeasurements : public PreintegrationBase {
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params : PreintegrationBase::Params {
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInit; ///< covariance of bias used for pre-integration
Params():biasAccCovariance(I_3x3), biasOmegaCovariance(I_3x3), biasAccOmegaInit(I_6x6) {}
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInit);
}
};
protected:
/* Covariance matrix of the preintegrated measurements
* COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
* (first-order propagation from *measurementCovariance*).
* PreintegratedCombinedMeasurements also include the biases and keep the correlation
* between the preintegrated measurements and the biases
*/
Eigen::Matrix<double, 15, 15> preintMeasCov_;
PreintegratedCombinedMeasurements() {}
friend class CombinedImuFactor;
public:
/**
* Default constructor, initializes the class with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
*/
PreintegratedCombinedMeasurements(const boost::shared_ptr<const Params>& p,
const imuBias::ConstantBias& biasHat)
: PreintegrationBase(p, biasHat) {
preintMeasCov_.setZero();
}
const Params& p() const { return *boost::static_pointer_cast<const Params>(p_);}
/// print
void print(const std::string& s = "Preintegrated Measurements:") const;
/// equals
bool equals(const PreintegratedCombinedMeasurements& expected,
double tol = 1e-9) const;
/// Re-initialize PreintegratedCombinedMeasurements
void resetIntegration();
/**
* Add a single IMU measurement to the preintegration.
* @param measuredAcc Measured acceleration (in body frame, as given by the
* sensor)
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param deltaT Time interval between two consecutive IMU measurements
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body
* frame)
*/
void integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<Matrix&> F_test = boost::none,
boost::optional<Matrix&> G_test = boost::none);
/// methods to access class variables
Matrix preintMeasCov() const { return preintMeasCov_; }
/// deprecated constructor
PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = false);
private:
/// Serialization function
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
};
/**
* @addtogroup SLAM * @addtogroup SLAM
* *
* If you are using the factor, please cite: * If you are using the factor, please cite:
@ -46,14 +146,12 @@ namespace gtsam {
* TRO, 28(1):61-76, 2012. * TRO, 28(1):61-76, 2012.
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
* Computation of the Jacobian Matrices", Tech. Report, 2013. * Computation of the Jacobian Matrices", Tech. Report, 2013.
*/ *
/**
* CombinedImuFactor is a 6-ways factor involving previous state (pose and * CombinedImuFactor is a 6-ways factor involving previous state (pose and
* velocity of the vehicle, as well as bias at previous time step), and current * velocity of the vehicle, as well as bias at previous time step), and current
* state (pose, velocity, bias at current time step). Following the pre- * state (pose, velocity, bias at current time step). Following the pre-
* integration scheme proposed in [2], the CombinedImuFactor includes many IMU * integration scheme proposed in [2], the CombinedImuFactor includes many IMU
* measurements, which are "summarized" using the CombinedPreintegratedMeasurements * measurements, which are "summarized" using the PreintegratedCombinedMeasurements
* class. There are 3 main differences wrpt the ImuFactor class: * class. There are 3 main differences wrpt the ImuFactor class:
* 1) The factor is 6-ways, meaning that it also involves both biases (previous * 1) The factor is 6-ways, meaning that it also involves both biases (previous
* and current time step).Therefore, the factor internally imposes the biases * and current time step).Therefore, the factor internally imposes the biases
@ -61,105 +159,24 @@ namespace gtsam {
* "biasOmegaCovariance" described the random walk that models bias evolution. * "biasOmegaCovariance" described the random walk that models bias evolution.
* 2) The preintegration covariance takes into account the noise in the bias * 2) The preintegration covariance takes into account the noise in the bias
* estimate used for integration. * estimate used for integration.
* 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves * 3) The covariance matrix of the PreintegratedCombinedMeasurements preserves
* the correlation between the bias uncertainty and the preintegrated * the correlation between the bias uncertainty and the preintegrated
* measurements uncertainty. * measurements uncertainty.
*/ */
class CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, Pose3, class CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, Pose3,
Vector3, imuBias::ConstantBias, imuBias::ConstantBias>, public ImuFactorBase { Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
public: public:
/**
* CombinedPreintegratedMeasurements integrates the IMU measurements
* (rotation rates and accelerations) and the corresponding covariance matrix.
* The measurements are then used to build the CombinedImuFactor. Integration
* is done incrementally (ideally, one integrates the measurement as soon as
* it is received from the IMU) so as to avoid costly integration at time of
* factor construction.
*/
class CombinedPreintegratedMeasurements: public PreintegrationBase {
friend class CombinedImuFactor;
protected:
Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk
Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration
Eigen::Matrix<double, 15, 15> preintMeasCov_; ///< Covariance matrix of the preintegrated measurements
///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation
///< between the preintegrated measurements and the biases
public:
/**
* Default constructor, initializes the class with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
* @param measuredAccCovariance Covariance matrix of measuredAcc
* @param measuredOmegaCovariance Covariance matrix of measured Angular Rate
* @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position)
* @param biasAccCovariance Covariance matrix of biasAcc (random walk describing BIAS evolution)
* @param biasOmegaCovariance Covariance matrix of biasOmega (random walk describing BIAS evolution)
* @param biasAccOmegaInit Covariance of biasAcc & biasOmega when preintegrating measurements
* @param use2ndOrderIntegration Controls the order of integration
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
*/
CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration =
false);
/// print
void print(const std::string& s = "Preintegrated Measurements:") const;
/// equals
bool equals(const CombinedPreintegratedMeasurements& expected, double tol =
1e-9) const;
/// Re-initialize CombinedPreintegratedMeasurements
void resetIntegration();
/**
* Add a single IMU measurement to the preintegration.
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param deltaT Time interval between two consecutive IMU measurements
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
*/
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none,
boost::optional<Matrix&> F_test = boost::none,
boost::optional<Matrix&> G_test = boost::none);
/// methods to access class variables
Matrix preintMeasCov() const {
return preintMeasCov_;
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
};
private: private:
typedef CombinedImuFactor This; typedef CombinedImuFactor This;
typedef NoiseModelFactor6<Pose3, Vector3, Pose3, Vector3, typedef NoiseModelFactor6<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias, imuBias::ConstantBias> Base; imuBias::ConstantBias, imuBias::ConstantBias> Base;
CombinedPreintegratedMeasurements _PIM_; PreintegratedCombinedMeasurements _PIM_;
/** Default constructor - only use for serialization */
CombinedImuFactor() {}
public: public:
@ -170,9 +187,6 @@ public:
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr; typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
#endif #endif
/** Default constructor - only use for serialization */
CombinedImuFactor();
/** /**
* Constructor * Constructor
* @param pose_i Previous pose key * @param pose_i Previous pose key
@ -181,21 +195,13 @@ public:
* @param vel_j Current velocity key * @param vel_j Current velocity key
* @param bias_i Previous bias key * @param bias_i Previous bias key
* @param bias_j Current bias key * @param bias_j Current bias key
* @param CombinedPreintegratedMeasurements CombinedPreintegratedMeasurements IMU measurements * @param PreintegratedCombinedMeasurements Combined IMU measurements
* @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
*/ */
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, CombinedImuFactor(
Key bias_j, Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements, const PreintegratedCombinedMeasurements& preintegratedMeasurements);
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
virtual ~CombinedImuFactor() { virtual ~CombinedImuFactor() {}
}
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const; virtual gtsam::NonlinearFactor::shared_ptr clone() const;
@ -211,7 +217,7 @@ public:
/** Access the preintegrated measurements. */ /** Access the preintegrated measurements. */
const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { const PreintegratedCombinedMeasurements& preintegratedMeasurements() const {
return _PIM_; return _PIM_;
} }
@ -226,16 +232,22 @@ public:
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 = boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
boost::none, boost::optional<Matrix&> H6 = boost::none) const; boost::none, boost::optional<Matrix&> H6 = boost::none) const;
// @deprecated The following function has been deprecated, use PreintegrationBase::predict /// @deprecated typename
typedef gtsam::PreintegratedCombinedMeasurements CombinedPreintegratedMeasurements;
/// @deprecated constructor
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
Key bias_j, const CombinedPreintegratedMeasurements& pim,
const Vector3& gravity, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
// @deprecated use PreintegrationBase::predict
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
Vector3& vel_j, const imuBias::ConstantBias& bias_i, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity, const CombinedPreintegratedMeasurements& pim,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) { const Vector3& gravity, const Vector3& omegaCoriolis,
PoseVelocityBias PVB = PIM.predict(pose_i, vel_i, bias_i, gravity, const bool use2ndOrderCoriolis = false);
omegaCoriolis, use2ndOrderCoriolis);
pose_j = PVB.pose;
vel_j = PVB.velocity;
}
private: private:
@ -246,13 +258,8 @@ private:
ar & boost::serialization::make_nvp("NoiseModelFactor6", ar & boost::serialization::make_nvp("NoiseModelFactor6",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
} }
}; };
// class CombinedImuFactor // class CombinedImuFactor
typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements;
} /// namespace gtsam } /// namespace gtsam

134
gtsam/navigation/ImuFactor.cpp
View File

@ -31,43 +31,32 @@ using namespace std;
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// Inner class PreintegratedMeasurements // Inner class PreintegratedMeasurements
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( void PreintegratedImuMeasurements::print(const string& s) const {
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const bool use2ndOrderIntegration) :
PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
integrationErrorCovariance, use2ndOrderIntegration) {
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
PreintegrationBase::print(s); PreintegrationBase::print(s);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool ImuFactor::PreintegratedMeasurements::equals( bool PreintegratedImuMeasurements::equals(
const PreintegratedMeasurements& expected, double tol) const { const PreintegratedImuMeasurements& other, double tol) const {
return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) return PreintegrationBase::equals(other, tol) &&
&& PreintegrationBase::equals(expected, tol); equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::resetIntegration() { void PreintegratedImuMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration(); PreintegrationBase::resetIntegration();
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::integrateMeasurement( void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor, OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> F_test,
OptionalJacobian<9, 9> G_test) { OptionalJacobian<9, 9> G_test) {
Vector3 correctedAcc, correctedOmega; Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
correctedAcc, correctedOmega, body_P_sensor); &correctedAcc, &correctedOmega);
// rotation increment computed from the current rotation rate measurement // rotation increment computed from the current rotation rate measurement
const Vector3 integratedOmega = correctedOmega * deltaT; const Vector3 integratedOmega = correctedOmega * deltaT;
@ -79,7 +68,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
// Update preintegrated measurements (also get Jacobian) // Update preintegrated measurements (also get Jacobian)
const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test const Matrix3 dRij = deltaRij().matrix(); // store this, which is useful to compute G_test
Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F);
@ -92,18 +81,18 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// NOTE 2: computation of G * (1/deltaT) * measurementCovariance * G.transpose() done block-wise, // NOTE 2: computation of G * (1/deltaT) * measurementCovariance * G.transpose() done block-wise,
// as G and measurementCovariance are block-diagonal matrices // as G and measurementCovariance are block-diagonal matrices
preintMeasCov_ = F * preintMeasCov_ * F.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose();
preintMeasCov_.block<3, 3>(0, 0) += integrationCovariance() * deltaT; preintMeasCov_.block<3, 3>(0, 0) += p().integrationCovariance * deltaT;
preintMeasCov_.block<3, 3>(3, 3) += R_i * accelerometerCovariance() preintMeasCov_.block<3, 3>(3, 3) += dRij * p().accelerometerCovariance
* R_i.transpose() * deltaT; * dRij.transpose() * deltaT;
preintMeasCov_.block<3, 3>(6, 6) += D_Rincr_integratedOmega preintMeasCov_.block<3, 3>(6, 6) += D_Rincr_integratedOmega
* gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; * p().gyroscopeCovariance * D_Rincr_integratedOmega.transpose() * deltaT;
Matrix3 F_pos_noiseacc; Matrix3 F_pos_noiseacc;
if (use2ndOrderIntegration()) { if (p().use2ndOrderIntegration) {
F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT; F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT;
preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc
* accelerometerCovariance() * F_pos_noiseacc.transpose(); * p().accelerometerCovariance * F_pos_noiseacc.transpose();
Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // has 1/deltaT Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance * dRij.transpose(); // has 1/deltaT
preintMeasCov_.block<3, 3>(0, 3) += temp; preintMeasCov_.block<3, 3>(0, 3) += temp;
preintMeasCov_.block<3, 3>(3, 0) += temp.transpose(); preintMeasCov_.block<3, 3>(3, 0) += temp.transpose();
} }
@ -114,34 +103,40 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
} }
if (G_test) { if (G_test) {
// This in only for testing & documentation, while the actual computation is done block-wise // This in only for testing & documentation, while the actual computation is done block-wise
if (!use2ndOrderIntegration()) if (!p().use2ndOrderIntegration)
F_pos_noiseacc = Z_3x3; F_pos_noiseacc = Z_3x3;
// intNoise accNoise omegaNoise // intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos
Z_3x3, R_i * deltaT, Z_3x3, // vel Z_3x3, dRij * deltaT, Z_3x3, // vel
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
} }
} }
//------------------------------------------------------------------------------
PreintegratedImuMeasurements::PreintegratedImuMeasurements(
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const bool use2ndOrderIntegration)
{
biasHat_ = biasHat;
boost::shared_ptr<Params> p = boost::make_shared<Params>();
p->gyroscopeCovariance = measuredOmegaCovariance;
p->accelerometerCovariance = measuredAccCovariance;
p->integrationCovariance = integrationErrorCovariance;
p->use2ndOrderIntegration = use2ndOrderIntegration;
p_ = p;
resetIntegration();
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// ImuFactor methods // ImuFactor methods
//------------------------------------------------------------------------------
ImuFactor::ImuFactor() :
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements, const PreintegratedImuMeasurements& pim)
const Vector3& gravity, const Vector3& omegaCoriolis, : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) : pose_j, vel_j, bias),
Base( _PIM_(pim) {}
noiseModel::Gaussian::Covariance(
preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j,
vel_j, bias), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor,
use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) {
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
@ -155,17 +150,17 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << keyFormatter(this->key4()) << "," << keyFormatter(this->key5())
<< ")\n"; << ")\n";
ImuFactorBase::print(""); Base::print("");
_PIM_.print(" preintegrated measurements:"); _PIM_.print(" preintegrated measurements:");
// Print standard deviations on covariance only // Print standard deviations on covariance only
cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl; cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { bool ImuFactor::equals(const NonlinearFactor& other, double tol) const {
const This *e = dynamic_cast<const This*>(&expected); const This *e = dynamic_cast<const This*>(&other);
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
&& ImuFactorBase::equals(*e, tol); && Base::equals(*e, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -174,9 +169,46 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1, const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3, boost::optional<Matrix&> H2, boost::optional<Matrix&> H3,
boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const { boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const {
return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5); H1, H2, H3, H4, H5);
}
//------------------------------------------------------------------------------
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& pim,
const Vector3& gravity, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor,
const bool use2ndOrderCoriolis)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias),
_PIM_(pim) {
boost::shared_ptr<PreintegratedMeasurements::Params> p =
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
p->gravity = gravity;
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
_PIM_.p_ = p;
}
//------------------------------------------------------------------------------
void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i,
const PreintegratedMeasurements& pim,
const Vector3& gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
// NOTE(frank): hack below only for backward compatibility
boost::shared_ptr<PreintegratedMeasurements::Params> p =
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
p->gravity = gravity;
p->omegaCoriolis = omegaCoriolis;
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
PreintegratedMeasurements newPim = pim;
newPim.p_ = p;
PoseVelocityBias pvb = newPim.predict(pose_i, vel_i, bias_i);
pose_j = pvb.pose;
vel_j = pvb.velocity;
} }
} // namespace gtsam } // namespace gtsam

195
gtsam/navigation/ImuFactor.h
View File

@ -24,11 +24,86 @@
/* GTSAM includes */ /* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/PreintegrationBase.h> #include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/navigation/ImuFactorBase.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
namespace gtsam { namespace gtsam {
/**
* PreintegratedIMUMeasurements accumulates (integrates) the IMU measurements
* (rotation rates and accelerations) and the corresponding covariance matrix.
* The measurements are then used to build the Preintegrated IMU factor.
* Integration is done incrementally (ideally, one integrates the measurement
* as soon as it is received from the IMU) so as to avoid costly integration
* at time of factor construction.
*/
class PreintegratedImuMeasurements: public PreintegrationBase {
friend class ImuFactor;
protected:
Eigen::Matrix<double, 9, 9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
///< (first-order propagation from *measurementCovariance*).
/// Default constructor for serialization
PreintegratedImuMeasurements() {}
public:
/**
* Constructor, initializes the class with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
* @param p Parameters, typically fixed in a single application
*/
PreintegratedImuMeasurements(const boost::shared_ptr<const Params>& p,
const imuBias::ConstantBias& biasHat) :
PreintegrationBase(p,biasHat) {
preintMeasCov_.setZero();
}
/// print
void print(const std::string& s = "Preintegrated Measurements:") const;
/// equals
bool equals(const PreintegratedImuMeasurements& expected,
double tol = 1e-9) const;
/// Re-initialize PreintegratedIMUMeasurements
void resetIntegration();
/**
* Add a single IMU measurement to the preintegration.
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param deltaT Time interval between this and the last IMU measurement
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
* @param Fout, Gout Jacobians used internally (only needed for testing)
*/
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT,
OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none);
/// Return pre-integrated measurement covariance
Matrix preintMeasCov() const { return preintMeasCov_; }
/// @deprecated constructor
PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const bool use2ndOrderIntegration = false);
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
};
/** /**
* *
* @addtogroup SLAM * @addtogroup SLAM
@ -53,96 +128,22 @@ namespace gtsam {
* the vehicle at previous time step), current state (pose and velocity at * the vehicle at previous time step), current state (pose and velocity at
* current time step), and the bias estimate. Following the preintegration * current time step), and the bias estimate. Following the preintegration
* scheme proposed in [2], the ImuFactor includes many IMU measurements, which * scheme proposed in [2], the ImuFactor includes many IMU measurements, which
* are "summarized" using the PreintegratedMeasurements class. * are "summarized" using the PreintegratedIMUMeasurements class.
* Note that this factor does not model "temporal consistency" of the biases * Note that this factor does not model "temporal consistency" of the biases
* (which are usually slowly varying quantities), which is up to the caller. * (which are usually slowly varying quantities), which is up to the caller.
* See also CombinedImuFactor for a class that does this for you. * See also CombinedImuFactor for a class that does this for you.
*/ */
class ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3, class ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias>, public ImuFactorBase { imuBias::ConstantBias> {
public: public:
/**
* PreintegratedMeasurements accumulates (integrates) the IMU measurements
* (rotation rates and accelerations) and the corresponding covariance matrix.
* The measurements are then used to build the Preintegrated IMU factor.
* Integration is done incrementally (ideally, one integrates the measurement
* as soon as it is received from the IMU) so as to avoid costly integration
* at time of factor construction.
*/
class PreintegratedMeasurements: public PreintegrationBase {
friend class ImuFactor;
protected:
Eigen::Matrix<double, 9, 9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
///< (first-order propagation from *measurementCovariance*).
public:
/**
* Default constructor, initializes the class with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
* @param measuredAccCovariance Covariance matrix of measuredAcc
* @param measuredOmegaCovariance Covariance matrix of measured Angular Rate
* @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position)
* @param use2ndOrderIntegration Controls the order of integration
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
*/
PreintegratedMeasurements(const imuBias::ConstantBias& bias,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const bool use2ndOrderIntegration = false);
/// print
void print(const std::string& s = "Preintegrated Measurements:") const;
/// equals
bool equals(const PreintegratedMeasurements& expected,
double tol = 1e-9) const;
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
/**
* Add a single IMU measurement to the preintegration.
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param deltaT Time interval between this and the last IMU measurement
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
* @param Fout, Gout Jacobians used internally (only needed for testing)
*/
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none,
OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout =
boost::none);
/// methods to access class variables
Matrix preintMeasCov() const {
return preintMeasCov_;
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
};
private: private:
typedef ImuFactor This; typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3, typedef NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> Base; imuBias::ConstantBias> Base;
PreintegratedMeasurements _PIM_; PreintegratedImuMeasurements _PIM_;
public: public:
@ -163,17 +164,9 @@ public:
* @param pose_j Current pose key * @param pose_j Current pose key
* @param vel_j Current velocity key * @param vel_j Current velocity key
* @param bias Previous bias key * @param bias Previous bias key
* @param preintegratedMeasurements Preintegrated IMU measurements
* @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
*/ */
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements, const PreintegratedImuMeasurements& preintegratedMeasurements);
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
virtual ~ImuFactor() { virtual ~ImuFactor() {
} }
@ -192,7 +185,7 @@ public:
/** Access the preintegrated measurements. */ /** Access the preintegrated measurements. */
const PreintegratedMeasurements& preintegratedMeasurements() const { const PreintegratedImuMeasurements& preintegratedMeasurements() const {
return _PIM_; return _PIM_;
} }
@ -206,16 +199,21 @@ public:
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 = boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
boost::none, boost::optional<Matrix&> H5 = boost::none) const; boost::none, boost::optional<Matrix&> H5 = boost::none) const;
/// @deprecated The following function has been deprecated, use PreintegrationBase::predict /// @deprecated typename
typedef PreintegratedImuMeasurements PreintegratedMeasurements;
/// @deprecated constructor
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
/// @deprecated use PreintegrationBase::predict
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
Vector3& vel_j, const imuBias::ConstantBias& bias_i, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
const PreintegratedMeasurements& PIM, const Vector3& gravity, const PreintegratedMeasurements& pim, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) { const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
PoseVelocityBias PVB = PIM.predict(pose_i, vel_i, bias_i, gravity,
omegaCoriolis, use2ndOrderCoriolis);
pose_j = PVB.pose;
vel_j = PVB.velocity;
}
private: private:
@ -226,13 +224,8 @@ private:
ar & boost::serialization::make_nvp("NoiseModelFactor5", ar & boost::serialization::make_nvp("NoiseModelFactor5",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
} }
}; };
// class ImuFactor // class ImuFactor
typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;
} /// namespace gtsam } /// namespace gtsam

94
gtsam/navigation/ImuFactorBase.h
View File

@ -1,94 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file PreintegrationBase.h
* @author Luca Carlone
* @author Stephen Williams
* @author Richard Roberts
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
**/
#pragma once
/* GTSAM includes */
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/PreintegrationBase.h>
namespace gtsam {
class ImuFactorBase {
protected:
Vector3 gravity_;
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
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_;
}
/// Needed for testable
//------------------------------------------------------------------------------
void print(const std::string& /*s*/) const {
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]"
<< std::endl;
std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]"
<< std::endl;
if (this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
/// Needed for testable
//------------------------------------------------------------------------------
bool equals(const ImuFactorBase& expected, double tol) const {
return equal_with_abs_tol(gravity_, expected.gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol)
&& (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_)
&& ((!body_P_sensor_ && !expected.body_P_sensor_)
|| (body_P_sensor_ && expected.body_P_sensor_
&& body_P_sensor_->equals(*expected.body_P_sensor_)));
}
};
} /// namespace gtsam

85
gtsam/navigation/PreintegratedRotation.h
View File

@ -21,7 +21,8 @@
#pragma once #pragma once
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Matrix.h>
namespace gtsam { namespace gtsam {
@ -31,49 +32,64 @@ namespace gtsam {
* It includes the definitions of the preintegrated rotation. * It includes the definitions of the preintegrated rotation.
*/ */
class PreintegratedRotation { class PreintegratedRotation {
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) public:
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params {
Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
Params():gyroscopeCovariance(I_3x3) {}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
}
};
private:
double deltaTij_; ///< Time interval from i to j double deltaTij_; ///< Time interval from i to j
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
/// Jacobian of preintegrated rotation w.r.t. angular rate bias protected:
Matrix3 delRdelBiasOmega_; /// Parameters
boost::shared_ptr<const Params> p_;
///< continuous-time "Covariance" of gyroscope measurements /// Default constructor for serialization
const Matrix3 gyroscopeCovariance_; PreintegratedRotation() {}
public: public:
/// Default constructor, initializes the variables in the base class /// Default constructor, resets integration to zero
explicit PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : explicit PreintegratedRotation(const boost::shared_ptr<const Params>& p) : p_(p) {
deltaRij_(Rot3()), deltaTij_(0.0), delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_( resetIntegration();
measuredOmegaCovariance) {
} }
/// Re-initialize PreintegratedMeasurements /// Re-initialize PreintegratedMeasurements
void resetIntegration() { void resetIntegration() {
deltaRij_ = Rot3();
deltaTij_ = 0.0; deltaTij_ = 0.0;
deltaRij_ = Rot3();
delRdelBiasOmega_ = Z_3x3; delRdelBiasOmega_ = Z_3x3;
} }
/// @name Access instance variables /// @name Access instance variables
/// @{ /// @{
// Return 3*3 matrix of rotation from time i to time j. Expensive in quaternion case
Matrix3 deltaRij() const {
return deltaRij_.matrix();
}
// Return log(rotation) from time i to time j. Expensive in both Rot3M and quaternion case.
Vector3 thetaRij(OptionalJacobian<3, 3> H = boost::none) const {
return Rot3::Logmap(deltaRij_, H);
}
const double& deltaTij() const { const double& deltaTij() const {
return deltaTij_; return deltaTij_;
} }
const Rot3& deltaRij() const {
return deltaRij_;
}
const Matrix3& delRdelBiasOmega() const { const Matrix3& delRdelBiasOmega() const {
return delRdelBiasOmega_; return delRdelBiasOmega_;
} }
const Matrix3& gyroscopeCovariance() const {
return gyroscopeCovariance_;
}
/// @} /// @}
/// @name Testable /// @name Testable
@ -85,13 +101,10 @@ class PreintegratedRotation {
std::cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl; std::cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl;
} }
bool equals(const PreintegratedRotation& expected, double tol) const { bool equals(const PreintegratedRotation& other, double tol) const {
return deltaRij_.equals(expected.deltaRij_, tol) return deltaRij_.equals(other.deltaRij_, tol) &&
&& fabs(deltaTij_ - expected.deltaTij_) < tol fabs(deltaTij_ - other.deltaTij_) < tol &&
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
tol)
&& equal_with_abs_tol(gyroscopeCovariance_,
expected.gyroscopeCovariance_, tol);
} }
/// @} /// @}
@ -128,8 +141,7 @@ class PreintegratedRotation {
if (H) { if (H) {
Matrix3 Jrinv_theta_bc; Matrix3 Jrinv_theta_bc;
// This was done via an expmap, now we go *back* to so<3> // This was done via an expmap, now we go *back* to so<3>
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc);
Jrinv_theta_bc);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
return biascorrectedOmega; return biascorrectedOmega;
@ -139,9 +151,9 @@ class PreintegratedRotation {
} }
/// Integrate coriolis correction in body frame rot_i /// Integrate coriolis correction in body frame rot_i
Vector3 integrateCoriolis(const Rot3& rot_i, Vector3 integrateCoriolis(const Rot3& rot_i) const {
const Vector3& omegaCoriolis) const { if (!p_->omegaCoriolis) return Vector3::Zero();
return rot_i.transpose() * omegaCoriolis * deltaTij(); return rot_i.transpose() * (*p_->omegaCoriolis) * deltaTij();
} }
private: private:
@ -149,8 +161,9 @@ class PreintegratedRotation {
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // NOLINT void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // NOLINT
ar & BOOST_SERIALIZATION_NVP(deltaRij_); ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_); ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
} }
}; };

176
gtsam/navigation/PreintegrationBase.cpp
View File

@ -20,27 +20,12 @@
**/ **/
#include "PreintegrationBase.h" #include "PreintegrationBase.h"
#include <boost/make_shared.hpp>
using namespace std;
namespace gtsam { namespace gtsam {
PreintegrationBase::PreintegrationBase(const imuBias::ConstantBias& bias,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3&integrationErrorCovariance,
const bool use2ndOrderIntegration)
: PreintegratedRotation(measuredOmegaCovariance),
biasHat_(bias),
use2ndOrderIntegration_(use2ndOrderIntegration),
deltaPij_(Vector3::Zero()),
deltaVij_(Vector3::Zero()),
delPdelBiasAcc_(Z_3x3),
delPdelBiasOmega_(Z_3x3),
delVdelBiasAcc_(Z_3x3),
delVdelBiasOmega_(Z_3x3),
accelerometerCovariance_(measuredAccCovariance),
integrationCovariance_(integrationErrorCovariance) {
}
/// Re-initialize PreintegratedMeasurements /// Re-initialize PreintegratedMeasurements
void PreintegrationBase::resetIntegration() { void PreintegrationBase::resetIntegration() {
PreintegratedRotation::resetIntegration(); PreintegratedRotation::resetIntegration();
@ -53,24 +38,23 @@ void PreintegrationBase::resetIntegration() {
} }
/// Needed for testable /// Needed for testable
void PreintegrationBase::print(const std::string& s) const { void PreintegrationBase::print(const string& s) const {
PreintegratedRotation::print(s); PreintegratedRotation::print(s);
std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
biasHat_.print(" biasHat"); biasHat_.print(" biasHat");
} }
/// Needed for testable /// Needed for testable
bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const {
return PreintegratedRotation::equals(other, tol) && biasHat_.equals(other.biasHat_, tol) return PreintegratedRotation::equals(other, tol) &&
&& equal_with_abs_tol(deltaPij_, other.deltaPij_, tol) biasHat_.equals(other.biasHat_, tol) &&
&& equal_with_abs_tol(deltaVij_, other.deltaVij_, tol) equal_with_abs_tol(deltaPij_, other.deltaPij_, tol) &&
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) equal_with_abs_tol(deltaVij_, other.deltaVij_, tol) &&
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) &&
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) &&
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol) equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) &&
&& equal_with_abs_tol(accelerometerCovariance_, other.accelerometerCovariance_, tol) equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
&& equal_with_abs_tol(integrationCovariance_, other.integrationCovariance_, tol);
} }
/// Update preintegrated measurements /// Update preintegrated measurements
@ -78,9 +62,10 @@ void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correcte
const Rot3& incrR, const double deltaT, const Rot3& incrR, const double deltaT,
OptionalJacobian<9, 9> F) { OptionalJacobian<9, 9> F) {
const Matrix3 dRij = deltaRij(); // expensive const Matrix3 dRij = deltaRij().matrix(); // expensive
const Vector3 temp = dRij * correctedAcc * deltaT; const Vector3 temp = dRij * correctedAcc * deltaT;
if (!use2ndOrderIntegration_) {
if (!p().use2ndOrderIntegration) {
deltaPij_ += deltaVij_ * deltaT; deltaPij_ += deltaVij_ * deltaT;
} else { } else {
deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
@ -89,13 +74,13 @@ void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correcte
Matrix3 R_i, F_angles_angles; Matrix3 R_i, F_angles_angles;
if (F) if (F)
R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij R_i = dRij; // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0); updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0);
if (F) { if (F) {
const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT; const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT;
Matrix3 F_pos_angles; Matrix3 F_pos_angles;
if (use2ndOrderIntegration_) if (p().use2ndOrderIntegration)
F_pos_angles = 0.5 * F_vel_angles * deltaT; F_pos_angles = 0.5 * F_vel_angles * deltaT;
else else
F_pos_angles = Z_3x3; F_pos_angles = Z_3x3;
@ -112,9 +97,9 @@ void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correcte
void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAcc, void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAcc,
const Matrix3& D_Rincr_integratedOmega, const Matrix3& D_Rincr_integratedOmega,
const Rot3& incrR, double deltaT) { const Rot3& incrR, double deltaT) {
const Matrix3 dRij = deltaRij(); // expensive const Matrix3 dRij = deltaRij().matrix(); // expensive
const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega();
if (!use2ndOrderIntegration_) { if (!p().use2ndOrderIntegration) {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
} else { } else {
@ -127,19 +112,19 @@ void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAc
} }
void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3& correctedAcc, const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3* correctedAcc,
Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor) { Vector3* correctedOmega) {
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 // Then compensate for sensor-body displacement: we express the quantities
// (originally in the IMU frame) into the body frame // (originally in the IMU frame) into the body frame
if (body_P_sensor) { if (p().body_P_sensor) {
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); Matrix3 body_R_sensor = p().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
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); Matrix3 body_omega_body__cross = skewSymmetric(*correctedOmega);
correctedAcc = body_R_sensor * correctedAcc *correctedAcc = body_R_sensor * (*correctedAcc)
- body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - body_omega_body__cross * body_omega_body__cross * p().body_P_sensor->translation().vector();
// linear acceleration vector in the body frame // linear acceleration vector in the body frame
} }
} }
@ -148,31 +133,27 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
const Vector3& gravity, const Vector3& omegaCoriolis,
const Rot3& deltaRij_biascorrected, const Vector3& deltaPij_biascorrected, const Rot3& deltaRij_biascorrected, const Vector3& deltaPij_biascorrected,
const Vector3& deltaVij_biascorrected, const Vector3& deltaVij_biascorrected) const {
const bool use2ndOrderCoriolis) const {
const double dt = deltaTij(), dt2 = dt * dt; const double dt = deltaTij(), dt2 = dt * dt;
// Rotation
const Matrix3 Ri = pose_i.rotation().matrix(); const Matrix3 Ri = pose_i.rotation().matrix();
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
const Vector3 dR = biascorrectedOmega
- Ri.transpose() * omegaCoriolis * dt; // Coriolis term
// Translation // Rotation, translation, and velocity:
Vector3 dP = Ri * deltaPij_biascorrected + vel_i * dt + 0.5 * gravity * dt2 Vector3 dR = Rot3::Logmap(deltaRij_biascorrected);
- omegaCoriolis.cross(vel_i) * dt2; // Coriolis term - we got rid of the 2 wrt INS paper Vector3 dP = Ri * deltaPij_biascorrected + vel_i * dt + 0.5 * p().gravity * dt2;
Vector3 dV = Ri * deltaVij_biascorrected + p().gravity * dt;
// Velocity if (p().omegaCoriolis) {
Vector3 dV = Ri * deltaVij_biascorrected + gravity * dt const Vector3& omegaCoriolis = *p().omegaCoriolis;
- 2 * omegaCoriolis.cross(vel_i) * dt; // Coriolis term dR -= Ri.transpose() * omegaCoriolis * dt; // Coriolis term
dP -= omegaCoriolis.cross(vel_i) * dt2; // NOTE(luca): we got rid of the 2 wrt INS paper
if (use2ndOrderCoriolis) { dV -= 2 * omegaCoriolis.cross(vel_i) * dt;
Vector3 temp = omegaCoriolis.cross(omegaCoriolis.cross(pose_i.translation().vector())); if (p().use2ndOrderCoriolis) {
dP -= 0.5 * temp * dt2; Vector3 temp = omegaCoriolis.cross(omegaCoriolis.cross(pose_i.translation().vector()));
dV -= temp * dt; dP -= 0.5 * temp * dt2;
dV -= temp * dt;
}
} }
// TODO(frank): pose update below is separate expmap for R,t. Is that kosher? // TODO(frank): pose update below is separate expmap for R,t. Is that kosher?
@ -183,13 +164,12 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
/// Predict state at time j /// Predict state at time j
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
PoseVelocityBias PreintegrationBase::predict( PoseVelocityBias PreintegrationBase::predict(
const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Pose3& pose_i, const Vector3& vel_i,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) const { const imuBias::ConstantBias& bias_i) const {
const imuBias::ConstantBias biasIncr = bias_i - biasHat_; const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
return predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, return predict(
biascorrectedDeltaRij(biasIncr.gyroscope()), pose_i, vel_i, bias_i, biascorrectedDeltaRij(biasIncr.gyroscope()),
biascorrectedDeltaPij(biasIncr), biascorrectedDeltaVij(biasIncr), biascorrectedDeltaPij(biasIncr), biascorrectedDeltaVij(biasIncr));
use2ndOrderCoriolis);
} }
/// Compute errors w.r.t. preintegrated measurements and Jacobians wrpt pose_i, vel_i, bias_i, pose_j, bias_j /// Compute errors w.r.t. preintegrated measurements and Jacobians wrpt pose_i, vel_i, bias_i, pose_j, bias_j
@ -197,9 +177,6 @@ PoseVelocityBias PreintegrationBase::predict(
Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_i,
const Vector3& gravity,
const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 6> H1,
OptionalJacobian<9, 3> H2, OptionalJacobian<9, 3> H2,
OptionalJacobian<9, 6> H3, OptionalJacobian<9, 6> H3,
@ -219,9 +196,9 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasIncr.gyroscope()); const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasIncr.gyroscope());
const Vector3 deltaPij_biascorrected = biascorrectedDeltaPij(biasIncr); const Vector3 deltaPij_biascorrected = biascorrectedDeltaPij(biasIncr);
const Vector3 deltaVij_biascorrected = biascorrectedDeltaVij(biasIncr); const Vector3 deltaVij_biascorrected = biascorrectedDeltaVij(biasIncr);
PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, PoseVelocityBias predictedState_j =
omegaCoriolis, deltaRij_biascorrected, deltaPij_biascorrected, predict(pose_i, vel_i, bias_i, deltaRij_biascorrected,
deltaVij_biascorrected, use2ndOrderCoriolis); deltaPij_biascorrected, deltaVij_biascorrected);
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fp = Ri.transpose() * (pos_j - predictedState_j.pose.translation().vector()); const Vector3 fp = Ri.transpose() * (pos_j - predictedState_j.pose.translation().vector());
@ -241,7 +218,8 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
H5 ? &D_cThetaRij_biasOmegaIncr : 0); H5 ? &D_cThetaRij_biasOmegaIncr : 0);
// Coriolis term, NOTE inconsistent with AHRS, where coriolisHat is *after* integration // Coriolis term, NOTE inconsistent with AHRS, where coriolisHat is *after* integration
const Vector3 coriolis = integrateCoriolis(rot_i, omegaCoriolis); // TODO(frank): move derivatives to predict and do coriolis branching there
const Vector3 coriolis = integrateCoriolis(rot_i);
const Vector3 correctedOmega = biascorrectedOmega - coriolis; const Vector3 correctedOmega = biascorrectedOmega - coriolis;
// Residual rotation error // Residual rotation error
@ -253,17 +231,16 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
const Vector3 fR = Rot3::Logmap(fRrot, H1 || H3 || H5 ? &D_fR_fRrot : 0); const Vector3 fR = Rot3::Logmap(fRrot, H1 || H3 || H5 ? &D_fR_fRrot : 0);
const double dt = deltaTij(), dt2 = dt*dt; const double dt = deltaTij(), dt2 = dt*dt;
Matrix3 Ritranspose_omegaCoriolisHat; Matrix3 RitOmegaCoriolisHat = Z_3x3;
if (H1 || H2) if ((H1 || H2) && p().omegaCoriolis)
Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); RitOmegaCoriolisHat = Ri.transpose() * skewSymmetric(*p().omegaCoriolis);
if (H1) { if (H1) {
const Matrix3 D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); const Matrix3 D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis);
Matrix3 dfPdPi = -I_3x3, dfVdPi = Z_3x3; Matrix3 dfPdPi = -I_3x3, dfVdPi = Z_3x3;
if (use2ndOrderCoriolis) { if (p().use2ndOrderCoriolis) {
// this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri // this is the same as: Ri.transpose() * p().omegaCoriolisHat * p().omegaCoriolisHat * Ri
const Matrix3 temp = Ritranspose_omegaCoriolisHat const Matrix3 temp = RitOmegaCoriolisHat * (-RitOmegaCoriolisHat.transpose());
* (-Ritranspose_omegaCoriolisHat.transpose());
dfPdPi += 0.5 * temp * dt2; dfPdPi += 0.5 * temp * dt2;
dfVdPi += temp * dt; dfVdPi += temp * dt;
} }
@ -278,8 +255,8 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
if (H2) { if (H2) {
(*H2) << (*H2) <<
Z_3x3, // dfR/dVi Z_3x3, // dfR/dVi
-Ri.transpose() * dt + Ritranspose_omegaCoriolisHat * dt2, // dfP/dVi -Ri.transpose() * dt + RitOmegaCoriolisHat * dt2, // dfP/dVi
-Ri.transpose() + 2 * Ritranspose_omegaCoriolisHat * dt; // dfV/dVi -Ri.transpose() + 2 * RitOmegaCoriolisHat * dt; // dfV/dVi
} }
if (H3) { if (H3) {
(*H3) << (*H3) <<
@ -306,19 +283,16 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
return r; return r;
} }
ImuBase::ImuBase() //------------------------------------------------------------------------------
: gravity_(Vector3(0.0, 0.0, 9.81)), PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3& vel_i,
omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), const imuBias::ConstantBias& bias_i, const Vector3& gravity, const Vector3& omegaCoriolis,
body_P_sensor_(boost::none), const bool use2ndOrderCoriolis) {
use2ndOrderCoriolis_(false) { // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
q->gravity = gravity;
q->omegaCoriolis = omegaCoriolis;
q->use2ndOrderCoriolis = use2ndOrderCoriolis;
p_ = q;
return predict(pose_i, vel_i, bias_i);
} }
ImuBase::ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis)
: gravity_(gravity),
omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor),
use2ndOrderCoriolis_(use2ndOrderCoriolis) {
}
} /// namespace gtsam } /// namespace gtsam

171
gtsam/navigation/PreintegrationBase.h
View File

@ -37,11 +37,9 @@ struct PoseVelocityBias {
Vector3 velocity; Vector3 velocity;
imuBias::ConstantBias bias; imuBias::ConstantBias bias;
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias) PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
: pose(_pose), const imuBias::ConstantBias _bias)
velocity(_velocity), : pose(_pose), velocity(_velocity), bias(_bias) {}
bias(_bias) {
}
}; };
/** /**
@ -52,71 +50,85 @@ struct PoseVelocityBias {
*/ */
class PreintegrationBase : public PreintegratedRotation { class PreintegrationBase : public PreintegratedRotation {
const imuBias::ConstantBias biasHat_; ///< Acceleration and gyro bias used for preintegration public:
const bool use2ndOrderIntegration_; ///< Controls the order of integration
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params : PreintegratedRotation::Params {
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
/// (to compensate errors in Euler integration)
bool use2ndOrderIntegration; ///< Controls the order of integration
/// (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
Vector3 gravity; ///< Gravity constant
Params()
: accelerometerCovariance(I_3x3),
integrationCovariance(I_3x3),
use2ndOrderIntegration(false),
use2ndOrderCoriolis(false),
gravity(0, 0, 9.8) {}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
ar & BOOST_SERIALIZATION_NVP(use2ndOrderIntegration);
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
ar & BOOST_SERIALIZATION_NVP(gravity);
}
};
protected:
/// Acceleration and gyro bias used for preintegration
imuBias::ConstantBias biasHat_;
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)
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
const Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements /// Default constructor for serialization
const Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty PreintegrationBase() {}
/// (to compensate errors in Euler integration)
public: public:
/** /**
* Default constructor, initializes the variables in the base class * Constructor, initializes the variables in the base class
* @param bias Current estimate of acceleration and rotation rate biases * @param bias Current estimate of acceleration and rotation rate biases
* @param use2ndOrderIntegration Controls the order of integration * @param p Parameters, typically fixed in a single application
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
*/ */
PreintegrationBase(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, PreintegrationBase(const boost::shared_ptr<const Params>& p,
const Matrix3& measuredOmegaCovariance, const imuBias::ConstantBias& biasHat)
const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration); : PreintegratedRotation(p), biasHat_(biasHat) {
resetIntegration();
}
/// Re-initialize PreintegratedMeasurements /// Re-initialize PreintegratedMeasurements
void resetIntegration(); void resetIntegration();
/// methods to access class variables const Params& p() const { return *boost::static_pointer_cast<const Params>(p_);}
bool use2ndOrderIntegration() const {
return use2ndOrderIntegration_;
}
const Vector3& deltaPij() const {
return deltaPij_;
}
const Vector3& deltaVij() const {
return deltaVij_;
}
const imuBias::ConstantBias& biasHat() const {
return biasHat_;
}
Vector6 biasHatVector() const {
return biasHat_.vector();
} // 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_;
}
const Matrix3& accelerometerCovariance() const { /// getters
return accelerometerCovariance_; const imuBias::ConstantBias& biasHat() const { return biasHat_; }
} const Vector3& deltaPij() const { return deltaPij_; }
const Matrix3& integrationCovariance() const { const Vector3& deltaVij() const { return deltaVij_; }
return integrationCovariance_; const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_; }
} const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_; }
const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_; }
const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_; }
// Exposed for MATLAB
Vector6 biasHatVector() const { return biasHat_.vector(); }
/// print /// print
void print(const std::string& s) const; void print(const std::string& s) const;
@ -134,9 +146,9 @@ class PreintegrationBase : public PreintegratedRotation {
double deltaT); double deltaT);
void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
const Vector3& measuredOmega, Vector3& correctedAcc, const Vector3& measuredOmega,
Vector3& correctedOmega, Vector3* correctedAcc,
boost::optional<const Pose3&> body_P_sensor); Vector3* correctedOmega);
Vector3 biascorrectedDeltaPij(const imuBias::ConstantBias& biasIncr) const { Vector3 biascorrectedDeltaPij(const imuBias::ConstantBias& biasIncr) const {
return deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer() return deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer()
@ -150,33 +162,36 @@ class PreintegrationBase : public PreintegratedRotation {
/// Predict state at time j, with bias-corrected quantities given /// Predict state at time j, with bias-corrected quantities given
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& gravity, const Vector3& omegaCoriolis, const imuBias::ConstantBias& bias_i,
const Rot3& deltaRij_biascorrected, const Rot3& deltaRij_biascorrected,
const Vector3& deltaPij_biascorrected, const Vector3& deltaVij_biascorrected, const Vector3& deltaPij_biascorrected,
const bool use2ndOrderCoriolis = false) const; const Vector3& deltaVij_biascorrected) const;
/// Predict state at time j /// Predict state at time j
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& gravity, const imuBias::ConstantBias& bias_i) const;
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const;
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j,
const Vector3& vel_j, const imuBias::ConstantBias& bias_i, const Vector3& vel_j, const imuBias::ConstantBias& bias_i,
const Vector3& gravity, const Vector3& omegaCoriolis, OptionalJacobian<9, 6> H1 = boost::none,
const bool use2ndOrderCoriolis, OptionalJacobian<9, 6> H1 =
boost::none,
OptionalJacobian<9, 3> H2 = boost::none, OptionalJacobian<9, 3> H2 = boost::none,
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 6> H3 = boost::none,
OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 3> H4 = boost::none,
OptionalJacobian<9, 6> H5 = boost::none) const; OptionalJacobian<9, 6> H5 = boost::none) const;
/// @deprecated predict
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);
private: private:
/** Serialization function */ /** Serialization function */
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_BASE_OBJECT_NVP(PreintegratedRotation);
ar & BOOST_SERIALIZATION_NVP(p_);
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_);
@ -187,32 +202,4 @@ class PreintegrationBase : public PreintegratedRotation {
} }
}; };
class ImuBase {
protected:
Vector3 gravity_;
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
public:
/// Default constructor, with decent gravity and zero coriolis
ImuBase();
/// Fully fledge constructor
ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
const Vector3& gravity() const {
return gravity_;
}
const Vector3& omegaCoriolis() const {
return omegaCoriolis_;
}
};
} /// namespace gtsam } /// namespace gtsam

2
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -50,7 +50,7 @@ Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i,
AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const Vector3& bias, const list<Vector3>& measuredOmegas, const Vector3& bias, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs, const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3()) { const Vector3& initialRotationRate = Vector3::Zero()) {
AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity()); AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity());
list<Vector3>::const_iterator itOmega = measuredOmegas.begin(); list<Vector3>::const_iterator itOmega = measuredOmegas.begin();

15
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -146,15 +146,9 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT( EXPECT(assert_equal(Vector(expected1.deltaPij()), actual1.deltaPij(), tol));
assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), EXPECT(assert_equal(Vector(expected1.deltaVij()), actual1.deltaVij(), tol));
tol)); EXPECT(assert_equal(expected1.deltaRij(), actual1.deltaRij(), tol));
EXPECT(
assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()),
tol));
EXPECT(
assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()),
tol));
DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
} }
@ -373,7 +367,6 @@ TEST(CombinedImuFactor, PredictRotation) {
TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
// Linearization point // Linearization point
imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3();
// Measurements // Measurements
list<Vector3> measuredAccs, measuredOmegas; list<Vector3> measuredAccs, measuredOmegas;
@ -408,7 +401,7 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
Matrix Factual, Gactual; Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
newDeltaT, body_P_sensor, Factual, Gactual); newDeltaT, Factual, Gactual);
bool use2ndOrderIntegration = false; bool use2ndOrderIntegration = false;

32
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -90,11 +90,11 @@ const Matrix3 kIntegrationErrorCovariance = intNoiseVar * Matrix3::Identity();
// Auxiliary functions to test preintegrated Jacobians // Auxiliary functions to test preintegrated Jacobians
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
/* ************************************************************************* */ /* ************************************************************************* */
ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( PreintegratedImuMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs, const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs, const list<Vector3>& measuredOmegas, const list<double>& deltaTs,
const bool use2ndOrderIntegration = false) { const bool use2ndOrderIntegration = false) {
ImuFactor::PreintegratedMeasurements result(bias, kMeasuredAccCovariance, PreintegratedImuMeasurements result(bias, kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
use2ndOrderIntegration); use2ndOrderIntegration);
@ -159,7 +159,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
bool use2ndOrderIntegration = true; bool use2ndOrderIntegration = true;
// Actual preintegrated values // Actual preintegrated values
ImuFactor::PreintegratedMeasurements actual1(bias, kMeasuredAccCovariance, PreintegratedImuMeasurements actual1(bias, kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
use2ndOrderIntegration); use2ndOrderIntegration);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -180,7 +180,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
double expectedDeltaT2(1); double expectedDeltaT2(1);
// Actual preintegrated values // Actual preintegrated values
ImuFactor::PreintegratedMeasurements actual2 = actual1; PreintegratedImuMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT( EXPECT(
@ -211,7 +211,7 @@ double deltaT = 1.0;
TEST(ImuFactor, ErrorAndJacobians) { TEST(ImuFactor, ErrorAndJacobians) {
using namespace common; using namespace common;
bool use2ndOrderIntegration = true; bool use2ndOrderIntegration = true;
ImuFactor::PreintegratedMeasurements pre_int_data(bias, PreintegratedImuMeasurements pre_int_data(bias,
kMeasuredAccCovariance, kMeasuredOmegaCovariance, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, use2ndOrderIntegration); kIntegrationErrorCovariance, use2ndOrderIntegration);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -290,7 +290,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
+ Vector3(0.2, 0.0, 0.0); + Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0; double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data( PreintegratedImuMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance); kIntegrationErrorCovariance);
@ -330,7 +330,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
+ Vector3(0.2, 0.0, 0.0); + Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0; double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data( PreintegratedImuMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance); kIntegrationErrorCovariance);
@ -452,7 +452,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
} }
// Actual preintegrated values // Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated = PreintegratedImuMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs); deltaTs);
@ -495,7 +495,6 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
// Linearization point // Linearization point
imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
// Measurements // Measurements
list<Vector3> measuredAccs, measuredOmegas; list<Vector3> measuredAccs, measuredOmegas;
@ -514,7 +513,7 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
} }
bool use2ndOrderIntegration = false; bool use2ndOrderIntegration = false;
// Actual preintegrated values // Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated = PreintegratedImuMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs, use2ndOrderIntegration); deltaTs, use2ndOrderIntegration);
@ -531,7 +530,7 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
Matrix Factual, Gactual; Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
newDeltaT, body_P_sensor, Factual, Gactual); newDeltaT, Factual, Gactual);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F // COMPUTE NUMERICAL DERIVATIVES FOR F
@ -619,7 +618,6 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
// Linearization point // Linearization point
imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
// Measurements // Measurements
list<Vector3> measuredAccs, measuredOmegas; list<Vector3> measuredAccs, measuredOmegas;
@ -638,7 +636,7 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
} }
bool use2ndOrderIntegration = true; bool use2ndOrderIntegration = true;
// Actual preintegrated values // Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated = PreintegratedImuMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs, use2ndOrderIntegration); deltaTs, use2ndOrderIntegration);
@ -655,7 +653,7 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
Matrix Factual, Gactual; Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
newDeltaT, body_P_sensor, Factual, Gactual); newDeltaT, Factual, Gactual);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F // COMPUTE NUMERICAL DERIVATIVES FOR F
@ -760,7 +758,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
Point3(1, 0, 0)); Point3(1, 0, 0));
ImuFactor::PreintegratedMeasurements pre_int_data( PreintegratedImuMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance); kIntegrationErrorCovariance);
@ -797,7 +795,7 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
Matrix I6x6(6, 6); Matrix I6x6(6, 6);
I6x6 = Matrix::Identity(6, 6); I6x6 = Matrix::Identity(6, 6);
ImuFactor::PreintegratedMeasurements pre_int_data( PreintegratedImuMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, true); kIntegrationErrorCovariance, true);
@ -835,7 +833,7 @@ TEST(ImuFactor, PredictRotation) {
Matrix I6x6(6, 6); Matrix I6x6(6, 6);
I6x6 = Matrix::Identity(6, 6); I6x6 = Matrix::Identity(6, 6);
ImuFactor::PreintegratedMeasurements pre_int_data( PreintegratedImuMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
kMeasuredAccCovariance, kMeasuredOmegaCovariance, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, true); kIntegrationErrorCovariance, true);