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

add navState method

release/4.3a0
Frank 2016-01-27 15:15:33 -08:00
parent ac6fb495a6
commit 8126e6b51d
2 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/navigation/Scenario.h
View File

@ -17,7 +17,7 @@
#pragma once #pragma once
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/navigation/NavState.h>
namespace gtsam { namespace gtsam {
@ -34,6 +34,7 @@ class Scenario {
// Derived quantities: // Derived quantities:
Rot3 rotation(double t) const { return pose(t).rotation(); } 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 { Vector3 velocity_b(double t) const {
const Rot3 nRb = rotation(t); const Rot3 nRb = rotation(t);

1
python/handwritten/navigation/Scenario.cpp
View File

@ -40,6 +40,7 @@ void exportScenario() {
.def("omega_b", &Scenario::omega_b) .def("omega_b", &Scenario::omega_b)
.def("velocity_n", &Scenario::velocity_n) .def("velocity_n", &Scenario::velocity_n)
.def("acceleration_n", &Scenario::acceleration_n) .def("acceleration_n", &Scenario::acceleration_n)
.def("navState", &Scenario::navState)
.def("rotation", &Scenario::rotation) .def("rotation", &Scenario::rotation)
.def("velocity_b", &Scenario::velocity_b) .def("velocity_b", &Scenario::velocity_b)
.def("acceleration_b", &Scenario::acceleration_b); .def("acceleration_b", &Scenario::acceleration_b);