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:


#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(azimuth_);
    ar & BOOST_SERIALIZATION_NVP(elevation_);
  }
#endif

};

template<> struct traits<BearingS2> : public Testable<BearingS2> {};

} // \namespace gtsam