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

Removed debug code

release/4.3a0
Frank Dellaert 2016-01-18 00:24:17 -08:00
parent 0912b6d497
commit e64fc532e3
3 changed files with 1 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/navigation/AggregateImuReadings.cpp
View File

@ -103,7 +103,6 @@ void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
const Matrix3& aCov = p_->accelerometerCovariance; const Matrix3& aCov = p_->accelerometerCovariance;
const Matrix3& wCov = p_->gyroscopeCovariance; const Matrix3& wCov = p_->gyroscopeCovariance;
// TODO(frank): use Eigen-tricks for symmetric matrices
cov_ = A * cov_ * A.transpose(); cov_ = A * cov_ * A.transpose();
cov_.noalias() += B * (aCov / dt) * B.transpose(); cov_.noalias() += B * (aCov / dt) * B.transpose();
cov_.noalias() += C * (wCov / dt) * C.transpose(); cov_.noalias() += C * (wCov / dt) * C.transpose();
@ -118,20 +117,17 @@ NavState AggregateImuReadings::predict(const NavState& state_i,
using namespace sugar; using namespace sugar;
Vector9 zeta = zeta_; Vector9 zeta = zeta_;
// Correct for initial velocity and gravity // Correct for initial velocity and gravity
#if 1
Rot3 Ri = state_i.attitude(); Rot3 Ri = state_i.attitude();
Matrix3 Rit = Ri.transpose(); Matrix3 Rit = Ri.transpose();
Vector3 gt = deltaTij_ * p_->n_gravity; Vector3 gt = deltaTij_ * p_->n_gravity;
dP(zeta) += Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt); dP(zeta) += Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt);
dV(zeta) += Rit * gt; dV(zeta) += Rit * gt;
#endif
return state_i.retract(zeta); return state_i.retract(zeta);
} }
SharedGaussian AggregateImuReadings::noiseModel() const { SharedGaussian AggregateImuReadings::noiseModel() const {
#ifndef LOCALCOORDINATES_ONLY
// Correct for application of retract, by calculating the retract derivative H // Correct for application of retract, by calculating the retract derivative H
// From NavState::retract: // From NavState::retract:
Matrix3 D_R_theta; Matrix3 D_R_theta;
@ -144,9 +140,6 @@ SharedGaussian AggregateImuReadings::noiseModel() const {
// TODO(frank): theta() itself is noisy, so should we not correct for that? // TODO(frank): theta() itself is noisy, so should we not correct for that?
Matrix9 HcH = H * cov_ * H.transpose(); Matrix9 HcH = H * cov_ * H.transpose();
return noiseModel::Gaussian::Covariance(HcH, false); return noiseModel::Gaussian::Covariance(HcH, false);
#else
return noiseModel::Gaussian::Covariance(cov_, false);
#endif
} }
Matrix9 AggregateImuReadings::preintMeasCov() const { Matrix9 AggregateImuReadings::preintMeasCov() const {

2
gtsam/navigation/AggregateImuReadings.h
View File

@ -20,8 +20,6 @@
#include <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/ImuFactor.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#define LOCALCOORDINATES_ONLY
namespace gtsam { namespace gtsam {
/** /**

4
gtsam/navigation/ScenarioRunner.cpp
View File

@ -65,12 +65,8 @@ Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N,
Vector9 sum = Vector9::Zero(); Vector9 sum = Vector9::Zero();
for (size_t i = 0; i < N; i++) { for (size_t i = 0; i < N; i++) {
auto pim = integrate(T, estimatedBias, true); auto pim = integrate(T, estimatedBias, true);
#ifndef LOCALCOORDINATES_ONLY
NavState sampled = predict(pim); NavState sampled = predict(pim);
Vector9 xi = sampled.localCoordinates(prediction); Vector9 xi = sampled.localCoordinates(prediction);
#else
Vector9 xi = pim.zeta();
#endif
samples.col(i) = xi; samples.col(i) = xi;
sum += xi; sum += xi;
} }