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

Trying to avoid mallocs

release/4.3a0
Frank Dellaert 2016-01-17 19:17:25 -08:00
parent e1d810d37a
commit c81f91999d
3 changed files with 45 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
gtsam/navigation/AggregateImuReadings.cpp
View File

@ -33,30 +33,22 @@ AggregateImuReadings::AggregateImuReadings(const boost::shared_ptr<Params>& p,
// Tangent space sugar. // Tangent space sugar.
namespace sugar { namespace sugar {
static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) { return v.segment<3>(0); } static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) { return v.segment<3>(0); }
static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) { return v.segment<3>(3); } static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) { return v.segment<3>(3); }
static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) { return v.segment<3>(6); } static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) { return v.segment<3>(6); }
typedef const Vector9 constV9;
static Eigen::Block<constV9, 3, 1> dR(constV9& v) { return v.segment<3>(0); }
static Eigen::Block<constV9, 3, 1> dP(constV9& v) { return v.segment<3>(3); }
static Eigen::Block<constV9, 3, 1> dV(constV9& v) { return v.segment<3>(6); }
} // namespace sugar } // namespace sugar
// See extensive discussion in ImuFactor.lyx // See extensive discussion in ImuFactor.lyx
Vector9 AggregateImuReadings::UpdateEstimate(const Vector9& zeta, void AggregateImuReadings::UpdateEstimate(const Vector3& a_body,
const Vector3& a_body, const Vector3& w_body, double dt,
const Vector3& w_body, double dt, Vector9* zeta,
OptionalJacobian<9, 9> A, OptionalJacobian<9, 9> A,
OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> B,
OptionalJacobian<9, 3> C) { OptionalJacobian<9, 3> C) {
using namespace sugar; using namespace sugar;
const auto theta = dR(zeta); const Vector3 theta = dR(*zeta);
const auto p = dP(zeta); const Vector3 v = dV(*zeta);
const auto v = dV(zeta);
// Calculate exact mean propagation // Calculate exact mean propagation
Matrix3 H; Matrix3 H;
@ -65,10 +57,9 @@ Vector9 AggregateImuReadings::UpdateEstimate(const Vector9& zeta,
const Vector3 a_nav = R * a_body; const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt; const double dt22 = 0.5 * dt * dt;
Vector9 zeta_plus; dR(*zeta) += invH * w_body * dt; // theta
dR(zeta_plus) = theta + invH * w_body * dt; // theta dP(*zeta) += v * dt + a_nav * dt22; // position
dP(zeta_plus) = p + v * dt + a_nav * dt22; // position dV(*zeta) += a_nav * dt; // velocity
dV(zeta_plus) = v + a_nav * dt; // velocity
if (A) { if (A) {
// First order (small angle) approximation of derivative of invH*w: // First order (small angle) approximation of derivative of invH*w:
@ -78,15 +69,21 @@ Vector9 AggregateImuReadings::UpdateEstimate(const Vector9& zeta,
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * H; const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * H;
A->setIdentity(); A->setIdentity();
A->block<3, 3>(0, 0) += invHw_H_theta * dt; A->block<3, 3>(0, 0).noalias() += invHw_H_theta * dt;
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; A->block<3, 3>(3, 0) = a_nav_H_theta * dt22;
A->block<3, 3>(3, 6) = I_3x3 * dt; A->block<3, 3>(3, 6) = I_3x3 * dt;
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; A->block<3, 3>(6, 0) = a_nav_H_theta * dt;
} }
if (B) *B << Z_3x3, R* dt22, R* dt; if (B) {
if (C) *C << invH* dt, Z_3x3, Z_3x3; B->block<3, 3>(0, 0) = Z_3x3;
B->block<3, 3>(3, 0) = R * dt22;
return zeta_plus; B->block<3, 3>(6, 0) = R * dt;
}
if (C) {
C->block<3, 3>(0, 0) = invH * dt;
C->block<3, 3>(3, 0) = Z_3x3;
C->block<3, 3>(6, 0) = Z_3x3;
}
} }
void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc, void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
@ -99,7 +96,7 @@ void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
// Do exact mean propagation // Do exact mean propagation
Matrix9 A; Matrix9 A;
Matrix93 B, C; Matrix93 B, C;
zeta_ = UpdateEstimate(zeta_, a_body, w_body, dt, A, B, C); UpdateEstimate(a_body, w_body, dt, &zeta_, A, B, C);
// propagate uncertainty // propagate uncertainty
// TODO(frank): use noiseModel routine so we can have arbitrary noise models. // TODO(frank): use noiseModel routine so we can have arbitrary noise models.
@ -108,8 +105,8 @@ void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
// TODO(frank): use Eigen-tricks for symmetric matrices // TODO(frank): use Eigen-tricks for symmetric matrices
cov_ = A * cov_ * A.transpose(); cov_ = A * cov_ * A.transpose();
cov_ += B * (aCov / dt) * B.transpose(); cov_.noalias() += B * (aCov / dt) * B.transpose();
cov_ += C * (wCov / dt) * C.transpose(); cov_.noalias() += C * (wCov / dt) * C.transpose();
deltaTij_ += dt; deltaTij_ += dt;
} }

