1/**
2 * @file BearingS2.h
3 *
4 * @brief Manifold measurement between two points on a unit sphere
5 *
6 * @date Jan 26, 2012
7 * @author Alex Cunningham
8 */
9
10#pragma once
11
12#include <gtsam_unstable/dllexport.h>
13#include <gtsam/geometry/Rot2.h>
14#include <gtsam/geometry/Pose3.h>
15
16namespace gtsam {
17
18class GTSAM_UNSTABLE_EXPORT BearingS2 {
19protected:
20 Rot2 azimuth_, elevation_;
21
22public:
23 static const size_t dimension = 2;
24
25 /// @name Constructors
26 /// @{
27
28 /** Default constructor - straight ahead */
29 BearingS2() {}
30
31 /** Build from components */
32 BearingS2(double azimuth, double elevation)
33 : azimuth_(Rot2::fromAngle(theta: azimuth)), elevation_(Rot2::fromAngle(theta: elevation)) {}
34
35 BearingS2(const Rot2& azimuth, const Rot2& elevation)
36 : azimuth_(azimuth), elevation_(elevation) {}
37
38 // access
39 const Rot2& azimuth() const { return azimuth_; }
40 const Rot2& elevation() const { return elevation_; }
41
42 /// @}
43 /// @name Measurements
44 /// @{
45
46 /**
47 * Observation function for downwards-facing camera
48 */
49 // FIXME: will not work for TARGET = Point3
50 template<class POSE, class TARGET>
51 static BearingS2 fromDownwardsObservation(const POSE& A, const TARGET& B) {
52 return fromDownwardsObservation(A.pose(), B.translation());
53 }
54
55 static BearingS2 fromDownwardsObservation(const Pose3& A, const Point3& B);
56
57 /** Observation function with standard, forwards-facing camera */
58 static BearingS2 fromForwardObservation(const Pose3& A, const Point3& B);
59
60 /// @}
61 /// @name Testable
62 /// @{
63
64 /** print with optional string */
65 void print(const std::string& s = "") const;
66
67 /** assert equality up to a tolerance */
68 bool equals(const BearingS2& x, double tol = 1e-9) const;
69
70 /// @}
71 /// @name Manifold
72 /// @{
73
74 /// Dimensionality of tangent space = 2 DOF - used to autodetect sizes
75 inline static size_t Dim() { return dimension; }
76
77 /// Dimensionality of tangent space = 2 DOF
78 inline size_t dim() const { return dimension; }
79
80 /// Retraction from R^2 to BearingS2 manifold neighborhood around current pose
81 /// Tangent space parameterization is [azimuth elevation]
82 BearingS2 retract(const Vector& v) const;
83
84 /// Local coordinates of BearingS2 manifold neighborhood around current pose
85 Vector localCoordinates(const BearingS2& p2) const;
86
87private:
88
89 /// @}
90 /// @name Advanced Interface
91 /// @{
92
93#if GTSAM_ENABLE_BOOST_SERIALIZATION //
94 // Serialization function
95 friend class boost::serialization::access;
96 template<class Archive>
97 void serialize(Archive & ar, const unsigned int /*version*/) {
98 ar & BOOST_SERIALIZATION_NVP(azimuth_);
99 ar & BOOST_SERIALIZATION_NVP(elevation_);
100 }
101#endif
102
103};
104
105/// traits
106template<> struct traits<BearingS2> : public Testable<BearingS2> {};
107
108} // \namespace gtsam
109