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

Minor cleanup, moved reset method closer to constructor

release/4.3a0
Frank Dellaert 2015-05-23 17:28:39 -07:00
parent 283faba562
commit a261587d4b
4 changed files with 73 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/navigation/ImuFactor.cpp
View File

@ -69,9 +69,11 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
correctedAcc, correctedOmega, body_P_sensor);
const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
// rotation increment computed from the current rotation rate measurement
const Vector3 integratedOmega = correctedOmega * deltaT;
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
// rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega);
// Update Jacobians
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
@ -82,12 +84,12 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F);
// first order covariance propagation:
// as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
/* ----------------------------------------------------------------------------------------------------------------------- */
// preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
// as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF
/* --------------------------------------------------------------------------------------------*/
// preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G'
// NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
// NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done block-wise,
// NOTE 2: computation of G * (1/deltaT) * measurementCovariance * G.transpose() done block-wise,
// as G and measurementCovariance are block-diagonal matrices
preintMeasCov_ = F * preintMeasCov_ * F.transpose();
preintMeasCov_.block<3, 3>(0, 0) += integrationCovariance() * deltaT;
@ -101,16 +103,17 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT;
preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc
* accelerometerCovariance() * F_pos_noiseacc.transpose();
Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT
Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // has 1/deltaT
preintMeasCov_.block<3, 3>(0, 3) += temp;
preintMeasCov_.block<3, 3>(3, 0) += temp.transpose();
}
// F_test and G_test are given as output for testing purposes and are not needed by the factor
if (F_test) { // This in only for testing
if (F_test) {
(*F_test) << F;
}
if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise
if (G_test) {
// This in only for testing & documentation, while the actual computation is done block-wise
if (!use2ndOrderIntegration())
F_pos_noiseacc = Z_3x3;
@ -176,4 +179,4 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5);
}
} /// namespace gtsam
} // namespace gtsam

56
gtsam/navigation/PreintegratedRotation.h
View File

@ -31,31 +31,40 @@ namespace gtsam {
* It includes the definitions of the preintegrated rotation.
*/
class PreintegratedRotation {
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j
/// Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix3 delRdelBiasOmega_;
Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements
public:
///< continuous-time "Covariance" of gyroscope measurements
const Matrix3 gyroscopeCovariance_;
/**
* Default constructor, initializes the variables in the base class
*/
PreintegratedRotation(const Matrix3& measuredOmegaCovariance) :
public:
/// Default constructor, initializes the variables in the base class
explicit PreintegratedRotation(const Matrix3& measuredOmegaCovariance) :
deltaRij_(Rot3()), deltaTij_(0.0), delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(
measuredOmegaCovariance) {
}
/// methods to access class variables
/// Re-initialize PreintegratedMeasurements
void resetIntegration() {
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delRdelBiasOmega_ = Z_3x3;
}
/// @name Access instance variables
/// @{
// Return 3*3 matrix of rotation from time i to time j. Expensive in quaternion case
Matrix3 deltaRij() const {
return deltaRij_.matrix();
} // expensive
}
// Return log(rotation) from time i to time j. Expensive in both Rot3M and quaternion case.
Vector3 thetaRij(OptionalJacobian<3, 3> H = boost::none) const {
return Rot3::Logmap(deltaRij_, H);
} // super-expensive
}
const double& deltaTij() const {
return deltaTij_;
}
@ -65,15 +74,17 @@ public:
const Matrix3& gyroscopeCovariance() const {
return gyroscopeCovariance_;
}
/// @}
/// @name Testable
/// @{
/// Needed for testable
void print(const std::string& s) const {
std::cout << s << std::endl;
std::cout << " deltaTij [" << deltaTij_ << "]" << std::endl;
std::cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl;
}
/// Needed for testable
bool equals(const PreintegratedRotation& expected, double tol) const {
return deltaRij_.equals(expected.deltaRij_, tol)
&& fabs(deltaTij_ - expected.deltaTij_) < tol
@ -83,12 +94,7 @@ public:
expected.gyroscopeCovariance_, tol);
}
/// Re-initialize PreintegratedMeasurements
void resetIntegration() {
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delRdelBiasOmega_ = Z_3x3;
}
/// @}
/// Update preintegrated measurements
void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
@ -104,8 +110,7 @@ public:
void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega,
const Rot3& incrR, double deltaT) {
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- D_Rincr_integratedOmega * deltaT;
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT;
}
/// Return a bias corrected version of the integrated rotation - expensive
@ -125,12 +130,11 @@ public:
// This was done via an expmap, now we go *back* to so<3>
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected,
Jrinv_theta_bc);
const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr);
const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
return biascorrectedOmega;
}
//else
// else
return Rot3::Logmap(deltaRij_biascorrected);
}
@ -140,15 +144,15 @@ public:
return rot_i.transpose() * omegaCoriolis * deltaTij();
}
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // NOLINT
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
}
};
} /// namespace gtsam
} // namespace gtsam

