PoseRTV.h

Go to the documentation of this file.

#pragma once

#include <gtsam_unstable/dllexport.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/ProductLieGroup.h>

namespace gtsam {

typedef Vector3 Velocity3;

class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup<Pose3,Velocity3> {
protected:

  typedef ProductLieGroup<Pose3,Velocity3> Base;
  typedef OptionalJacobian<9, 9> ChartJacobian;

public:

  // constructors - with partial versions
  PoseRTV() {}
  PoseRTV(const Point3& t, const Rot3& rot, const Velocity3& vel)
  : Base(Pose3(rot, t), vel) {}
  PoseRTV(const Rot3& rot, const Point3& t, const Velocity3& vel)
  : Base(Pose3(rot, t), vel) {}
  explicit PoseRTV(const Point3& t)
  : Base(Pose3(Rot3(), t),Vector3::Zero()) {}
  PoseRTV(const Pose3& pose, const Velocity3& vel)
  : Base(pose, vel) {}
  explicit PoseRTV(const Pose3& pose)
  : Base(pose,Vector3::Zero()) {}

  // Construct from Base
  PoseRTV(const Base& base)
  : Base(base) {}

  PoseRTV(double roll, double pitch, double yaw, double x, double y, double z,
      double vx, double vy, double vz);

  explicit PoseRTV(const Vector& v);

  // access
  const Pose3& pose() const { return first; }
  const Velocity3& v() const { return second; }
  const Point3& t() const { return pose().translation(); }
  const Rot3& R() const { return pose().rotation(); }

  // longer function names
  const Point3& translation() const { return pose().translation(); }
  const Rot3& rotation() const { return pose().rotation(); }
  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 pose().translation(); }
  const Velocity3& velocityVec() const { return velocity(); }

  // testable
  bool equals(const PoseRTV& other, double tol=1e-6) const;
  void print(const std::string& s="") const;

  using Base::dimension;
  using Base::dim;
  using Base::Dim;
  using Base::retract;
  using Base::localCoordinates;
  using Base::LocalCoordinates;


  double range(const PoseRTV& other,
               OptionalJacobian<1,9> H1=boost::none,
               OptionalJacobian<1,9> H2=boost::none) const;


  PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const;

  PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const;

  PoseRTV generalDynamics(const Vector& accel, const Vector& gyro, double dt) const;

  Vector6 imuPrediction(const PoseRTV& x2, double dt) const;

  Point3 translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const;

  inline Point3 translationIntegration(const PoseRTV& x2, double dt) const {
    return translationIntegration(x2.rotation(), x2.velocity(), dt);
  }

  inline Vector translationIntegrationVec(const PoseRTV& x2, double dt) const {
    return translationIntegration(x2, dt);
  }

  PoseRTV transformed_from(const Pose3& trans,
      ChartJacobian Dglobal = boost::none,
      OptionalJacobian<9, 6> Dtrans = boost::none) const;


  static Matrix RRTMbn(const Vector3& euler);
  static Matrix RRTMbn(const Rot3& att);

  static Matrix RRTMnb(const Vector3& euler);
  static Matrix RRTMnb(const Rot3& att);

private:
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(first);
    ar & BOOST_SERIALIZATION_NVP(second);
  }
};


template<>
struct traits<PoseRTV> : public internal::LieGroup<PoseRTV> {};

// Define Range functor specializations that are used in RangeFactor
template <typename A1, typename A2> struct Range;

template<>
struct Range<PoseRTV, PoseRTV> : HasRange<PoseRTV, PoseRTV, double> {};

} // \namespace gtsam