From 8126e6b51daa2e3e10be4df486cd0422772e7fe1 Mon Sep 17 00:00:00 2001
From: Frank <frank@skyd.io>
Date: Wed, 27 Jan 2016 15:15:33 -0800
Subject: [PATCH] add navState method

---
 gtsam/navigation/Scenario.h                | 3 ++-
 python/handwritten/navigation/Scenario.cpp | 1 +
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h
index 5544ae4b3..ad684f5f8 100644
--- a/gtsam/navigation/Scenario.h
+++ b/gtsam/navigation/Scenario.h
@@ -17,7 +17,7 @@
 
 #pragma once
 #include <gtsam/linear/NoiseModel.h>
-#include <gtsam/geometry/Pose3.h>
+#include <gtsam/navigation/NavState.h>
 
 namespace gtsam {
 
@@ -34,6 +34,7 @@ class Scenario {
   // Derived quantities:
 
   Rot3 rotation(double t) const { return pose(t).rotation(); }
+  NavState navState(double t) const { return NavState(pose(t), velocity_n(t)); }
 
   Vector3 velocity_b(double t) const {
     const Rot3 nRb = rotation(t);
diff --git a/python/handwritten/navigation/Scenario.cpp b/python/handwritten/navigation/Scenario.cpp
index a49a02cc0..e11f04a57 100644
--- a/python/handwritten/navigation/Scenario.cpp
+++ b/python/handwritten/navigation/Scenario.cpp
@@ -40,6 +40,7 @@ void exportScenario() {
       .def("omega_b", &Scenario::omega_b)
       .def("velocity_n", &Scenario::velocity_n)
       .def("acceleration_n", &Scenario::acceleration_n)
+      .def("navState", &Scenario::navState)
       .def("rotation", &Scenario::rotation)
       .def("velocity_b", &Scenario::velocity_b)
       .def("acceleration_b", &Scenario::acceleration_b);