Added handy NavState expressions

release/4.3a0
Frank Dellaert 2019-05-30 10:52:06 -04:00
parent 34c6348d45
commit ef8cddd455
2 changed files with 106 additions and 0 deletions

View File

@ -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

View File

@ -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);
}
/* ************************************************************************* */