diff --git a/gtsam/navigation/expressions.h b/gtsam/navigation/expressions.h new file mode 100644 index 000000000..231ccb20d --- /dev/null +++ b/gtsam/navigation/expressions.h @@ -0,0 +1,44 @@ +/** + * @file expressions.h + * @brief Common expressions for solving navigation problems + * @date May, 2019 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +typedef Expression NavState_; +typedef Expression Velocity3_; + +namespace internal { +// define getters that return a value rather than a reference +Rot3 attitude(const NavState& X, OptionalJacobian<3, 9> H) { + return X.attitude(H); +} +Point3 position(const NavState& X, OptionalJacobian<3, 9> H) { + return X.position(H); +} +Velocity3 velocity(const NavState& X, OptionalJacobian<3, 9> H) { + return X.velocity(H); +} +} // namespace internal + +// overloads for getters +inline Rot3_ attitude(const NavState_& X) { + return Rot3_(internal::attitude, X); +} +inline Point3_ position(const NavState_& X) { + return Point3_(internal::position, X); +} +inline Velocity3_ velocity(const NavState_& X) { + return Velocity3_(internal::velocity, X); +} + +} // namespace gtsam diff --git a/gtsam/navigation/tests/testNavExpressions.cpp b/gtsam/navigation/tests/testNavExpressions.cpp new file mode 100644 index 000000000..374732fd4 --- /dev/null +++ b/gtsam/navigation/tests/testNavExpressions.cpp @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testExpressions.cpp + * @date May 2019 + * @author Frank Dellaert + * @brief unit tests for navigation expression helpers + */ + +#include +#include +#include +#include +#include +#include + +#include + +using namespace gtsam; + +// A NavState unknown expression wXb with key 42 +Expression wXb_(42); + +/* ************************************************************************* */ +// Example: absolute position measurement +TEST(Expressions, Position) { + auto absolutePosition_ = position(wXb_); + + // Test with some values + Values values; + values.insert(42, NavState(Rot3(), Point3(1, 2, 3), Velocity3(4, 5, 6))); + EXPECT(assert_equal(Point3(1, 2, 3), absolutePosition_.value(values))); +} + +/* ************************************************************************* */ +// Example: velocity measurement in body frame +TEST(Expressions, Velocity) { + // We want to predict h(wXb) = velocity in body frame + // h(wXb) = bRw(wXb) * wV(wXb) + auto bodyVelocity_ = unrotate(attitude(wXb_), velocity(wXb_)); + + // Test with some values + Values values; + values.insert(42, NavState(Rot3(), Point3(1, 2, 3), Velocity3(4, 5, 6))); + EXPECT(assert_equal(Velocity3(4, 5, 6), bodyVelocity_.value(values))); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */