Pose3Upright.h

Go to the documentation of this file.

#pragma once

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

namespace gtsam {

class GTSAM_UNSTABLE_EXPORT Pose3Upright {
public:
  static const size_t dimension = 4;

protected:

  Pose2 T_;
  double z_;

public:

  Pose3Upright() : z_(0.0) {}

  Pose3Upright(const Pose3Upright& x) : T_(x.T_), z_(x.z_) {}
  Pose3Upright(const Rot2& bearing, const Point3& t);
  Pose3Upright(double x, double y, double z, double theta);
  Pose3Upright(const Pose2& pose, double z);

  Pose3Upright(const Pose3& fullpose);


  void print(const std::string& s = "") const;

  bool equals(const Pose3Upright& pose, double tol = 1e-9) const;


  double x() const { return T_.x(); }
  double y() const { return T_.y(); }
  double z() const { return z_; }
  double theta() const { return T_.theta(); }

  Point2 translation2() const;
  Point3 translation() const;
  Rot2 rotation2() const;
  Rot3 rotation() const;
  Pose2 pose2() const;
  Pose3 pose() const;


  inline static size_t Dim() { return dimension; }

  inline size_t dim() const { return dimension; }

  Pose3Upright retract(const Vector& v) const;

  Vector localCoordinates(const Pose3Upright& p2) const;


  static Pose3Upright Identity() { return Pose3Upright(); }

  Pose3Upright inverse(OptionalJacobian<4,4> H1={}) const;

  Pose3Upright compose(const Pose3Upright& p2,
      OptionalJacobian<4,4> H1={},
      OptionalJacobian<4,4> H2={}) const;

  inline Pose3Upright operator*(const Pose3Upright& T) const { return compose(T); }

  Pose3Upright between(const Pose3Upright& p2,
      OptionalJacobian<4,4> H1={},
      OptionalJacobian<4,4> H2={}) const;


  static Pose3Upright Expmap(const Vector& xi);

  static Vector Logmap(const Pose3Upright& p);


private:

#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  //
  // Serialization function
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(T_);
    ar & BOOST_SERIALIZATION_NVP(z_);
  }
#endif

}; // \class Pose3Upright

template<>
struct traits<Pose3Upright> : public internal::Manifold<Pose3Upright> {};


} // \namespace gtsam