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

Changed access specifier of preintegrated measurement variables to protected.

release/4.3a0
krunalchande 2014-11-21 16:12:33 -05:00
parent f96e7ef32f
commit ce5f7911c5
6 changed files with 193 additions and 209 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
gtsam/navigation/AHRSFactor.h
View File

@ -37,28 +37,29 @@ public:
/** CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) /** CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates)
* and the corresponding covariance matrix. The measurements are then used to build the Preintegrated AHRS factor*/ * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated AHRS factor*/
class PreintegratedMeasurements { class PreintegratedMeasurements {
public: friend class AHRSFactor;
protected:
imuBias::ConstantBias biasHat_;///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer imuBias::ConstantBias biasHat_;///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer
Matrix measurementCovariance_;///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3) Matrix measurementCovariance_;///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3)
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j double deltaTij_; ///< Time interval from i to j
Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
public:
/** Default constructor, initialize with no measurements */ /** Default constructor, initialize with no measurements */
PreintegratedMeasurements( PreintegratedMeasurements(
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
const Matrix3& measuredOmegaCovariance ///< Covariance matrix of measured angular rate const Matrix3& measuredOmegaCovariance ///< Covariance matrix of measured angular rate
) : biasHat_(bias), measurementCovariance_(3,3), deltaTij_(0.0), ) : biasHat_(bias), measurementCovariance_(3,3), deltaTij_(0.0),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(3,3) { delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(3,3) {
measurementCovariance_ <<measuredOmegaCovariance; measurementCovariance_ <<measuredOmegaCovariance;
PreintMeasCov = Matrix::Zero(3,3); PreintMeasCov_ = Matrix::Zero(3,3);
} }
PreintegratedMeasurements() : PreintegratedMeasurements() :
biasHat_(imuBias::ConstantBias()), measurementCovariance_(Matrix::Zero(3,3)), deltaTij_(0.0), biasHat_(imuBias::ConstantBias()), measurementCovariance_(Matrix::Zero(3,3)), deltaTij_(0.0),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(3,3)) {} delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(3,3)) {}
/** print */ /** print */
void print(const std::string& s = "Preintegrated Measurements: ") const { void print(const std::string& s = "Preintegrated Measurements: ") const {
@ -67,7 +68,7 @@ public:
deltaRij_.print(" deltaRij "); deltaRij_.print(" deltaRij ");
std::cout << " measurementCovariance [" << measurementCovariance_ << " ]" std::cout << " measurementCovariance [" << measurementCovariance_ << " ]"
<< std::endl; << std::endl;
std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl; std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
} }
/** equals */ /** equals */
@ -78,28 +79,20 @@ public:
expected.measurementCovariance_, tol) expected.measurementCovariance_, tol)
&& deltaRij_.equals(expected.deltaRij_, tol) && deltaRij_.equals(expected.deltaRij_, tol)
&& std::fabs(deltaTij_ - expected.deltaTij_) < tol && std::fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_,
tol); tol);
} }
Matrix measurementCovariance() const { Matrix measurementCovariance() const {return measurementCovariance_;}
return measurementCovariance_; Matrix deltaRij() const {return deltaRij_.matrix();}
} double deltaTij() const {return deltaTij_;}
Matrix deltaRij() const { Vector biasHat() const {return biasHat_.vector();}
return deltaRij_.matrix(); Matrix3 delRdelBiasOmega() {return delRdelBiasOmega_;}
} Matrix PreintMeasCov() {return PreintMeasCov_;}
double deltaTij() const {
return deltaTij_;
}
Vector biasHat() const {
return biasHat_.vector();
}
void resetIntegration() { void resetIntegration() {
deltaRij_ = Rot3(); deltaRij_ = Rot3();
deltaTij_ = 0.0; deltaTij_ = 0.0;
delRdelBiasOmega = Matrix3::Zero(); delRdelBiasOmega_ = Matrix3::Zero();
PreintMeasCov = Matrix::Zero(9, 9); PreintMeasCov_ = Matrix::Zero(9, 9);
} }
/** Add a single Gyroscope measurement to the preintegration. */ /** Add a single Gyroscope measurement to the preintegration. */
@ -125,7 +118,7 @@ public:
// Update Jacobians // Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_
- Jr_theta_incr * deltaT; - Jr_theta_incr * deltaT;
// Update preintegrated measurements covariance // Update preintegrated measurements covariance
@ -151,7 +144,7 @@ public:
// 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 = F * PreintMeasCov * F.transpose() PreintMeasCov_ = F * PreintMeasCov_ * F.transpose()
+ measurementCovariance_ * deltaT; + measurementCovariance_ * deltaT;
// Update preintegrated measurements // Update preintegrated measurements
@ -186,7 +179,7 @@ public:
ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
ar & BOOST_SERIALIZATION_NVP(deltaRij_); ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_); ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega); ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
} }
}; };
@ -222,7 +215,7 @@ public:
) : ) :
Base( Base(
noiseModel::Gaussian::Covariance( noiseModel::Gaussian::Covariance(
preintegratedMeasurements.PreintMeasCov), rot_i, rot_j, bias), preintegratedMeasurements_( preintegratedMeasurements.PreintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_(
preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
body_P_sensor) { body_P_sensor) {
} }
@ -294,7 +287,7 @@ public:
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
Rot3 deltaRij_biascorrected = Rot3 deltaRij_biascorrected =
preintegratedMeasurements_.deltaRij_.retract( preintegratedMeasurements_.deltaRij_.retract(
preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr,
Rot3::EXPMAP); Rot3::EXPMAP);
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
@ -337,9 +330,9 @@ public:
Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse( Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(
theta_biascorrected); theta_biascorrected);
Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3( Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(
preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc
* Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
H3->resize(3, 6); H3->resize(3, 6);
(*H3) << (*H3) <<
@ -375,7 +368,7 @@ public:
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected = const Rot3 deltaRij_biascorrected =
preintegratedMeasurements.deltaRij_.retract( preintegratedMeasurements.deltaRij_.retract(
preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr,
Rot3::EXPMAP); Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);

146
gtsam/navigation/CombinedImuFactor.h
View File

@ -70,7 +70,8 @@ namespace gtsam {
/** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) /** CombinedPreintegratedMeasurements 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*/ * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
class CombinedPreintegratedMeasurements { class CombinedPreintegratedMeasurements {
public: friend class CombinedImuFactor;
protected:
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector
///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
@ -80,17 +81,18 @@ namespace gtsam {
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j double deltaTij_; ///< Time interval from i to j
Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
bool use2ndOrderIntegration_; ///< Controls the order of integration bool use2ndOrderIntegration_; ///< Controls the order of integration
// public: // public:
///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases
///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
/** Default constructor, initialize with no IMU measurements */ /** Default constructor, initialize with no IMU measurements */
public:
CombinedPreintegratedMeasurements( CombinedPreintegratedMeasurements(
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
@ -102,9 +104,9 @@ namespace gtsam {
const bool use2ndOrderIntegration = false ///< Controls the order of integration const bool use2ndOrderIntegration = false ///< Controls the order of integration
///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements) ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements)
) : biasHat_(bias), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), ) : biasHat_(bias), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)), delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)),
use2ndOrderIntegration_(use2ndOrderIntegration) use2ndOrderIntegration_(use2ndOrderIntegration)
{ {
// COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21)
@ -120,9 +122,9 @@ namespace gtsam {
CombinedPreintegratedMeasurements() : CombinedPreintegratedMeasurements() :
biasHat_(imuBias::ConstantBias()), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), biasHat_(imuBias::ConstantBias()), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)), delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)),
use2ndOrderIntegration_(false) ///< Controls the order of integration use2ndOrderIntegration_(false) ///< Controls the order of integration
{ {
} }
@ -136,7 +138,7 @@ namespace gtsam {
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
deltaRij_.print(" deltaRij "); deltaRij_.print(" deltaRij ");
std::cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl; std::cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl;
std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl; std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
} }
/** equals */ /** equals */
@ -147,11 +149,11 @@ namespace gtsam {
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
&& deltaRij_.equals(expected.deltaRij_, tol) && deltaRij_.equals(expected.deltaRij_, tol)
&& std::fabs(deltaTij_ - expected.deltaTij_) < tol && std::fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol) && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol) && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol) && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol) && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
} }
void resetIntegration(){ void resetIntegration(){
@ -159,12 +161,12 @@ namespace gtsam {
deltaVij_ = Vector3::Zero(); deltaVij_ = Vector3::Zero();
deltaRij_ = Rot3(); deltaRij_ = Rot3();
deltaTij_ = 0.0; deltaTij_ = 0.0;
delPdelBiasAcc = Matrix3::Zero(); delPdelBiasAcc_ = Matrix3::Zero();
delPdelBiasOmega = Matrix3::Zero(); delPdelBiasOmega_ = Matrix3::Zero();
delVdelBiasAcc = Matrix3::Zero(); delVdelBiasAcc_ = Matrix3::Zero();
delVdelBiasOmega = Matrix3::Zero(); delVdelBiasOmega_ = Matrix3::Zero();
delRdelBiasOmega = Matrix3::Zero(); delRdelBiasOmega_ = Matrix3::Zero();
PreintMeasCov = Matrix::Zero(15,15); PreintMeasCov_ = Matrix::Zero(15,15);
} }
/** Add a single IMU measurement to the preintegration. */ /** Add a single IMU measurement to the preintegration. */
@ -195,17 +197,17 @@ namespace gtsam {
// Update Jacobians // Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){ if(!use2ndOrderIntegration_){
delPdelBiasAcc += delVdelBiasAcc * deltaT; delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
}else{ }else{
delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij_.matrix() delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
} }
delVdelBiasAcc += -deltaRij_.matrix() * deltaT; delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
delVdelBiasOmega += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * 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
@ -272,7 +274,7 @@ namespace gtsam {
G_measCov_Gt.block(3,6,3,3) = block23; G_measCov_Gt.block(3,6,3,3) = block23;
G_measCov_Gt.block(6,3,3,3) = block23.transpose(); G_measCov_Gt.block(6,3,3,3) = block23.transpose();
PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt; PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt;
// Update preintegrated measurements // Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
@ -314,22 +316,18 @@ namespace gtsam {
return Rot3::Logmap(R_t_to_t0); return Rot3::Logmap(R_t_to_t0);
} }
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
Matrix deltaRij() const { Matrix measurementCovariance() const {return measurementCovariance_;}
return deltaRij_.matrix(); Rot3 deltaRij() const {return deltaRij_;}
} double deltaTij() const{return deltaTij_;}
double deltaTij() const{ Vector deltaPij() const {return deltaPij_;}
return deltaTij_; Vector deltaVij() const {return deltaVij_;}
} Vector biasHat() const { return biasHat_.vector();}
Matrix delPdelBiasAcc() { return delPdelBiasAcc_;}
Vector deltaPij() const { Matrix delPdelBiasOmega() { return delPdelBiasOmega_;}
return deltaPij_; Matrix delVdelBiasAcc() { return delVdelBiasAcc_;}
} Matrix delVdelBiasOmega() { return delVdelBiasOmega_;}
Vector deltaVij() const { Matrix delRdelBiasOmega() { return delRdelBiasOmega_;}
return deltaVij_; Matrix PreintMeasCov() { return PreintMeasCov_;}
}
Vector biasHat() const {
return biasHat_.vector();
}
private: private:
@ -343,11 +341,11 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(deltaVij_); ar & BOOST_SERIALIZATION_NVP(deltaVij_);
ar & BOOST_SERIALIZATION_NVP(deltaRij_); ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_); ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc); ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega); ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc); ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega); ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega); ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
} }
}; };
@ -389,7 +387,7 @@ namespace gtsam {
boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
) : ) :
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
preintegratedMeasurements_(preintegratedMeasurements), preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity), gravity_(gravity),
omegaCoriolis_(omegaCoriolis), omegaCoriolis_(omegaCoriolis),
@ -466,7 +464,7 @@ namespace gtsam {
// We compute factor's Jacobians, according to [3] // We compute factor's Jacobians, according to [3]
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
@ -523,12 +521,12 @@ namespace gtsam {
(*H1) << (*H1) <<
// dfP/dRi // dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
// dfP/dPi // dfP/dPi
dfPdPi, dfPdPi,
// dfV/dRi // dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
// dfV/dPi // dfV/dPi
dfVdPi, dfVdPi,
// dfR/dRi // dfR/dRi
@ -591,17 +589,17 @@ namespace gtsam {
if(H5) { if(H5) {
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
H5->resize(15,6); H5->resize(15,6);
(*H5) << (*H5) <<
// dfP/dBias_i // dfP/dBias_i
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc, - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega, - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
// dfV/dBias_i // dfV/dBias_i
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc, - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega, - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
// dfR/dBias_i // dfR/dBias_i
Matrix::Zero(3,3), Matrix::Zero(3,3),
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega),
@ -632,16 +630,16 @@ namespace gtsam {
const Vector3 fp = const Vector3 fp =
pos_j - pos_i pos_j - pos_i
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
+ preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
- vel_i * deltaTij - vel_i * deltaTij
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
- 0.5 * gravity_ * deltaTij*deltaTij; - 0.5 * gravity_ * deltaTij*deltaTij;
const Vector3 fv = const Vector3 fv =
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
+ preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
- gravity_ * deltaTij; - gravity_ * deltaTij;
@ -675,15 +673,15 @@ namespace gtsam {
// Predict state at time j // Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
+ vel_i * deltaTij + vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij; + 0.5 * gravity * deltaTij*deltaTij;
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij); + gravity * deltaTij);
@ -692,7 +690,7 @@ namespace gtsam {
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
} }
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -

141
gtsam/navigation/ImuFactor.h
View File

@ -46,7 +46,6 @@ namespace gtsam {
*/ */
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> { class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
public: public:
/** Struct to store results of preintegrating IMU measurements. Can be build /** Struct to store results of preintegrating IMU measurements. Can be build
@ -55,7 +54,8 @@ namespace gtsam {
/** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) /** CombinedPreintegratedMeasurements 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*/ * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
class PreintegratedMeasurements { class PreintegratedMeasurements {
public: friend class ImuFactor;
protected:
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
Matrix measurementCovariance_; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) Matrix measurementCovariance_; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
@ -64,14 +64,14 @@ namespace gtsam {
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j double deltaTij_; ///< Time interval from i to j
Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
bool use2ndOrderIntegration_; ///< Controls the order of integration bool use2ndOrderIntegration_; ///< Controls the order of integration
public:
/** Default constructor, initialize with no IMU measurements */ /** Default constructor, initialize with no IMU measurements */
PreintegratedMeasurements( PreintegratedMeasurements(
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
@ -80,9 +80,9 @@ namespace gtsam {
const Matrix3& integrationErrorCovariance, ///< Covariance matrix of integration errors const Matrix3& integrationErrorCovariance, ///< Covariance matrix of integration errors
const bool use2ndOrderIntegration = false ///< Controls the order of integration const bool use2ndOrderIntegration = false ///< Controls the order of integration
) : biasHat_(bias), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), ) : biasHat_(bias), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(use2ndOrderIntegration) delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(use2ndOrderIntegration)
{ {
measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(),
@ -92,9 +92,9 @@ namespace gtsam {
PreintegratedMeasurements() : PreintegratedMeasurements() :
biasHat_(imuBias::ConstantBias()), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), biasHat_(imuBias::ConstantBias()), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(false) delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(false)
{ {
measurementCovariance_ = Matrix::Zero(9,9); measurementCovariance_ = Matrix::Zero(9,9);
PreintMeasCov_ = Matrix::Zero(9,9); PreintMeasCov_ = Matrix::Zero(9,9);
@ -120,31 +120,24 @@ namespace gtsam {
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
&& deltaRij_.equals(expected.deltaRij_, tol) && deltaRij_.equals(expected.deltaRij_, tol)
&& std::fabs(deltaTij_ - expected.deltaTij_) < tol && std::fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol) && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol) && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol) && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol) && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
}
Matrix measurementCovariance() const {
return measurementCovariance_;
}
Matrix deltaRij() const {
return deltaRij_.matrix();
}
double deltaTij() const{
return deltaTij_;
}
Vector deltaPij() const {
return deltaPij_;
}
Vector deltaVij() const {
return deltaVij_;
}
Vector biasHat() const {
return biasHat_.vector();
} }
Matrix measurementCovariance() const {return measurementCovariance_;}
Rot3 deltaRij() const {return deltaRij_;}
double deltaTij() const{return deltaTij_;}
Vector deltaPij() const {return deltaPij_;}
Vector deltaVij() const {return deltaVij_;}
Vector biasHat() const { return biasHat_.vector();}
Matrix delPdelBiasAcc() { return delPdelBiasAcc_;}
Matrix delPdelBiasOmega() { return delPdelBiasOmega_;}
Matrix delVdelBiasAcc() { return delVdelBiasAcc_;}
Matrix delVdelBiasOmega() { return delVdelBiasOmega_;}
Matrix delRdelBiasOmega() { return delRdelBiasOmega_;}
Matrix PreintMeasCov() { return PreintMeasCov_;}
void resetIntegration(){ void resetIntegration(){
@ -152,11 +145,11 @@ namespace gtsam {
deltaVij_ = Vector3::Zero(); deltaVij_ = Vector3::Zero();
deltaRij_ = Rot3(); deltaRij_ = Rot3();
deltaTij_ = 0.0; deltaTij_ = 0.0;
delPdelBiasAcc = Matrix3::Zero(); delPdelBiasAcc_ = Matrix3::Zero();
delPdelBiasOmega = Matrix3::Zero(); delPdelBiasOmega_ = Matrix3::Zero();
delVdelBiasAcc = Matrix3::Zero(); delVdelBiasAcc_ = Matrix3::Zero();
delVdelBiasOmega = Matrix3::Zero(); delVdelBiasOmega_ = Matrix3::Zero();
delRdelBiasOmega = Matrix3::Zero(); delRdelBiasOmega_ = Matrix3::Zero();
PreintMeasCov_ = Matrix::Zero(9,9); PreintMeasCov_ = Matrix::Zero(9,9);
} }
@ -190,16 +183,16 @@ namespace gtsam {
// Update Jacobians // Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
if(!use2ndOrderIntegration_){ if(!use2ndOrderIntegration_){
delPdelBiasAcc += delVdelBiasAcc * deltaT; delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
}else{ }else{
delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij_.matrix() delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
} }
delVdelBiasAcc += -deltaRij_.matrix() * deltaT; delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
delVdelBiasOmega += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
// Update preintegrated measurements covariance // Update preintegrated measurements covariance
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
@ -303,11 +296,11 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(deltaVij_); ar & BOOST_SERIALIZATION_NVP(deltaVij_);
ar & BOOST_SERIALIZATION_NVP(deltaRij_); ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_); ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc); ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega); ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc); ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega); ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega); ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
} }
}; };
@ -423,7 +416,7 @@ namespace gtsam {
// We compute factor's Jacobians // We compute factor's Jacobians
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
@ -459,12 +452,12 @@ namespace gtsam {
(*H1) << (*H1) <<
// dfP/dRi // dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
// dfP/dPi // dfP/dPi
dfPdPi, dfPdPi,
// dfV/dRi // dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
// dfV/dPi // dfV/dPi
dfVdPi, dfVdPi,
// dfR/dRi // dfR/dRi
@ -512,17 +505,17 @@ namespace gtsam {
if(H5) { if(H5) {
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
H5->resize(9,6); H5->resize(9,6);
(*H5) << (*H5) <<
// dfP/dBias // dfP/dBias
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc, - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega, - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
// dfV/dBias // dfV/dBias
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc, - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega, - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
// dfR/dBias // dfR/dBias
Matrix::Zero(3,3), Matrix::Zero(3,3),
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
@ -533,16 +526,16 @@ namespace gtsam {
const Vector3 fp = const Vector3 fp =
pos_j - pos_i pos_j - pos_i
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
+ preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
- vel_i * deltaTij - vel_i * deltaTij
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
- 0.5 * gravity_ * deltaTij*deltaTij; - 0.5 * gravity_ * deltaTij*deltaTij;
const Vector3 fv = const Vector3 fv =
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
+ preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
- gravity_ * deltaTij; - gravity_ * deltaTij;
@ -570,15 +563,15 @@ namespace gtsam {
// Predict state at time j // Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
+ vel_i * deltaTij + vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij; + 0.5 * gravity * deltaTij*deltaTij;
vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij); + gravity * deltaTij);
@ -587,7 +580,7 @@ namespace gtsam {
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
} }
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -

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

@ -362,7 +362,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
// Compare Jacobians // Compare Jacobians
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
EXPECT( EXPECT(
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega_,
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
} }
@ -451,7 +451,7 @@ TEST (AHRSFactor, graphTest) {
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create Factor // Create Factor
noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov); noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov_);
// cout<<"model: \n"<<noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov)<<endl; // cout<<"model: \n"<<noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov)<<endl;
// cout<<"pre int measurement cov: \n "<<pre_int_data.PreintMeasCov<<endl; // cout<<"pre int measurement cov: \n "<<pre_int_data.PreintMeasCov<<endl;
NonlinearFactorGraph graph; NonlinearFactorGraph graph;

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

@ -68,7 +68,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{ {
return evaluatePreintegratedMeasurements(bias, return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij_; measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij();
} }
Vector3 evaluatePreintegratedMeasurementsVelocity( Vector3 evaluatePreintegratedMeasurementsVelocity(
@ -79,7 +79,7 @@ Vector3 evaluatePreintegratedMeasurementsVelocity(
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{ {
return evaluatePreintegratedMeasurements(bias, return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaVij_; measuredAccs, measuredOmegas, deltaTs).deltaVij();
} }
Rot3 evaluatePreintegratedMeasurementsRotation( Rot3 evaluatePreintegratedMeasurementsRotation(
@ -90,7 +90,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{ {
return evaluatePreintegratedMeasurements(bias, return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaRij_; measuredAccs, measuredOmegas, deltaTs).deltaRij();
} }
} }
@ -127,7 +127,7 @@ TEST( CombinedImuFactor, PreintegratedMeasurements )
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expected1.deltaPij_), Vector(actual1.deltaPij_), tol)); EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol));
// EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol)); // EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol));
// EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol)); // EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol));
// DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol); // DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol);
@ -181,7 +181,7 @@ TEST( CombinedImuFactor, ErrorWithBiases )
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov); noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
@ -254,12 +254,12 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians // Compare Jacobians
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc)); EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega)); EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc)); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega)); EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
} }
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>

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

