matchesParamsWith, a few new constructors, and Doxygen streamlining
Frank Dellaert
parent
f6200f4a2b
commit
a69c43bf43
|
|
@ -113,6 +113,9 @@ public:
|
|||
friend class CombinedImuFactor;
|
||||
|
||||
public:
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Default constructor, initializes the class with no measurements
|
||||
* @param bias Current estimate of acceleration and rotation rate biases
|
||||
|
|
@ -123,18 +126,32 @@ public:
|
|||
preintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
|
||||
/// @}
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "Preintegrated Measurements:") const;
|
||||
|
||||
/// equals
|
||||
bool equals(const PreintegratedCombinedMeasurements& expected,
|
||||
double tol = 1e-9) const;
|
||||
/// @name Basic utilities
|
||||
/// @{
|
||||
|
||||
/// Re-initialize PreintegratedCombinedMeasurements
|
||||
void resetIntegration();
|
||||
|
||||
/// const reference to params, shadows definition in base class
|
||||
Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
|
||||
/// @}
|
||||
|
||||
/// @name Access instance variables
|
||||
/// @{
|
||||
Matrix preintMeasCov() const { return preintMeasCov_; }
|
||||
/// @}
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
void print(const std::string& s = "Preintegrated Measurements:") const;
|
||||
bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const;
|
||||
/// @}
|
||||
|
||||
/// @name Main functionality
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Add a single IMU measurement to the preintegration.
|
||||
* @param measuredAcc Measured acceleration (in body frame, as given by the
|
||||
|
|
@ -147,8 +164,10 @@ public:
|
|||
void integrateMeasurement(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, double deltaT);
|
||||
|
||||
/// methods to access class variables
|
||||
Matrix preintMeasCov() const { return preintMeasCov_; }
|
||||
/// @}
|
||||
|
||||
/// @name Deprecated
|
||||
/// @{
|
||||
|
||||
/// deprecated constructor
|
||||
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
|
||||
|
|
@ -159,6 +178,8 @@ public:
|
|||
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
|
||||
const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true);
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -81,11 +81,21 @@ public:
|
|||
* @param p Parameters, typically fixed in a single application
|
||||
*/
|
||||
PreintegratedImuMeasurements(const boost::shared_ptr<Params>& p,
|
||||
const imuBias::ConstantBias& biasHat) :
|
||||
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
|
||||
PreintegrationBase(p, biasHat) {
|
||||
preintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct preintegrated directly from members: base class and preintMeasCov
|
||||
* @param base PreintegrationBase instance
|
||||
* @param preintMeasCov Covariance matrix used in noise model.
|
||||
*/
|
||||
PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov)
|
||||
: PreintegrationBase(base),
|
||||
preintMeasCov_(preintMeasCov) {
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "Preintegrated Measurements:") const;
|
||||
|
||||
|
|
@ -167,7 +177,7 @@ public:
|
|||
#endif
|
||||
|
||||
/** Default constructor - only use for serialization */
|
||||
ImuFactor();
|
||||
ImuFactor() {}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
|
|
@ -241,4 +251,10 @@ private:
|
|||
};
|
||||
// class ImuFactor
|
||||
|
||||
template <>
|
||||
struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
|
||||
|
||||
template <>
|
||||
struct traits<ImuFactor> : public Testable<ImuFactor> {};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -42,14 +42,15 @@ void PreintegratedRotation::resetIntegration() {
|
|||
}
|
||||
|
||||
void PreintegratedRotation::print(const string& s) const {
|
||||
cout << s << endl;
|
||||
cout << s;
|
||||
cout << " deltaTij [" << deltaTij_ << "]" << endl;
|
||||
cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
|
||||
}
|
||||
|
||||
bool PreintegratedRotation::equals(const PreintegratedRotation& other,
|
||||
double tol) const {
|
||||
return deltaRij_.equals(other.deltaRij_, tol)
|
||||
return this->matchesParamsWith(other)
|
||||
&& deltaRij_.equals(other.deltaRij_, tol)
|
||||
&& fabs(deltaTij_ - other.deltaTij_) < tol
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
|
||||
}
|
||||
|
|
@ -75,12 +76,16 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
|
|||
return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
|
||||
}
|
||||
|
||||
void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
|
||||
const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
|
||||
Matrix3* F) {
|
||||
void PreintegratedRotation::integrateMeasurement(
|
||||
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, OptionalJacobian<3, 3> F) {
|
||||
Matrix3 D_incrR_integratedOmega;
|
||||
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, D_incrR_integratedOmega);
|
||||
|
||||
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
|
||||
D_incrR_integratedOmega);
|
||||
// If asked, pass first derivative as well
|
||||
if (optional_D_incrR_integratedOmega) {
|
||||
*optional_D_incrR_integratedOmega << D_incrR_integratedOmega;
|
||||
}
|
||||
|
||||
// Update deltaTij and rotation
|
||||
deltaTij_ += deltaT;
|
||||
|
|
@ -88,8 +93,7 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
|
|||
|
||||
// Update Jacobian
|
||||
const Matrix3 incrRt = incrR.transpose();
|
||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
|
||||
- *D_incrR_integratedOmega * deltaT;
|
||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT;
|
||||
}
|
||||
|
||||
Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -32,56 +32,74 @@ namespace gtsam {
|
|||
* It includes the definitions of the preintegrated rotation.
|
||||
*/
|
||||
class PreintegratedRotation {
|
||||
public:
|
||||
|
||||
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
|
||||
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) {
|
||||
}
|
||||
Params() : gyroscopeCovariance(I_3x3) {}
|
||||
|
||||
virtual void print(const std::string& s) const;
|
||||
|
||||
private:
|
||||
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);
|
||||
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);
|
||||
}
|
||||
};
|
||||
|
||||
protected:
|
||||
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
|
||||
|
||||
protected:
|
||||
/// Parameters
|
||||
boost::shared_ptr<Params> p_;
|
||||
|
||||
/// Default constructor for serialization
|
||||
PreintegratedRotation() {
|
||||
}
|
||||
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
|
||||
|
||||
/// Default constructor for serialization
|
||||
PreintegratedRotation() {}
|
||||
|
||||
public:
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
public:
|
||||
/// Default constructor, resets integration to zero
|
||||
explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) :
|
||||
p_(p) {
|
||||
explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) : p_(p) {
|
||||
resetIntegration();
|
||||
}
|
||||
|
||||
/// Explicit initialization of all class members
|
||||
PreintegratedRotation(const boost::shared_ptr<Params>& p,
|
||||
double deltaTij, const Rot3& deltaRij,
|
||||
const Matrix3& delRdelBiasOmega)
|
||||
: p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_(delRdelBiasOmega) {}
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Basic utilities
|
||||
/// @{
|
||||
|
||||
/// Re-initialize PreintegratedMeasurements
|
||||
void resetIntegration();
|
||||
|
||||
/// check parameters equality: checks whether shared pointer points to same Params object.
|
||||
bool matchesParamsWith(const PreintegratedRotation& other) const {
|
||||
return p_ == other.p_;
|
||||
}
|
||||
/// @}
|
||||
|
||||
/// @name Access instance variables
|
||||
/// @{
|
||||
const boost::shared_ptr<Params>& params() const {
|
||||
return p_;
|
||||
}
|
||||
const double& deltaTij() const {
|
||||
return deltaTij_;
|
||||
}
|
||||
|
|
@ -95,41 +113,47 @@ public:
|
|||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
void print(const std::string& s) const;
|
||||
bool equals(const PreintegratedRotation& other, double tol) const;
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Main functionality
|
||||
/// @{
|
||||
|
||||
/// Take the gyro measurement, correct it using the (constant) bias estimate
|
||||
/// and possibly the sensor pose, and then integrate it forward in time to yield
|
||||
/// an incremental rotation.
|
||||
Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat,
|
||||
double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
|
||||
Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
|
||||
|
||||
/// Calculate an incremental rotation given the gyro measurement and a time interval,
|
||||
/// and update both deltaTij_ and deltaRij_.
|
||||
void integrateMeasurement(const Vector3& measuredOmega,
|
||||
const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
|
||||
Matrix3* F);
|
||||
void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
|
||||
OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none,
|
||||
OptionalJacobian<3, 3> F = boost::none);
|
||||
|
||||
/// Return a bias corrected version of the integrated rotation, with optional Jacobian
|
||||
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
|
||||
OptionalJacobian<3, 3> H = boost::none) const;
|
||||
OptionalJacobian<3, 3> H = boost::none) const;
|
||||
|
||||
/// Integrate coriolis correction in body frame rot_i
|
||||
Vector3 integrateCoriolis(const Rot3& rot_i) const;
|
||||
|
||||
private:
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // NOLINT
|
||||
ar & BOOST_SERIALIZATION_NVP(p_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NOLINT
|
||||
ar& BOOST_SERIALIZATION_NVP(p_);
|
||||
ar& BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||
ar& BOOST_SERIALIZATION_NVP(deltaRij_);
|
||||
ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
template <>
|
||||
struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -64,7 +64,8 @@ void PreintegrationBase::print(const string& s) const {
|
|||
//------------------------------------------------------------------------------
|
||||
bool PreintegrationBase::equals(const PreintegrationBase& other,
|
||||
double tol) const {
|
||||
return fabs(deltaTij_ - other.deltaTij_) < tol
|
||||
return this->matchesParamsWith(other)
|
||||
&& fabs(deltaTij_ - other.deltaTij_) < tol
|
||||
&& deltaXij_.equals(other.deltaXij_, tol)
|
||||
&& biasHat_.equals(other.biasHat_, tol)
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -100,7 +100,14 @@ public:
|
|||
|
||||
protected:
|
||||
|
||||
double deltaTij_; ///< Time interval from i to j
|
||||
/// Parameters
|
||||
boost::shared_ptr<Params> p_;
|
||||
|
||||
/// Acceleration and gyro bias used for preintegration
|
||||
imuBias::ConstantBias biasHat_;
|
||||
|
||||
/// Time interval from i to j
|
||||
double deltaTij_;
|
||||
|
||||
/**
|
||||
* Preintegrated navigation state, from frame i to frame j
|
||||
|
|
@ -109,12 +116,6 @@ protected:
|
|||
*/
|
||||
NavState deltaXij_;
|
||||
|
||||
/// Parameters
|
||||
boost::shared_ptr<Params> p_;
|
||||
|
||||
/// Acceleration and gyro bias used for preintegration
|
||||
imuBias::ConstantBias biasHat_;
|
||||
|
||||
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
||||
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
|
||||
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
|
||||
|
|
@ -127,20 +128,53 @@ protected:
|
|||
|
||||
public:
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Constructor, initializes the variables in the base class
|
||||
* @param bias Current estimate of acceleration and rotation rate biases
|
||||
* @param p Parameters, typically fixed in a single application
|
||||
* @param bias Current estimate of acceleration and rotation rate biases
|
||||
*/
|
||||
PreintegrationBase(const boost::shared_ptr<Params>& p,
|
||||
const imuBias::ConstantBias& biasHat) :
|
||||
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
|
||||
p_(p), biasHat_(biasHat) {
|
||||
resetIntegration();
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor which takes in all members for generic construction
|
||||
*/
|
||||
PreintegrationBase(const boost::shared_ptr<Params>& p, const imuBias::ConstantBias& biasHat,
|
||||
double deltaTij, const NavState& deltaXij, const Matrix3& delPdelBiasAcc,
|
||||
const Matrix3& delPdelBiasOmega, const Matrix3& delVdelBiasAcc,
|
||||
const Matrix3& delVdelBiasOmega)
|
||||
: p_(p),
|
||||
biasHat_(biasHat),
|
||||
deltaTij_(deltaTij),
|
||||
deltaXij_(deltaXij),
|
||||
delPdelBiasAcc_(delPdelBiasAcc),
|
||||
delPdelBiasOmega_(delPdelBiasOmega),
|
||||
delVdelBiasAcc_(delVdelBiasAcc),
|
||||
delVdelBiasOmega_(delVdelBiasOmega) {}
|
||||
/// @}
|
||||
|
||||
/// @name Basic utilities
|
||||
/// @{
|
||||
/// Re-initialize PreintegratedMeasurements
|
||||
void resetIntegration();
|
||||
|
||||
/// check parameters equality: checks whether shared pointer points to same Params object.
|
||||
bool matchesParamsWith(const PreintegrationBase& other) const {
|
||||
return p_ == other.p_;
|
||||
}
|
||||
|
||||
/// shared pointer to params
|
||||
const boost::shared_ptr<Params>& params() const {
|
||||
return p_;
|
||||
}
|
||||
|
||||
/// const reference to params
|
||||
const Params& p() const {
|
||||
return *boost::static_pointer_cast<Params>(p_);
|
||||
}
|
||||
|
|
@ -148,8 +182,10 @@ public:
|
|||
void set_body_P_sensor(const Pose3& body_P_sensor) {
|
||||
p_->body_P_sensor = body_P_sensor;
|
||||
}
|
||||
/// @}
|
||||
|
||||
/// getters
|
||||
/// @name Instance variables access
|
||||
/// @{
|
||||
const NavState& deltaXij() const {
|
||||
return deltaXij_;
|
||||
}
|
||||
|
|
@ -183,17 +219,20 @@ public:
|
|||
const Matrix3& delVdelBiasOmega() const {
|
||||
return delVdelBiasOmega_;
|
||||
}
|
||||
|
||||
// Exposed for MATLAB
|
||||
Vector6 biasHatVector() const {
|
||||
return biasHat_.vector();
|
||||
}
|
||||
/// @}
|
||||
|
||||
/// print
|
||||
/// @name Testable
|
||||
/// @{
|
||||
void print(const std::string& s) const;
|
||||
|
||||
/// check equality
|
||||
bool equals(const PreintegrationBase& other, double tol) const;
|
||||
/// @}
|
||||
|
||||
/// @name Main functionality
|
||||
/// @{
|
||||
|
||||
/// Subtract estimate and correct for sensor pose
|
||||
/// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
|
||||
|
|
@ -236,11 +275,18 @@ public:
|
|||
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
|
||||
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Deprecated
|
||||
/// @{
|
||||
|
||||
/// @deprecated predict
|
||||
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
|
||||
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
|
|||
Loading…
Reference in New Issue