10
gtsam/navigation/AggregateImuReadings.h
View File

@ -76,11 +76,11 @@ class AggregateImuReadings {
// Update integrated vector on tangent manifold zeta with acceleration // Update integrated vector on tangent manifold zeta with acceleration
// readings a_body and gyro readings w_body, bias-corrected in body frame. // readings a_body and gyro readings w_body, bias-corrected in body frame.
static Vector9 UpdateEstimate(const Vector9& zeta, const Vector3& a_body, static void UpdateEstimate(const Vector3& a_body, const Vector3& w_body,
const Vector3& w_body, double dt, double dt, Vector9* zeta,
OptionalJacobian<9, 9> A = boost::none, OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none, OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none); OptionalJacobian<9, 3> C = boost::none);
}; };
} // namespace gtsam } // namespace gtsam

26
gtsam/navigation/tests/testAggregateImuReadings.cpp
View File

@ -38,19 +38,22 @@ static boost::shared_ptr<AggregateImuReadings::Params> defaultParams() {
return p; return p;
} }
boost::function<Vector9(const Vector9&, const Vector3&, const Vector3&)> f = Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt, Vector9 zeta_plus = zeta;
boost::none, boost::none, boost::none); AggregateImuReadings::UpdateEstimate(a, w, kDt, &zeta_plus);
return zeta_plus;
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(AggregateImuReadings, UpdateEstimate1) { TEST(AggregateImuReadings, UpdateEstimate1) {
AggregateImuReadings pim(defaultParams()); AggregateImuReadings pim(defaultParams());
Matrix9 aH1; const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
Matrix93 aH2, aH3;
Vector9 zeta; Vector9 zeta;
zeta.setZero(); zeta.setZero();
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta2 = zeta;
pim.UpdateEstimate(zeta, acc, omega, kDt, aH1, aH2, aH3); Matrix9 aH1;
Matrix93 aH2, aH3;
pim.UpdateEstimate(acc, omega, kDt, &zeta2, aH1, aH2, aH3);
EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9));
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
@ -59,12 +62,13 @@ TEST(AggregateImuReadings, UpdateEstimate1) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(AggregateImuReadings, UpdateEstimate2) { TEST(AggregateImuReadings, UpdateEstimate2) {
AggregateImuReadings pim(defaultParams()); AggregateImuReadings pim(defaultParams());
Matrix9 aH1; const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
Matrix93 aH2, aH3;
Vector9 zeta; Vector9 zeta;
zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta2 = zeta;
pim.UpdateEstimate(zeta, acc, omega, kDt, aH1, aH2, aH3); Matrix9 aH1;
Matrix93 aH2, aH3;
pim.UpdateEstimate(acc, omega, kDt, &zeta2, aH1, aH2, aH3);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3)); EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3));
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-7)); EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-7));