PoseRTV as ProductManifold works

gtsam_unstable/dynamics/PoseRTV.cpp

@@ -14,7 +14,7 @@ namespace gtsam {
 
 using namespace std;
 
-static const Vector g = delta(3, 2, 9.81);
+static const Vector kGravity = delta(3, 2, 9.81);
 
 /* ************************************************************************* */
 double bound(double a, double min, double max) {
@@ -24,28 +24,30 @@ double bound(double a, double min, double max) {
 }
 
 /* ************************************************************************* */
-PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, double z,
+PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y,
-    double vx, double vy, double vz)
+    double z, double vx, double vy, double vz) :
-: Rt_(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), v_(vx, vy, vz) {}
+    Base(Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)),
+        Velocity3(vx, vy, vz)) {
+}
 
 /* ************************************************************************* */
-PoseRTV::PoseRTV(const Vector& rtv)
+PoseRTV::PoseRTV(const Vector& rtv) :
-: Rt_(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), v_(rtv.tail(3))
+    Base(Pose3(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))),
-{
+        Velocity3(rtv.tail(3))) {
 }
 
 /* ************************************************************************* */
 Vector PoseRTV::vector() const {
   Vector rtv(9);
-  rtv.head(3) = Rt_.rotation().xyz();
+  rtv.head(3) = rotation().xyz();
-  rtv.segment(3,3) = Rt_.translation().vector();
+  rtv.segment(3,3) = translation().vector();
-  rtv.tail(3) = v_.vector();
+  rtv.tail(3) = velocity().vector();
   return rtv;
 }
 
 /* ************************************************************************* */
 bool PoseRTV::equals(const PoseRTV& other, double tol) const {
-  return Rt_.equals(other.Rt_, tol) && v_.equals(other.v_, tol);
+  return pose().equals(other.pose(), tol) && velocity().equals(other.velocity(), tol);
 }
 
 /* ************************************************************************* */
@@ -53,7 +55,7 @@ void PoseRTV::print(const string& s) const {
   cout << s << ":" << endl;
   gtsam::print((Vector)R().xyz(), "  R:rpy");
   t().print("  T");
-  v_.print("  V");
+  velocity().print("  V");
 }
 
 /* ************************************************************************* */
@@ -67,8 +69,8 @@ PoseRTV PoseRTV::Expmap(const Vector9& v, ChartJacobian H) {
 /* ************************************************************************* */
 Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) {
   if (H) CONCEPT_NOT_IMPLEMENTED;
-  Vector6 Lx = Pose3::Logmap(p.Rt_);
+  Vector6 Lx = Pose3::Logmap(p.pose());
-  Vector3 Lv = p.v_.vector();
+  Vector3 Lv = p.velocity().vector();
   return (Vector9() << Lx, Lv).finished();
 }
 
@@ -79,8 +81,8 @@ PoseRTV PoseRTV::retract(const Vector& v,
   if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED;
   assert(v.size() == 9);
   // First order approximation
-  Pose3 newPose = Rt_.retract(sub(v, 0, 6));
+  Pose3 newPose = pose().retract(sub(v, 0, 6));
-  Velocity3 newVel = v_ + Rt_.rotation() * Point3(sub(v, 6, 9));
+  Velocity3 newVel = velocity() + rotation() * Point3(sub(v, 6, 9));
   return PoseRTV(newPose, newVel);
 }
 
@@ -92,7 +94,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1,
   const Pose3& x0 = pose(), &x1 = p1.pose();
   // First order approximation
   Vector6 poseLogmap = x0.localCoordinates(x1);
-  Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector();
+  Vector3 lv = rotation().unrotate(p1.velocity() - velocity()).vector();
   return (Vector(9) << poseLogmap, lv).finished();
 }
 
