Merge pull request #32 from borglab/feature/navigation_expressions

Feature/navigation expressions
release/4.3a0
Frank Dellaert 2019-05-30 11:12:20 -04:00 committed by GitHub
commit c1c456b021
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 139 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);
}
/* ************************************************************************* */

View File

@ -41,6 +41,7 @@ inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) {
}
namespace internal {
// define getter that returns value rather than reference
Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) {
return pose.rotation(H);
}
@ -54,10 +55,42 @@ inline Point3_ rotate(const Rot3_& x, const Point3_& p) {
return Point3_(x, &Rot3::rotate, p);
}
inline Unit3_ rotate(const Rot3_& x, const Unit3_& p) {
return Unit3_(x, &Rot3::rotate, p);
}
inline Point3_ unrotate(const Rot3_& x, const Point3_& p) {
return Point3_(x, &Rot3::unrotate, p);
}
inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) {
return Unit3_(x, &Rot3::unrotate, p);
}
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
namespace internal {
// define a rotate and unrotate for Vector3
Vector3 rotate(const Rot3& R, const Vector3& v,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) {
return R.rotate(v, H1, H2);
}
Vector3 unrotate(const Rot3& R, const Vector3& v,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) {
return R.unrotate(v, H1, H2);
}
} // namespace internal
inline Expression<Vector3> rotate(const Rot3_& R,
const Expression<Vector3>& v) {
return Expression<Vector3>(internal::rotate, R, v);
}
inline Expression<Vector3> unrotate(const Rot3_& R,
const Expression<Vector3>& v) {
return Expression<Vector3>(internal::unrotate, R, v);
}
#endif
// Projection
typedef Expression<Cal3_S2> Cal3_S2_;