Added handy NavState expressions
parent
34c6348d45
commit
ef8cddd455
|
@ -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
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
Loading…
Reference in New Issue