32
gtsam/navigation/PreintegrationBase.cpp
View File

@ -41,6 +41,17 @@ PreintegrationBase::PreintegrationBase(const imuBias::ConstantBias& bias,
integrationCovariance_(integrationErrorCovariance) {
}
/// Re-initialize PreintegratedMeasurements
void PreintegrationBase::resetIntegration() {
PreintegratedRotation::resetIntegration();
deltaPij_ = Vector3::Zero();
deltaVij_ = Vector3::Zero();
delPdelBiasAcc_ = Z_3x3;
delPdelBiasOmega_ = Z_3x3;
delVdelBiasAcc_ = Z_3x3;
delVdelBiasOmega_ = Z_3x3;
}
/// Needed for testable
void PreintegrationBase::print(const std::string& s) const {
PreintegratedRotation::print(s);
@ -62,17 +73,6 @@ bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) con
&& equal_with_abs_tol(integrationCovariance_, other.integrationCovariance_, tol);
}
/// Re-initialize PreintegratedMeasurements
void PreintegrationBase::resetIntegration() {
PreintegratedRotation::resetIntegration();
deltaPij_ = Vector3::Zero();
deltaVij_ = Vector3::Zero();
delPdelBiasAcc_ = Z_3x3;
delPdelBiasOmega_ = Z_3x3;
delVdelBiasAcc_ = Z_3x3;
delVdelBiasOmega_ = Z_3x3;
}
/// Update preintegrated measurements
void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correctedAcc,
const Rot3& incrR, const double deltaT,
@ -161,7 +161,7 @@ PoseVelocityBias PreintegrationBase::predict(
const Vector3 pos_i = pose_i.translation().vector();
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------*/
const Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr
+ delPdelBiasOmega() * biasOmegaIncr;
if (deltaPij_biascorrected_out) // if desired, store this
@ -177,13 +177,13 @@ PoseVelocityBias PreintegrationBase::predict(
if (deltaVij_biascorrected_out) // if desired, store this
*deltaVij_biascorrected_out = deltaVij_biascorrected;
Vector3 vel_j = Vector3(
vel_i + Rot_i_matrix * deltaVij_biascorrected - 2 * omegaCoriolis.cross(vel_i) * dt // Coriolis term
Vector3 vel_j = Vector3(vel_i + Rot_i_matrix * deltaVij_biascorrected -
2 * omegaCoriolis.cross(vel_i) * dt // Coriolis term
+ gravity * dt);
if (use2ndOrderCoriolis) {
pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2; // 2nd order coriolis term for position
vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt; // 2nd order term for velocity
pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2; // for position
vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt; // for velocity
}
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);

10
gtsam/navigation/PreintegrationBase.h
View File

@ -52,8 +52,8 @@ struct PoseVelocityBias {
*/
class PreintegrationBase : public PreintegratedRotation {
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
bool use2ndOrderIntegration_; ///< Controls the order of integration
const imuBias::ConstantBias biasHat_; ///< Acceleration and gyro bias used for preintegration
const bool use2ndOrderIntegration_; ///< Controls the order of integration
Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
@ -79,6 +79,9 @@ class PreintegrationBase : public PreintegratedRotation {
const Matrix3& measuredOmegaCovariance,
const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration);
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
/// methods to access class variables
bool use2ndOrderIntegration() const {
return use2ndOrderIntegration_;
@ -121,9 +124,6 @@ class PreintegrationBase : public PreintegratedRotation {
/// check equality
bool equals(const PreintegrationBase& other, double tol) const;
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
/// Update preintegrated measurements
void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR,
const double deltaT, OptionalJacobian<9, 9> F);