@@ -100,7 +102,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1,
 PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); }
 PoseRTV PoseRTV::inverse(ChartJacobian H1) const {
   if (H1) *H1 = numericalDerivative11<PoseRTV,PoseRTV>(inverse_, *this, 1e-5);
-  return PoseRTV(Rt_.inverse(), - v_);
+  return PoseRTV(pose().inverse(), - velocity());
 }
 
 /* ************************************************************************* */
@@ -109,7 +111,7 @@ PoseRTV PoseRTV::compose(const PoseRTV& p, ChartJacobian H1,
     ChartJacobian H2) const {
   if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5);
   if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5);
-  return PoseRTV(Rt_.compose(p.Rt_), v_+p.v_);
+  return PoseRTV(pose().compose(p.pose()), velocity()+p.velocity());
 }
 
 /* ************************************************************************* */
@@ -187,7 +189,7 @@ PoseRTV PoseRTV::generalDynamics(
   Rot3 r2 = rotation().retract(gyro * dt);
 
   //  Integrate Velocity Equations
-  Velocity3 v2 = v_ + (Velocity3(dt * (r2.matrix() * accel + g)));
+  Velocity3 v2 = velocity() + Velocity3(dt * (r2.matrix() * accel + kGravity));
 
   //  Integrate Position Equations
   Point3 t2 = translationIntegration(r2, v2, dt);
@@ -205,7 +207,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
 
   // acceleration
   Vector3 accel = (v2-v1).vector() / dt;
-  imu.head<3>() = r2.transpose() * (accel - g);
+  imu.head<3>() = r2.transpose() * (accel - kGravity);
 
   // rotation rates
   // just using euler angles based on matlab code
@@ -249,7 +251,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
     OptionalJacobian<9, 6> Dtrans) const {
   // Note that we rotate the velocity
   Matrix DVr, DTt;
-  Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt);
+  Velocity3 newvel = trans.rotation().rotate(velocity(), DVr, DTt);
   if (!Dglobal && !Dtrans)
     return PoseRTV(trans.compose(pose()), newvel);
 
@@ -273,7 +275,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
     //    insertSub(*Dtrans, DTc, 0, 0); // correct in tests
     //
     //    // rotating the velocity
-    //    Matrix vRhat = skewSymmetric(-v_.x(), -v_.y(), -v_.z());
+    //    Matrix vRhat = skewSymmetric(-velocity().x(), -velocity().y(), -velocity().z());
     //    trans.rotation().print("Transform rotation");
     //    gtsam::print(vRhat, "vRhat");
     //    gtsam::print(DVr, "DVr");

gtsam_unstable/dynamics/PoseRTV.h

@@ -7,11 +7,46 @@
 #pragma once
 
 #include <gtsam_unstable/base/dllexport.h>
