FromPoseVelocity

gtsam/navigation/NavState.cpp

@@ -24,6 +24,16 @@ namespace gtsam {
 
 #define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_;
 
+//------------------------------------------------------------------------------
+NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel,
+    OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) {
+  if (H1)
+    *H1 << I_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
+  if (H2)
+    *H2 << Z_3x3, Z_3x3, pose.rotation().transpose();
+  return NavState(pose, vel);
+}
+
 //------------------------------------------------------------------------------
 const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const {
   if (H)
@@ -252,9 +262,9 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
 
 //------------------------------------------------------------------------------
 Vector9 NavState::correctPIM(const Vector9& pim, double dt,
-    const Vector3& n_gravity,
+    const Vector3& n_gravity, const boost::optional<Vector3>& omegaCoriolis,
-    const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis,
+    bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
-    OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
+    OptionalJacobian<9, 9> H2) const {
   const Rot3& nRb = R_;
   const Velocity3& n_v = v_; // derivative is Ri !
   const double dt2 = dt * dt;

gtsam/navigation/NavState.h

@@ -67,6 +67,9 @@ public:
   NavState(const Matrix3& R, const Vector9 tv) :
       R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
   }
+  /// Named constructor with derivatives
+  static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
+      OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
 
   /// @}
   /// @name Component Access

gtsam/navigation/tests/testNavState.cpp

@@ -26,6 +26,7 @@ using namespace gtsam;
 
 static const Rot3 kRotation = Rot3::RzRyRx(0.1, 0.2, 0.3);
 static const Point3 kPosition(1.0, 2.0, 3.0);
+static const Pose3 kPose(kRotation, kPosition);
 static const Velocity3 kVelocity(0.4, 0.5, 0.6);
 static const NavState kIdentity;
 static const NavState kState1(kRotation, kPosition, kVelocity);
@@ -33,6 +34,16 @@ static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04);
 static const Vector3 kGravity(0, 0, 9.81);
 static const Vector9 kZeroXi = Vector9::Zero();
 
+/* ************************************************************************* */
+TEST(NavState, Constructor) {
+  boost::function<NavState(const Pose3&, const Vector3&)> construct =
+      boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
+          boost::none);
+  Matrix aH1, aH2;
+  EXPECT(assert_equal(kState1, NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2)));
+  EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1));
+  EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2));
+}
 /* ************************************************************************* */
 TEST( NavState, Attitude) {
   Matrix39 aH, eH;
@@ -127,9 +138,14 @@ TEST( NavState, Manifold ) {
   // Check localCoordinates derivatives
   kState1.localCoordinates(state2, aH1, aH2);
   boost::function<Vector9(const NavState&, const NavState&)> localCoordinates =
-      boost::bind(&NavState::localCoordinates, _1, _2, boost::none, boost::none);
+      boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
-  EXPECT(assert_equal(numericalDerivative21(localCoordinates, kState1, state2), aH1));
+          boost::none);
-  EXPECT(assert_equal(numericalDerivative22(localCoordinates, kState1, state2), aH2));
+  EXPECT(
+      assert_equal(numericalDerivative21(localCoordinates, kState1, state2),
+          aH1));
+  EXPECT(
+      assert_equal(numericalDerivative22(localCoordinates, kState1, state2),
+          aH2));
 }
 
 /* ************************************************************************* */