Merge pull request #32 from borglab/feature/navigation_expressions

Feature/navigation expressions

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 <gtsam/geometry/Rot3.h>
+#include <gtsam/navigation/NavState.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam/slam/expressions.h>
+
+namespace gtsam {
+
+typedef Expression<NavState> NavState_;
+typedef Expression<Velocity3> 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

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 <gtsam/geometry/Rot3.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/navigation/NavState.h>
+#include <gtsam/navigation/expressions.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/expressions.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace gtsam;
+
+// A NavState unknown expression wXb with key 42
+Expression<NavState> 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);
+}
+/* ************************************************************************* */

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<Vector3> rotate(const Rot3_& R,
+                                  const Expression<Vector3>& v) {
+  return Expression<Vector3>(internal::rotate, R, v);
+}
+inline Expression<Vector3> unrotate(const Rot3_& R,
+                                    const Expression<Vector3>& v) {
+  return Expression<Vector3>(internal::unrotate, R, v);
+}
+#endif
+
 // Projection
 
 typedef Expression<Cal3_S2> Cal3_S2_;