split of measurement covariance into diagonal blocks.
Luca
parent
d9a7f516ef
commit
c4bd02c3fa
3
gtsam.h
3
gtsam.h
|
|
@ -2417,7 +2417,6 @@ class ImuFactorPreintegratedMeasurements {
|
|||
Matrix delVdelBiasOmega() const;
|
||||
Matrix delRdelBiasOmega() const;
|
||||
Matrix preintMeasCov() const;
|
||||
Matrix measurementCovariance() const;
|
||||
|
||||
// Standard Interface
|
||||
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
|
||||
|
|
@ -2473,7 +2472,6 @@ class CombinedImuFactorPreintegratedMeasurements {
|
|||
Matrix delVdelBiasOmega() const;
|
||||
Matrix delRdelBiasOmega() const;
|
||||
Matrix preintMeasCov() const;
|
||||
Matrix measurementCovariance() const;
|
||||
|
||||
// Standard Interface
|
||||
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
|
||||
|
|
@ -2501,7 +2499,6 @@ class AHRSFactorPreintegratedMeasurements {
|
|||
bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
|
||||
|
||||
// get Data
|
||||
Matrix measurementCovariance() const;
|
||||
Matrix deltaRij() const;
|
||||
double deltaTij() const;
|
||||
Vector biasHat() const;
|
||||
|
|
|
|||
|
|
@ -29,15 +29,15 @@ namespace gtsam {
|
|||
//------------------------------------------------------------------------------
|
||||
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements(
|
||||
const Vector3& bias, const Matrix3& measuredOmegaCovariance) :
|
||||
biasHat_(bias) {
|
||||
measurementCovariance_ << measuredOmegaCovariance;
|
||||
PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias)
|
||||
{
|
||||
preintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
|
||||
biasHat_(Vector3()) {
|
||||
measurementCovariance_.setZero();
|
||||
PreintegratedRotation(I_3x3), biasHat_(Vector3())
|
||||
{
|
||||
preintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
|
|
@ -45,7 +45,6 @@ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
|
|||
void AHRSFactor::PreintegratedMeasurements::print(const string& s) const {
|
||||
PreintegratedRotation::print(s);
|
||||
cout << "biasHat [" << biasHat_.transpose() << "]" << endl;
|
||||
cout << " measurementCovariance [" << measurementCovariance_ << " ]" << endl;
|
||||
cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl;
|
||||
}
|
||||
|
||||
|
|
@ -53,9 +52,7 @@ void AHRSFactor::PreintegratedMeasurements::print(const string& s) const {
|
|||
bool AHRSFactor::PreintegratedMeasurements::equals(
|
||||
const PreintegratedMeasurements& other, double tol) const {
|
||||
return PreintegratedRotation::equals(other, tol)
|
||||
&& equal_with_abs_tol(biasHat_, other.biasHat_, tol)
|
||||
&& equal_with_abs_tol(measurementCovariance_,
|
||||
other.measurementCovariance_, tol);
|
||||
&& equal_with_abs_tol(biasHat_, other.biasHat_, tol);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -96,7 +93,7 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
|
|||
// first order uncertainty propagation
|
||||
// the deltaT allows to pass from continuous time noise to discrete time noise
|
||||
preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose()
|
||||
+ measurementCovariance_ * deltaT;
|
||||
+ gyroscopeCovariance() * deltaT;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -42,8 +42,6 @@ public:
|
|||
|
||||
protected:
|
||||
Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer
|
||||
Matrix3 measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3)
|
||||
|
||||
Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
|
||||
|
||||
public:
|
||||
|
|
@ -59,9 +57,6 @@ public:
|
|||
PreintegratedMeasurements(const Vector3& bias,
|
||||
const Matrix3& measuredOmegaCovariance);
|
||||
|
||||
const Matrix3& measurementCovariance() const {
|
||||
return measurementCovariance_;
|
||||
}
|
||||
Vector3 biasHat() const {
|
||||
return biasHat_;
|
||||
}
|
||||
|
|
@ -105,7 +100,6 @@ public:
|
|||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
|
||||
ar & BOOST_SERIALIZATION_NVP(biasHat_);
|
||||
ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -36,28 +36,28 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu
|
|||
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
|
||||
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
|
||||
const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) :
|
||||
PreintegrationBase(bias, use2ndOrderIntegration)
|
||||
PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
|
||||
integrationErrorCovariance, use2ndOrderIntegration),
|
||||
biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance),
|
||||
biasAccOmegaInit_(biasAccOmegaInit)
|
||||
{
|
||||
measurementCovariance_.setZero();
|
||||
measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
|
||||
measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
|
||||
measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
|
||||
measurementCovariance_.block<3,3>(9,9) = biasAccCovariance;
|
||||
measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance;
|
||||
measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit;
|
||||
preintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{
|
||||
PreintegrationBase::print(s);
|
||||
cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl;
|
||||
cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl;
|
||||
cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl;
|
||||
cout << " biasAccOmegaInit [ " << biasAccOmegaInit_ << " ]" << endl;
|
||||
cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{
|
||||
return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
|
||||
return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, 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);
|
||||
}
|
||||
|
|
@ -120,17 +120,17 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
|
|||
Matrix G_measCov_Gt = Matrix::Zero(15,15);
|
||||
|
||||
// BLOCK DIAGONAL TERMS
|
||||
G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0);
|
||||
G_measCov_Gt.block<3,3>(0,0) = deltaT * integrationCovariance();
|
||||
G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) *
|
||||
(measurementCovariance_.block<3,3>(3,3) + measurementCovariance_.block<3,3>(15,15) ) *
|
||||
(accelerometerCovariance() + biasAccOmegaInit_.block<3,3>(0,0) ) *
|
||||
(H_vel_biasacc.transpose());
|
||||
G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) *
|
||||
(measurementCovariance_.block<3,3>(6,6) + measurementCovariance_.block<3,3>(18,18) ) *
|
||||
(gyroscopeCovariance() + biasAccOmegaInit_.block<3,3>(3,3) ) *
|
||||
(H_angles_biasomega.transpose());
|
||||
G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * measurementCovariance_.block<3,3>(9,9);
|
||||
G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * measurementCovariance_.block<3,3>(12,12);
|
||||
G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * biasAccCovariance_;
|
||||
G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * biasOmegaCovariance_;
|
||||
// OFF BLOCK DIAGONAL TERMS
|
||||
Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) * H_angles_biasomega.transpose();
|
||||
Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3,3>(3,0) * H_angles_biasomega.transpose();
|
||||
G_measCov_Gt.block<3,3>(3,6) = block23;
|
||||
G_measCov_Gt.block<3,3>(6,3) = block23.transpose();
|
||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
|
||||
|
|
|
|||
|
|
@ -82,8 +82,9 @@ public:
|
|||
|
||||
protected:
|
||||
|
||||
Eigen::Matrix<double,21,21> measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector
|
||||
///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
|
||||
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]
|
||||
|
|
@ -130,7 +131,6 @@ public:
|
|||
boost::optional<Matrix&> F_test = boost::none, boost::optional<Matrix&> G_test = boost::none);
|
||||
|
||||
/// methods to access class variables
|
||||
Matrix measurementCovariance() const {return measurementCovariance_;}
|
||||
Matrix preintMeasCov() const { return preintMeasCov_;}
|
||||
|
||||
private:
|
||||
|
|
@ -140,7 +140,6 @@ public:
|
|||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
|
||||
ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
|
||||
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
|
||||
}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -35,26 +35,22 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
|
|||
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
|
||||
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
|
||||
const bool use2ndOrderIntegration) :
|
||||
PreintegrationBase(bias, use2ndOrderIntegration)
|
||||
PreintegrationBase(bias,
|
||||
measuredAccCovariance, measuredOmegaCovariance,
|
||||
integrationErrorCovariance, use2ndOrderIntegration)
|
||||
{
|
||||
measurementCovariance_.setZero();
|
||||
measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
|
||||
measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
|
||||
measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
|
||||
preintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
|
||||
PreintegrationBase::print(s);
|
||||
cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl;
|
||||
cout << " preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const {
|
||||
return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
|
||||
&& equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
|
||||
return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
|
||||
&& PreintegrationBase::equals(expected, tol);
|
||||
}
|
||||
|
||||
|
|
@ -94,9 +90,9 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
|
|||
// NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise,
|
||||
// as G and measurementCovariance are blockdiagonal matrices
|
||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose();
|
||||
preintMeasCov_.block<3,3>(0,0) += measurementCovariance_.block<3,3>(0,0) * deltaT;
|
||||
preintMeasCov_.block<3,3>(3,3) += R_i * measurementCovariance_.block<3,3>(3,3) * R_i.transpose() * deltaT;
|
||||
preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * measurementCovariance_.block<3,3>(6,6) * D_Rincr_integratedOmega.transpose() * deltaT;
|
||||
preintMeasCov_.block<3,3>(0,0) += integrationCovariance() * deltaT;
|
||||
preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT;
|
||||
preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT;
|
||||
|
||||
// F_test and G_test are given as output for testing purposes and are not needed by the factor
|
||||
if(F_test){ // This in only for testing
|
||||
|
|
|
|||
|
|
@ -75,9 +75,6 @@ public:
|
|||
|
||||
protected:
|
||||
|
||||
Eigen::Matrix<double,9,9> measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of
|
||||
///< the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
|
||||
|
||||
Eigen::Matrix<double,9,9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
|
||||
///< (first-order propagation from *measurementCovariance*).
|
||||
|
||||
|
|
@ -118,7 +115,6 @@ public:
|
|||
OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none);
|
||||
|
||||
/// methods to access class variables
|
||||
Matrix measurementCovariance() const {return measurementCovariance_;}
|
||||
Matrix preintMeasCov() const { return preintMeasCov_;}
|
||||
|
||||
private:
|
||||
|
|
@ -128,7 +124,6 @@ public:
|
|||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
|
||||
ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
|
||||
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
|
||||
}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -37,33 +37,39 @@ class PreintegratedRotation {
|
|||
|
||||
/// Jacobian of preintegrated rotation w.r.t. angular rate bias
|
||||
Matrix3 delRdelBiasOmega_;
|
||||
Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* Default constructor, initializes the variables in the base class
|
||||
*/
|
||||
PreintegratedRotation() :
|
||||
PreintegratedRotation(const Matrix3& measuredOmegaCovariance) :
|
||||
deltaRij_(Rot3()), deltaTij_(0.0),
|
||||
delRdelBiasOmega_(Z_3x3) {}
|
||||
delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(measuredOmegaCovariance) {}
|
||||
|
||||
/// methods to access class variables
|
||||
Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive
|
||||
Vector3 thetaRij(boost::optional<Matrix3&> H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive
|
||||
const double& deltaTij() const{return deltaTij_;}
|
||||
const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;}
|
||||
const Matrix3& gyroscopeCovariance() const { return gyroscopeCovariance_;}
|
||||
|
||||
/// Needed for testable
|
||||
void print(const std::string& s) const {
|
||||
std::cout << s << std::endl;
|
||||
std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl;
|
||||
deltaRij_.print(" deltaRij ");
|
||||
std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl;
|
||||
std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" << std::endl;
|
||||
}
|
||||
|
||||
/// Needed for testable
|
||||
bool equals(const PreintegratedRotation& expected, double tol) const {
|
||||
return deltaRij_.equals(expected.deltaRij_, tol)
|
||||
&& fabs(deltaTij_ - expected.deltaTij_) < tol
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(gyroscopeCovariance_, expected.gyroscopeCovariance_, tol);
|
||||
}
|
||||
|
||||
/// Re-initialize PreintegratedMeasurements
|
||||
|
|
|
|||
|
|
@ -59,6 +59,10 @@ class PreintegrationBase : public PreintegratedRotation {
|
|||
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
|
||||
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
|
||||
|
||||
Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements
|
||||
Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty
|
||||
/// (to compensate errors in Euler integration)
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
|
|
@ -67,11 +71,16 @@ public:
|
|||
* @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)
|
||||
*/
|
||||
PreintegrationBase(const imuBias::ConstantBias& bias, const bool use2ndOrderIntegration) :
|
||||
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) {}
|
||||
delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
|
||||
accelerometerCovariance_(measuredAccCovariance),
|
||||
integrationCovariance_(integrationErrorCovariance) {}
|
||||
|
||||
/// methods to access class variables
|
||||
const Vector3& deltaPij() const {return deltaPij_;}
|
||||
|
|
@ -83,9 +92,14 @@ public:
|
|||
const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;}
|
||||
const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;}
|
||||
|
||||
const Matrix3& accelerometerCovariance() const { return accelerometerCovariance_;}
|
||||
const Matrix3& integrationCovariance() const { return integrationCovariance_;}
|
||||
|
||||
/// Needed for testable
|
||||
void print(const std::string& s) const {
|
||||
PreintegratedRotation::print(s);
|
||||
std::cout << " accelerometerCovariance [ " << accelerometerCovariance_ << " ]" << std::endl;
|
||||
std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]" << std::endl;
|
||||
std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
|
||||
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
|
||||
biasHat_.print(" biasHat");
|
||||
|
|
@ -100,7 +114,9 @@ public:
|
|||
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol);
|
||||
&& equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(accelerometerCovariance_, expected.accelerometerCovariance_, tol)
|
||||
&& equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol);
|
||||
}
|
||||
|
||||
/// Re-initialize PreintegratedMeasurements
|
||||
|
|
|
|||
Loading…
Reference in New Issue