Changed access specifier of preintegrated measurement variables to protected.
krunalchande
parent
f96e7ef32f
commit
ce5f7911c5
|
|
@ -37,28 +37,29 @@ public:
|
|||
/** CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates)
|
||||
* and the corresponding covariance matrix. The measurements are then used to build the Preintegrated AHRS factor*/
|
||||
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
|
||||
Matrix measurementCovariance_;///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3)
|
||||
|
||||
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
|
||||
double deltaTij_; ///< Time interval from i to j
|
||||
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*)
|
||||
|
||||
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*)
|
||||
public:
|
||||
/** Default constructor, initialize with no measurements */
|
||||
PreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
|
||||
const Matrix3& measuredOmegaCovariance ///< Covariance matrix of measured angular rate
|
||||
) : biasHat_(bias), measurementCovariance_(3,3), deltaTij_(0.0),
|
||||
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(3,3) {
|
||||
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(3,3) {
|
||||
measurementCovariance_ <<measuredOmegaCovariance;
|
||||
PreintMeasCov = Matrix::Zero(3,3);
|
||||
PreintMeasCov_ = Matrix::Zero(3,3);
|
||||
}
|
||||
|
||||
PreintegratedMeasurements() :
|
||||
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 */
|
||||
void print(const std::string& s = "Preintegrated Measurements: ") const {
|
||||
|
|
@ -67,7 +68,7 @@ public:
|
|||
deltaRij_.print(" deltaRij ");
|
||||
std::cout << " measurementCovariance [" << measurementCovariance_ << " ]"
|
||||
<< std::endl;
|
||||
std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl;
|
||||
std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
|
||||
}
|
||||
|
||||
/** equals */
|
||||
|
|
@ -78,28 +79,20 @@ public:
|
|||
expected.measurementCovariance_, tol)
|
||||
&& deltaRij_.equals(expected.deltaRij_, tol)
|
||||
&& std::fabs(deltaTij_ - expected.deltaTij_) < tol
|
||||
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega,
|
||||
&& 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 biasHat() const {
|
||||
return biasHat_.vector();
|
||||
}
|
||||
|
||||
Matrix measurementCovariance() const {return measurementCovariance_;}
|
||||
Matrix deltaRij() const {return deltaRij_.matrix();}
|
||||
double deltaTij() const {return deltaTij_;}
|
||||
Vector biasHat() const {return biasHat_.vector();}
|
||||
Matrix3 delRdelBiasOmega() {return delRdelBiasOmega_;}
|
||||
Matrix PreintMeasCov() {return PreintMeasCov_;}
|
||||
void resetIntegration() {
|
||||
deltaRij_ = Rot3();
|
||||
deltaTij_ = 0.0;
|
||||
delRdelBiasOmega = Matrix3::Zero();
|
||||
PreintMeasCov = Matrix::Zero(9, 9);
|
||||
delRdelBiasOmega_ = Matrix3::Zero();
|
||||
PreintMeasCov_ = Matrix::Zero(9, 9);
|
||||
}
|
||||
|
||||
/** Add a single Gyroscope measurement to the preintegration. */
|
||||
|
|
@ -125,7 +118,7 @@ public:
|
|||
|
||||
// Update Jacobians
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega
|
||||
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_
|
||||
- Jr_theta_incr * deltaT;
|
||||
|
||||
// Update preintegrated measurements covariance
|
||||
|
|
@ -151,7 +144,7 @@ public:
|
|||
|
||||
// first order uncertainty propagation
|
||||
// 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;
|
||||
|
||||
// Update preintegrated measurements
|
||||
|
|
@ -186,7 +179,7 @@ public:
|
|||
ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega);
|
||||
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -222,7 +215,7 @@ public:
|
|||
) :
|
||||
Base(
|
||||
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_(
|
||||
body_P_sensor) {
|
||||
}
|
||||
|
|
@ -294,7 +287,7 @@ public:
|
|||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
Rot3 deltaRij_biascorrected =
|
||||
preintegratedMeasurements_.deltaRij_.retract(
|
||||
preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr,
|
||||
preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr,
|
||||
Rot3::EXPMAP);
|
||||
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
|
|
@ -337,9 +330,9 @@ public:
|
|||
Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(
|
||||
theta_biascorrected);
|
||||
Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(
|
||||
preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr);
|
||||
preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
|
||||
Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc
|
||||
* Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
|
||||
* Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
|
||||
|
||||
H3->resize(3, 6);
|
||||
(*H3) <<
|
||||
|
|
@ -375,7 +368,7 @@ public:
|
|||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Rot3 deltaRij_biascorrected =
|
||||
preintegratedMeasurements.deltaRij_.retract(
|
||||
preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr,
|
||||
preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr,
|
||||
Rot3::EXPMAP);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
|
|
|
|||
|
|
@ -70,7 +70,8 @@ namespace gtsam {
|
|||
/** 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*/
|
||||
class CombinedPreintegratedMeasurements {
|
||||
public:
|
||||
friend class CombinedImuFactor;
|
||||
protected:
|
||||
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
|
||||
Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector
|
||||
///< [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)
|
||||
double deltaTij_; ///< Time interval from i to j
|
||||
|
||||
Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration 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 delVdelBiasOmega; ///< Jacobian of preintegrated velocity 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*)
|
||||
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration 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 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity 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*)
|
||||
bool use2ndOrderIntegration_; ///< Controls the order of integration
|
||||
// public:
|
||||
///< 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]
|
||||
/** Default constructor, initialize with no IMU measurements */
|
||||
public:
|
||||
CombinedPreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
|
||||
const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
|
||||
|
|
@ -102,9 +104,9 @@ namespace gtsam {
|
|||
const bool use2ndOrderIntegration = false ///< Controls the order of integration
|
||||
///< (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),
|
||||
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
|
||||
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
|
||||
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)),
|
||||
delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
|
||||
delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
|
||||
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)),
|
||||
use2ndOrderIntegration_(use2ndOrderIntegration)
|
||||
{
|
||||
// COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21)
|
||||
|
|
@ -120,9 +122,9 @@ namespace gtsam {
|
|||
|
||||
CombinedPreintegratedMeasurements() :
|
||||
biasHat_(imuBias::ConstantBias()), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
|
||||
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
|
||||
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
|
||||
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)),
|
||||
delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
|
||||
delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
|
||||
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)),
|
||||
use2ndOrderIntegration_(false) ///< Controls the order of integration
|
||||
{
|
||||
}
|
||||
|
|
@ -136,7 +138,7 @@ namespace gtsam {
|
|||
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
|
||||
deltaRij_.print(" deltaRij ");
|
||||
std::cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl;
|
||||
std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl;
|
||||
std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
|
||||
}
|
||||
|
||||
/** equals */
|
||||
|
|
@ -147,11 +149,11 @@ namespace gtsam {
|
|||
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
|
||||
&& deltaRij_.equals(expected.deltaRij_, tol)
|
||||
&& std::fabs(deltaTij_ - expected.deltaTij_) < tol
|
||||
&& 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(delRdelBiasOmega, expected.delRdelBiasOmega, tol);
|
||||
&& 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(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
|
||||
}
|
||||
|
||||
void resetIntegration(){
|
||||
|
|
@ -159,12 +161,12 @@ namespace gtsam {
|
|||
deltaVij_ = Vector3::Zero();
|
||||
deltaRij_ = Rot3();
|
||||
deltaTij_ = 0.0;
|
||||
delPdelBiasAcc = Matrix3::Zero();
|
||||
delPdelBiasOmega = Matrix3::Zero();
|
||||
delVdelBiasAcc = Matrix3::Zero();
|
||||
delVdelBiasOmega = Matrix3::Zero();
|
||||
delRdelBiasOmega = Matrix3::Zero();
|
||||
PreintMeasCov = Matrix::Zero(15,15);
|
||||
delPdelBiasAcc_ = Matrix3::Zero();
|
||||
delPdelBiasOmega_ = Matrix3::Zero();
|
||||
delVdelBiasAcc_ = Matrix3::Zero();
|
||||
delVdelBiasOmega_ = Matrix3::Zero();
|
||||
delRdelBiasOmega_ = Matrix3::Zero();
|
||||
PreintMeasCov_ = Matrix::Zero(15,15);
|
||||
}
|
||||
|
||||
/** Add a single IMU measurement to the preintegration. */
|
||||
|
|
@ -195,17 +197,17 @@ namespace gtsam {
|
|||
// Update Jacobians
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
delPdelBiasAcc += delVdelBiasAcc * deltaT;
|
||||
delPdelBiasOmega += delVdelBiasOmega * deltaT;
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
|
||||
}else{
|
||||
delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
|
||||
delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij_.matrix()
|
||||
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega;
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
|
||||
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
|
||||
}
|
||||
|
||||
delVdelBiasAcc += -deltaRij_.matrix() * deltaT;
|
||||
delVdelBiasOmega += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega;
|
||||
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT;
|
||||
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
|
||||
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
|
||||
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
|
||||
|
||||
// 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
|
||||
|
|
@ -272,7 +274,7 @@ namespace gtsam {
|
|||
G_measCov_Gt.block(3,6,3,3) = block23;
|
||||
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
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
|
|
@ -314,22 +316,18 @@ namespace gtsam {
|
|||
return Rot3::Logmap(R_t_to_t0);
|
||||
}
|
||||
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
|
||||
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_;}
|
||||
|
||||
|
||||
private:
|
||||
|
|
@ -343,11 +341,11 @@ namespace gtsam {
|
|||
ar & BOOST_SERIALIZATION_NVP(deltaVij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc);
|
||||
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega);
|
||||
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc);
|
||||
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega);
|
||||
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega);
|
||||
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
|
||||
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
|
||||
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),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
|
|
@ -466,7 +464,7 @@ namespace gtsam {
|
|||
|
||||
// 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)
|
||||
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
|
|
@ -523,12 +521,12 @@ namespace gtsam {
|
|||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
|
|
@ -591,17 +589,17 @@ namespace gtsam {
|
|||
|
||||
if(H5) {
|
||||
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr);
|
||||
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
|
||||
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
|
||||
|
||||
H5->resize(15,6);
|
||||
(*H5) <<
|
||||
// dfP/dBias_i
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
|
||||
// dfV/dBias_i
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
|
||||
// dfR/dBias_i
|
||||
Matrix::Zero(3,3),
|
||||
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega),
|
||||
|
|
@ -632,16 +630,16 @@ namespace gtsam {
|
|||
const Vector3 fp =
|
||||
pos_j - pos_i
|
||||
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr)
|
||||
+ preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
|
||||
- vel_i * deltaTij
|
||||
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
- 0.5 * gravity_ * deltaTij*deltaTij;
|
||||
|
||||
const Vector3 fv =
|
||||
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr)
|
||||
+ preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
|
||||
- gravity_ * deltaTij;
|
||||
|
||||
|
|
@ -675,15 +673,15 @@ namespace gtsam {
|
|||
// Predict state at time j
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
|
||||
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
|
||||
+ preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
|
||||
+ vel_i * deltaTij
|
||||
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ 0.5 * gravity * deltaTij*deltaTij;
|
||||
|
||||
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
|
||||
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
|
||||
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
|
|
@ -692,7 +690,7 @@ namespace gtsam {
|
|||
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)
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
|
|
|
|||
|
|
@ -46,7 +46,6 @@ namespace gtsam {
|
|||
*/
|
||||
|
||||
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
|
||||
|
||||
public:
|
||||
|
||||
/** 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)
|
||||
* and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
|
||||
class PreintegratedMeasurements {
|
||||
public:
|
||||
friend class ImuFactor;
|
||||
protected:
|
||||
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)
|
||||
|
||||
|
|
@ -64,14 +64,14 @@ namespace gtsam {
|
|||
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
|
||||
double deltaTij_; ///< Time interval from i to j
|
||||
|
||||
Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias
|
||||
Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate 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 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
|
||||
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration 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
|
||||
Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
|
||||
bool use2ndOrderIntegration_; ///< Controls the order of integration
|
||||
|
||||
public:
|
||||
/** Default constructor, initialize with no IMU measurements */
|
||||
PreintegratedMeasurements(
|
||||
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 bool use2ndOrderIntegration = false ///< Controls the order of integration
|
||||
) : biasHat_(bias), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
|
||||
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
|
||||
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
|
||||
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(use2ndOrderIntegration)
|
||||
delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
|
||||
delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
|
||||
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(use2ndOrderIntegration)
|
||||
{
|
||||
measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(),
|
||||
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(),
|
||||
|
|
@ -92,9 +92,9 @@ namespace gtsam {
|
|||
|
||||
PreintegratedMeasurements() :
|
||||
biasHat_(imuBias::ConstantBias()), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
|
||||
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
|
||||
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
|
||||
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(false)
|
||||
delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
|
||||
delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
|
||||
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(false)
|
||||
{
|
||||
measurementCovariance_ = Matrix::Zero(9,9);
|
||||
PreintMeasCov_ = Matrix::Zero(9,9);
|
||||
|
|
@ -120,31 +120,24 @@ namespace gtsam {
|
|||
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
|
||||
&& deltaRij_.equals(expected.deltaRij_, tol)
|
||||
&& std::fabs(deltaTij_ - expected.deltaTij_) < tol
|
||||
&& 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(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();
|
||||
&& 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(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
|
||||
}
|
||||
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(){
|
||||
|
|
@ -152,11 +145,11 @@ namespace gtsam {
|
|||
deltaVij_ = Vector3::Zero();
|
||||
deltaRij_ = Rot3();
|
||||
deltaTij_ = 0.0;
|
||||
delPdelBiasAcc = Matrix3::Zero();
|
||||
delPdelBiasOmega = Matrix3::Zero();
|
||||
delVdelBiasAcc = Matrix3::Zero();
|
||||
delVdelBiasOmega = Matrix3::Zero();
|
||||
delRdelBiasOmega = Matrix3::Zero();
|
||||
delPdelBiasAcc_ = Matrix3::Zero();
|
||||
delPdelBiasOmega_ = Matrix3::Zero();
|
||||
delVdelBiasAcc_ = Matrix3::Zero();
|
||||
delVdelBiasOmega_ = Matrix3::Zero();
|
||||
delRdelBiasOmega_ = Matrix3::Zero();
|
||||
PreintMeasCov_ = Matrix::Zero(9,9);
|
||||
}
|
||||
|
||||
|
|
@ -190,16 +183,16 @@ namespace gtsam {
|
|||
// Update Jacobians
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
delPdelBiasAcc += delVdelBiasAcc * deltaT;
|
||||
delPdelBiasOmega += delVdelBiasOmega * deltaT;
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
|
||||
}else{
|
||||
delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
|
||||
delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij_.matrix()
|
||||
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega;
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
|
||||
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
|
||||
}
|
||||
delVdelBiasAcc += -deltaRij_.matrix() * deltaT;
|
||||
delVdelBiasOmega += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega;
|
||||
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT;
|
||||
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
|
||||
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
|
||||
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
|
||||
|
||||
// Update preintegrated measurements covariance
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
|
|
@ -303,11 +296,11 @@ namespace gtsam {
|
|||
ar & BOOST_SERIALIZATION_NVP(deltaVij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc);
|
||||
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega);
|
||||
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc);
|
||||
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega);
|
||||
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega);
|
||||
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
|
||||
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -423,7 +416,7 @@ namespace gtsam {
|
|||
|
||||
// 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)
|
||||
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
|
|
@ -459,12 +452,12 @@ namespace gtsam {
|
|||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
|
|
@ -512,17 +505,17 @@ namespace gtsam {
|
|||
if(H5) {
|
||||
|
||||
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr);
|
||||
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
|
||||
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
|
||||
|
||||
H5->resize(9,6);
|
||||
(*H5) <<
|
||||
// dfP/dBias
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
|
||||
// dfV/dBias
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
|
||||
// dfR/dBias
|
||||
Matrix::Zero(3,3),
|
||||
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
|
||||
|
|
@ -533,16 +526,16 @@ namespace gtsam {
|
|||
const Vector3 fp =
|
||||
pos_j - pos_i
|
||||
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr)
|
||||
+ preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
|
||||
- vel_i * deltaTij
|
||||
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
- 0.5 * gravity_ * deltaTij*deltaTij;
|
||||
|
||||
const Vector3 fv =
|
||||
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr)
|
||||
+ preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
|
||||
- gravity_ * deltaTij;
|
||||
|
||||
|
|
@ -570,15 +563,15 @@ namespace gtsam {
|
|||
// Predict state at time j
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
|
||||
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
|
||||
+ preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
|
||||
+ vel_i * deltaTij
|
||||
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ 0.5 * gravity * deltaTij*deltaTij;
|
||||
|
||||
vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
|
||||
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
|
||||
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
|
|
@ -587,7 +580,7 @@ namespace gtsam {
|
|||
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)
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
|
|
|
|||
|
|
@ -362,7 +362,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
|
|||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
|
||||
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
|
||||
}
|
||||
|
||||
|
|
@ -451,7 +451,7 @@ TEST (AHRSFactor, graphTest) {
|
|||
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// 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<<"pre int measurement cov: \n "<<pre_int_data.PreintMeasCov<<endl;
|
||||
NonlinearFactorGraph graph;
|
||||
|
|
|
|||
|
|
@ -68,7 +68,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
|
|||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
return evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij_;
|
||||
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij();
|
||||
}
|
||||
|
||||
Vector3 evaluatePreintegratedMeasurementsVelocity(
|
||||
|
|
@ -79,7 +79,7 @@ Vector3 evaluatePreintegratedMeasurementsVelocity(
|
|||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
return evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs).deltaVij_;
|
||||
measuredAccs, measuredOmegas, deltaTs).deltaVij();
|
||||
}
|
||||
|
||||
Rot3 evaluatePreintegratedMeasurementsRotation(
|
||||
|
|
@ -90,7 +90,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
|
|||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
return evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs).deltaRij_;
|
||||
measuredAccs, measuredOmegas, deltaTs).deltaRij();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -127,7 +127,7 @@ TEST( CombinedImuFactor, PreintegratedMeasurements )
|
|||
|
||||
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(expected1.deltaRij, actual1.deltaRij, tol));
|
||||
// DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol);
|
||||
|
|
@ -181,7 +181,7 @@ TEST( CombinedImuFactor, ErrorWithBiases )
|
|||
// Create factor
|
||||
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);
|
||||
|
||||
|
||||
|
|
@ -254,12 +254,12 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
|
|||
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc));
|
||||
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega));
|
||||
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc));
|
||||
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega));
|
||||
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
|
||||
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
|
||||
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
|
||||
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
|
||||
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>
|
||||
|
|
|
|||
|
|
@ -80,7 +80,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
|
|||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
return evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs).deltaPij_;
|
||||
measuredAccs, measuredOmegas, deltaTs).deltaPij();
|
||||
}
|
||||
|
||||
Vector3 evaluatePreintegratedMeasurementsVelocity(
|
||||
|
|
@ -91,7 +91,7 @@ Vector3 evaluatePreintegratedMeasurementsVelocity(
|
|||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
return evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs).deltaVij_;
|
||||
measuredAccs, measuredOmegas, deltaTs).deltaVij();
|
||||
}
|
||||
|
||||
Rot3 evaluatePreintegratedMeasurementsRotation(
|
||||
|
|
@ -102,7 +102,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
|
|||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
return evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij_;
|
||||
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij();
|
||||
}
|
||||
|
||||
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);
|
||||
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij_), 1e-6));
|
||||
EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij_), 1e-6));
|
||||
EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij_, 1e-6));
|
||||
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij_, 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(expectedDeltaR1, actual1.deltaRij(), 1e-6));
|
||||
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
|
||||
|
||||
// Integrate again
|
||||
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;
|
||||
actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij_), 1e-6));
|
||||
EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij_), 1e-6));
|
||||
EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij_, 1e-6));
|
||||
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij_, 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(expectedDeltaR2, actual2.deltaRij(), 1e-6));
|
||||
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -432,12 +432,12 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
|
|||
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc));
|
||||
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega));
|
||||
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc));
|
||||
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega));
|
||||
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
|
||||
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
|
||||
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
|
||||
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
|
||||
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>
|
||||
|
|
|
|||
Loading…
Reference in New Issue