@ -80,7 +80,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{ {
return evaluatePreintegratedMeasurements(bias, return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaPij_; measuredAccs, measuredOmegas, deltaTs).deltaPij();
} }
Vector3 evaluatePreintegratedMeasurementsVelocity( Vector3 evaluatePreintegratedMeasurementsVelocity(
@ -91,7 +91,7 @@ Vector3 evaluatePreintegratedMeasurementsVelocity(
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{ {
return evaluatePreintegratedMeasurements(bias, return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaVij_; measuredAccs, measuredOmegas, deltaTs).deltaVij();
} }
Rot3 evaluatePreintegratedMeasurementsRotation( Rot3 evaluatePreintegratedMeasurementsRotation(
@ -102,7 +102,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{ {
return evaluatePreintegratedMeasurements(bias, return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij_; measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij();
} }
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT)
@ -140,10 +140,10 @@ TEST( ImuFactor, PreintegratedMeasurements )
ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij_), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij_), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij_, 1e-6)); EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij(), 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij_, 1e-6); DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
// Integrate again // Integrate again
Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0; Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0;
@ -155,10 +155,10 @@ TEST( ImuFactor, PreintegratedMeasurements )
ImuFactor::PreintegratedMeasurements actual2 = actual1; ImuFactor::PreintegratedMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij_), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij_), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij_, 1e-6)); EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij(), 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij_, 1e-6); DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -432,12 +432,12 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians // Compare Jacobians
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc)); EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega)); EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc)); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega)); EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
} }
//#include <gtsam/linear/GaussianFactorGraph.h> //#include <gtsam/linear/GaussianFactorGraph.h>