Merge pull request #32 from borglab/feature/navigation_expressions
Feature/navigation expressionsrelease/4.3a0
						commit
						c1c456b021
					
				|  | @ -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); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
|  | @ -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_; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue