testNavExpressions.cpp

Go to the documentation of this file.

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