BearingS2.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam_unstable/dllexport.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Pose3.h>
 
namespace gtsam {
 
class GTSAM_UNSTABLE_EXPORT BearingS2 {
protected:
  Rot2 azimuth_, elevation_;
 
public:
  static const size_t dimension = 2;


  BearingS2() {}

  BearingS2(double azimuth, double elevation)
  : azimuth_(Rot2::fromAngle(azimuth)), elevation_(Rot2::fromAngle(elevation)) {}
 
  BearingS2(const Rot2& azimuth, const Rot2& elevation)
  : azimuth_(azimuth), elevation_(elevation) {}
 
  // access
  const Rot2& azimuth() const { return azimuth_; }
  const Rot2& elevation() const { return elevation_; }


  // FIXME: will not work for TARGET = Point3
  template<class POSE, class TARGET>
  static BearingS2 fromDownwardsObservation(const POSE& A, const TARGET& B) {
    return fromDownwardsObservation(A.pose(), B.translation());
  }
 
  static BearingS2 fromDownwardsObservation(const Pose3& A, const Point3& B);

  static BearingS2 fromForwardObservation(const Pose3& A, const Point3& B);


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

  bool equals(const BearingS2& x, double tol = 1e-9) const;


  inline static size_t Dim() { return dimension; }

  inline size_t dim() const { return dimension; }

  BearingS2 retract(const Vector& v) const;

  Vector localCoordinates(const BearingS2& p2) const;
 
private:

 
  // Serialization function
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(azimuth_);
    ar & BOOST_SERIALIZATION_NVP(elevation_);
  }
 
};

template<> struct traits<BearingS2> : public Testable<BearingS2> {};
 
} // \namespace gtsam