-#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/Lie.h>
 
 namespace gtsam {
 
+/// CRTP to construct the product Lie group of two other Lie groups, G and H
+/// Assumes manifold structure from G and H, and binary constructor
+template<class Derived, typename G, typename H>
+class ProductLieGroup: public ProductManifold<Derived, G, H> {
+  BOOST_CONCEPT_ASSERT((IsLieGroup<G>));
+  BOOST_CONCEPT_ASSERT((IsLieGroup<H>));
+  typedef ProductManifold<Derived, G, H> Base;
+
+public:
+  enum {dimension = G::dimension + H::dimension};
+  inline static size_t Dim() {return dimension;}
+  inline size_t dim() const {return dimension;}
+
+  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
+
+  /// Default constructor yields identity
+  ProductLieGroup():Base(traits<G>::Identity(),traits<H>::Identity()) {}
+
+  // Construct from two subgroup elements
+  ProductLieGroup(const G& g, const H& h):Base(g,h) {}
+
+  ProductLieGroup operator*(const Derived& other) const {
+    return Derived(traits<G>::Compose(this->g(),other.g()), traits<H>::Compose(this->h(),other.h()));
+  }
+  ProductLieGroup inverse() const {
+    return Derived(this->g().inverse(), this->h().inverse());
+  }
+};
+
+// Define any direct product group to be a model of the multiplicative Group concept
+template<class Derived, typename G, typename H>
+struct traits<ProductLieGroup<Derived, G, H> > : internal::LieGroupTraits<
+    ProductLieGroup<Derived, G, H> > {
+};
+
 /// Syntactic sugar to clarify components
 typedef Point3 Velocity3;
 
@@ -19,12 +54,10 @@ typedef Point3 Velocity3;
  * Robot state for use with IMU measurements
  * - contains translation, translational velocity and rotation
  */
-class GTSAM_UNSTABLE_EXPORT PoseRTV {
+class GTSAM_UNSTABLE_EXPORT PoseRTV : private ProductLieGroup<PoseRTV,Pose3,Velocity3> {
 protected:
 
-  Pose3 Rt_;
+  typedef ProductLieGroup<PoseRTV,Pose3,Velocity3> Base;
-  Velocity3 v_;
-
   typedef OptionalJacobian<9, 9> ChartJacobian;
 
 public:
@@ -32,16 +65,16 @@ public:
 
   // constructors - with partial versions
   PoseRTV() {}
-  PoseRTV(const Point3& pt, const Rot3& rot, const Velocity3& vel)
+  PoseRTV(const Point3& t, const Rot3& rot, const Velocity3& vel)
-  : Rt_(rot, pt), v_(vel) {}
+  : Base(Pose3(rot, t), vel) {}
-  PoseRTV(const Rot3& rot, const Point3& pt, const Velocity3& vel)
+  PoseRTV(const Rot3& rot, const Point3& t, const Velocity3& vel)
-  : Rt_(rot, pt), v_(vel) {}
+  : Base(Pose3(rot, t), vel) {}
-  explicit PoseRTV(const Point3& pt)
+  explicit PoseRTV(const Point3& t)
-  : Rt_(Rot3::identity(), pt) {}
+  : Base(Pose3(Rot3(), t),Velocity3()) {}
   PoseRTV(const Pose3& pose, const Velocity3& vel)
-  : Rt_(pose), v_(vel) {}
+  : Base(pose, vel) {}
   explicit PoseRTV(const Pose3& pose)
-  : Rt_(pose) {}
+  : Base(pose,Velocity3()) {}
 
   /** build from components - useful for data files */
   PoseRTV(double roll, double pitch, double yaw, double x, double y, double z,
@@ -51,21 +84,21 @@ public:
   explicit PoseRTV(const Vector& v);
 
   // access
-  const Point3& t() const { return Rt_.translation(); }
+  const Pose3& pose() const { return first; }
-  const Rot3& R() const { return Rt_.rotation(); }
+  const Velocity3& v() const { return second; }
-  const Velocity3& v() const { return v_; }
+  const Point3& t() const { return pose().translation(); }
-  const Pose3& pose() const { return Rt_; }
+  const Rot3& R() const { return pose().rotation(); }
 
   // longer function names
-  const Point3& translation() const { return Rt_.translation(); }
+  const Point3& translation() const { return pose().translation(); }
-  const Rot3& rotation() const { return Rt_.rotation(); }
+  const Rot3& rotation() const { return pose().rotation(); }
-  const Velocity3& velocity() const { return v_; }
+  const Velocity3& velocity() const { return second; }
 
   // Access to vector for ease of use with Matlab
   // and avoidance of Point3
   Vector vector() const;
-  Vector translationVec() const { return Rt_.translation().vector(); }
+  Vector translationVec() const { return pose().translation().vector(); }
-  Vector velocityVec() const { return v_.vector(); }
+  Vector velocityVec() const { return velocity().vector(); }
 
   // testable
   bool equals(const PoseRTV& other, double tol=1e-6) const;
@@ -183,8 +216,8 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_NVP(Rt_);
+    ar & BOOST_SERIALIZATION_NVP(first);
-    ar & BOOST_SERIALIZATION_NVP(v_);
+    ar & BOOST_SERIALIZATION_NVP(second);
   }
 };