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

Now use applyInvDexp

release/4.3a0
Frank Dellaert 2016-02-01 12:55:24 -08:00
parent 117f9c3a4e
commit 4f214ad7d1
2 changed files with 12 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/navigation/PreintegrationBase.cpp
View File

@ -120,32 +120,34 @@ Vector9 PreintegrationBase::UpdatePreintegrated(
const Vector3& a_body, const Vector3& w_body, double dt,
const Vector9& preintegrated, OptionalJacobian<9, 9> A,
OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) {
// TODO(frank): expose DexpImpl functor and save on computation
static const MultiplyWithInverseFunction<Vector3, 3> applyInvDexp(SO3::ApplyExpmapDerivative);
const auto theta = preintegrated.segment<3>(0);
const auto position = preintegrated.segment<3>(3);
const auto velocity = preintegrated.segment<3>(6);
// This functor allows for saving computation when exponential map and its
// derivatives are needed at the same location in so<3>
so3::DexpFunctor local(theta);
// Calculate exact mean propagation
Matrix3 H, invH, invHw_H_theta;
const Vector invHw = applyInvDexp(theta, w_body, A ? &invHw_H_theta : 0, invH);
const Matrix3 R = Rot3::Expmap(theta, A ? &H : 0).matrix();
Matrix3 w_tangent_H_theta, invH;
const Vector w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const SO3 R = local.expmap();
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
Vector9 preintegratedPlus;
preintegratedPlus << //
theta + invHw* dt, // theta
preintegratedPlus << // new preintegrated vector:
theta + w_tangent* dt, // theta
position + velocity* dt + a_nav* dt22, // position
velocity + a_nav* dt; // velocity
if (A) {
// Exact derivative of R*a with respect to theta:
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * H;
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();
A->setIdentity();
A->block<3, 3>(0, 0).noalias() += invHw_H_theta * dt; // theta
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta

21
gtsam/navigation/tests/testPreintegrationBase.cpp
View File

@ -43,27 +43,6 @@ static boost::shared_ptr<PreintegrationParams> Params() {
}
}
/* ************************************************************************* */
TEST(ExpressionFactor, ApplyInvDexp) {
auto model = noiseModel::Isotropic::Sigma(3, 1);
/// Functor implements ExpmapDerivative(omega).inverse() * v, with derivatives
MultiplyWithInverseFunction<Vector3, 3> applyInvDexp(SO3::ApplyExpmapDerivative);
Vector3_ f_expr(applyInvDexp, Vector3_(0), Vector3_(1));
// Check derivatives
Vector3 omega(1, 2, 3);
const Vector3 v(0.1, 0.2, 0.3);
const Vector3 expected = SO3::ExpmapDerivative(omega).inverse() * v;
CHECK(assert_equal(expected, applyInvDexp(omega,v)));
Values values;
values.insert<Vector3>(0, omega);
values.insert<Vector3>(1, v);
ExpressionFactor<Vector3> factor(model, Vector3::Zero(), f_expr);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
}
/* ************************************************************************* */
TEST(PreintegrationBase, UpdateEstimate1) {
PreintegrationBase pim(testing::Params());