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

Removed inheritance from PreintegratedRotation

release/4.3a0
dellaert 2015-07-31 11:43:53 -07:00
parent dd468ab495
commit 628a4cc4cc
4 changed files with 49 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/navigation/ImuFactor.cpp
View File

@ -81,10 +81,10 @@ void PreintegratedImuMeasurements::integrateMeasurement(
// 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();
D_t_t(&preintMeasCov_) += p().integrationCovariance * deltaT;
D_v_v(&preintMeasCov_) += dRij * p().accelerometerCovariance * dRij.transpose() * deltaT;
D_R_R(&preintMeasCov_) += D_incrR_integratedOmega * p().gyroscopeCovariance
* D_incrR_integratedOmega.transpose() * deltaT;
D_t_t(&preintMeasCov_) += p().integrationCovariance * deltaT;
D_v_v(&preintMeasCov_) += dRij * p().accelerometerCovariance * dRij.transpose() * deltaT;
Matrix3 F_pos_noiseacc;
F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT;

16
gtsam/navigation/PreintegrationBase.cpp
View File

@ -28,7 +28,9 @@ namespace gtsam {
/// Re-initialize PreintegratedMeasurements
void PreintegrationBase::resetIntegration() {
PreintegratedRotation::resetIntegration();
deltaTij_ = 0.0;
deltaRij_ = Rot3();
delRdelBiasOmega_ = Z_3x3;
deltaPij_ = Vector3::Zero();
deltaVij_ = Vector3::Zero();
delPdelBiasAcc_ = Z_3x3;
@ -39,7 +41,9 @@ void PreintegrationBase::resetIntegration() {
/// Needed for testable
void PreintegrationBase::print(const string& s) const {
PreintegratedRotation::print(s);
cout << s << endl;
cout << " deltaTij [" << deltaTij_ << "]" << endl;
cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
biasHat_.print(" biasHat");
@ -48,7 +52,9 @@ void PreintegrationBase::print(const string& s) const {
/// Needed for testable
bool PreintegrationBase::equals(const PreintegrationBase& other,
double tol) const {
return PreintegratedRotation::equals(other, tol)
return deltaRij_.equals(other.deltaRij_, tol)
&& fabs(deltaTij_ - other.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(deltaPij_, other.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, other.deltaVij_, tol)
@ -130,7 +136,9 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
// Correct deltaRij, derivative is delRdelBiasOmega_
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
Matrix3 D_deltaRij_bias;
Rot3 deltaRij = biascorrectedDeltaRij(biasIncr.gyroscope(), H ? &D_deltaRij_bias : 0);
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
const Rot3 deltaRij = deltaRij_.expmap(biasInducedOmega, boost::none, H ? &D_deltaRij_bias : 0);
if (H) D_deltaRij_bias *= delRdelBiasOmega_;
Vector9 xi;
Matrix3 D_dR_deltaRij;

24
gtsam/navigation/PreintegrationBase.h
View File

@ -51,7 +51,7 @@ struct PoseVelocityBias {
* It includes the definitions of the preintegrated variables and the methods
* to access, print, and compare them.
*/
class PreintegrationBase: public PreintegratedRotation {
class PreintegrationBase {
public:
@ -100,6 +100,13 @@ public:
protected:
double deltaTij_; ///< Time interval from i to j
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
/// Parameters
boost::shared_ptr<Params> p_;
/// Acceleration and gyro bias used for preintegration
imuBias::ConstantBias biasHat_;
@ -124,7 +131,7 @@ public:
*/
PreintegrationBase(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat) :
PreintegratedRotation(p), biasHat_(biasHat) {
p_(p), biasHat_(biasHat) {
resetIntegration();
}
@ -136,6 +143,15 @@ public:
}
/// getters
const double& deltaTij() const {
return deltaTij_;
}
const Rot3& deltaRij() const {
return deltaRij_;
}
const Matrix3& delRdelBiasOmega() const {
return delRdelBiasOmega_;
}
const imuBias::ConstantBias& biasHat() const {
return biasHat_;
}
@ -202,8 +218,10 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(deltaPij_);
ar & BOOST_SERIALIZATION_NVP(deltaVij_);

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

@ -364,15 +364,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Make sure of biascorrectedDeltaRij with this example...
Matrix3 aH;
pim.biascorrectedDeltaRij(bias.gyroscope(), aH);
Matrix3 eH = numericalDerivative11<Rot3, Vector3>(
boost::bind(&PreintegrationBase::biascorrectedDeltaRij, pim, _1,
boost::none), bias.gyroscope());
EXPECT(assert_equal(eH, aH));
// ... and of biasCorrectedDelta
// Make sure of biasCorrectedDelta
Matrix96 actualH;
pim.biasCorrectedDelta(bias, actualH);
Matrix expectedH = numericalDerivative11<Vector9, imuBias::ConstantBias>(
@ -803,11 +795,14 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim,
const NavState& state, const imuBias::ConstantBias& bias, double dt,
const Pose3& body_P_sensor, const Vector3& measuredAcc,
const Vector3& measuredOmega, size_t N = 1000) {
const Vector3& measuredOmega, size_t N = 100) {
// Get mean prediction from "ground truth" measurements
PreintegratedImuMeasurements pim1 = pim;
pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor);
for (size_t k=0;k<10;k++)
pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor);
NavState mean = pim1.predict(state, bias);
cout << "pim1.preintMeasCov():" << endl;
cout << pim1.preintMeasCov() << endl;
// Do a Monte Carlo analysis to determine empirical density on the predicted state
Sampler sampleAccelerationNoise(Vector3::Constant(sqrt(accNoiseVar)));
@ -818,18 +813,23 @@ Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim,
PreintegratedImuMeasurements pim2 = pim;
Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample();
Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample();
pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, body_P_sensor);
for (size_t k=0;k<10;k++)
pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, body_P_sensor);
NavState prediction = pim2.predict(state, bias);
samples.col(i) = mean.localCoordinates(prediction);
sum += samples.col(i);
}
Vector9 sampleMean = sum / N;
cout << ":" << endl;
cout << sampleMean << endl;
Matrix9 Q;
Q.setZero();
for (size_t i = 0; i < N; i++) {
Vector9 xi = samples.col(i) - sampleMean;
Q += xi * xi.transpose() / (N - 1);
}
cout << "Q:" << endl;
cout << Q << endl;
return Q;
}
@ -857,8 +857,6 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true);
Matrix9 Q = MonteCarlo(pim, NavState(x1, v1), biasHat, dt, body_P_sensor,
measuredAcc, measuredOmega, 1000);
cout << Q << endl;
Matrix expected(9,9);
expected <<
@ -993,14 +991,10 @@ TEST(ImuFactor, PredictArbitrary) {
kIntegrationErrorCovariance, true);
Pose3 x1;
Vector3 v1 = Vector3(0, 0, 0);
Matrix9 Q = MonteCarlo(pim, NavState(x1, v1), biasHat, dt, Pose3(),
Matrix9 Q = MonteCarlo(pim, NavState(x1, v1), biasHat, 0.1, Pose3(),
measuredAcc, measuredOmega, 1000);
cout << Q << endl;
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
cout << pim.preintMeasCov() << endl;
for (int i = 0; i < 999; ++i)
for (int i = 0; i < 1000; ++i)
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
Matrix expected(9,9);