From 34c6348d456295292f82d8c12c1e114eec92daa4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 May 2019 10:51:16 -0400 Subject: [PATCH 1/2] Added some more overloads, including if Point3 != Vector3 --- gtsam/slam/expressions.h | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index b0791b506c..17a8d849f7 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -41,6 +41,7 @@ inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) { } namespace internal { +// define getter that returns value rather than reference Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) { return pose.rotation(H); } @@ -54,10 +55,42 @@ inline Point3_ rotate(const Rot3_& x, const Point3_& p) { return Point3_(x, &Rot3::rotate, p); } +inline Unit3_ rotate(const Rot3_& x, const Unit3_& p) { + return Unit3_(x, &Rot3::rotate, p); +} + inline Point3_ unrotate(const Rot3_& x, const Point3_& p) { return Point3_(x, &Rot3::unrotate, p); } +inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) { + return Unit3_(x, &Rot3::unrotate, p); +} + +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS +namespace internal { +// define a rotate and unrotate for Vector3 +Vector3 rotate(const Rot3& R, const Vector3& v, + OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) { + return R.rotate(v, H1, H2); +} +Vector3 unrotate(const Rot3& R, const Vector3& v, + OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) { + return R.unrotate(v, H1, H2); +} +} // namespace internal +inline Expression rotate(const Rot3_& R, + const Expression& v) { + return Expression(internal::rotate, R, v); +} +inline Expression unrotate(const Rot3_& R, + const Expression& v) { + return Expression(internal::unrotate, R, v); +} +#endif + // Projection typedef Expression Cal3_S2_; From ef8cddd455d4316e806569ce7efe81fbe93b71e9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 30 May 2019 10:52:06 -0400 Subject: [PATCH 2/2] Added handy NavState expressions --- gtsam/navigation/expressions.h | 44 +++++++++++++ gtsam/navigation/tests/testNavExpressions.cpp | 62 +++++++++++++++++++ 2 files changed, 106 insertions(+) create mode 100644 gtsam/navigation/expressions.h create mode 100644 gtsam/navigation/tests/testNavExpressions.cpp diff --git a/gtsam/navigation/expressions.h b/gtsam/navigation/expressions.h new file mode 100644 index 0000000000..231ccb20d2 --- /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 0000000000..374732fd4a --- /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); +} +/* ************************************************************************* */