1/**
2 * @file Pose3Upright.h
3 *
4 * @brief Variation of a Pose3 in which the rotation is constained to purely yaw
5 * This state is essentially a Pose2 with a z component, with conversions to
6 * higher and lower dimensional states.
7 *
8 * @date Jan 24, 2012
9 * @author Alex Cunningham
10 */
11
12#pragma once
13
14#include <gtsam_unstable/dllexport.h>
15#include <gtsam/geometry/Pose3.h>
16#include <gtsam/geometry/Pose2.h>
17
18namespace gtsam {
19
20/**
21 * A 3D Pose with fixed pitch and roll
22 * @ingroup geometry
23 * \nosubgrouping
24 */
25class GTSAM_UNSTABLE_EXPORT Pose3Upright {
26public:
27 static const size_t dimension = 4;
28
29protected:
30
31 Pose2 T_;
32 double z_;
33
34public:
35 /// @name Standard Constructors
36 /// @{
37
38 /// Default constructor initializes at origin
39 Pose3Upright() : z_(0.0) {}
40
41 /// Copy constructor
42 Pose3Upright(const Pose3Upright& x) : T_(x.T_), z_(x.z_) {}
43 Pose3Upright(const Rot2& bearing, const Point3& t);
44 Pose3Upright(double x, double y, double z, double theta);
45 Pose3Upright(const Pose2& pose, double z);
46 Pose3Upright& operator=(const Pose3Upright& x) = default;
47
48 /// Down-converts from a full Pose3
49 Pose3Upright(const Pose3& fullpose);
50
51 /// @}
52 /// @name Testable
53 /// @{
54
55 /** print with optional string */
56 void print(const std::string& s = "") const;
57
58 /** assert equality up to a tolerance */
59 bool equals(const Pose3Upright& pose, double tol = 1e-9) const;
60
61 /// @}
62 /// @name Standard Interface
63 /// @{
64
65 double x() const { return T_.x(); }
66 double y() const { return T_.y(); }
67 double z() const { return z_; }
68 double theta() const { return T_.theta(); }
69
70 Point2 translation2() const;
71 Point3 translation() const;
72 Rot2 rotation2() const;
73 Rot3 rotation() const;
74 Pose2 pose2() const;
75 Pose3 pose() const;
76
77 /// @}
78 /// @name Manifold
79 /// @{
80
81 /// Dimensionality of tangent space = 4 DOF - used to autodetect sizes
82 inline static size_t Dim() { return dimension; }
83
84 /// Dimensionality of tangent space = 4 DOF
85 inline size_t dim() const { return dimension; }
86
87 /// Retraction from R^4 to Pose3Upright manifold neighborhood around current pose
88 /// Tangent space parameterization is [x y z theta]
89 Pose3Upright retract(const Vector& v) const;
90
91 /// Local 3D coordinates of Pose3Upright manifold neighborhood around current pose
92 Vector localCoordinates(const Pose3Upright& p2) const;
93
94 /// @}
95 /// @name Group
96 /// @{
97
98 /// identity for group operation
99 static Pose3Upright Identity() { return Pose3Upright(); }
100
101 /// inverse transformation with derivatives
102 Pose3Upright inverse(OptionalJacobian<4,4> H1={}) const;
103
104 ///compose this transformation onto another (first *this and then p2)
105 Pose3Upright compose(const Pose3Upright& p2,
106 OptionalJacobian<4,4> H1={},
107 OptionalJacobian<4,4> H2={}) const;
108
109 /// compose syntactic sugar
110 inline Pose3Upright operator*(const Pose3Upright& T) const { return compose(p2: T); }
111
112 /**
113 * Return relative pose between p1 and p2, in p1 coordinate frame
114 * as well as optionally the derivatives
115 */
116 Pose3Upright between(const Pose3Upright& p2,
117 OptionalJacobian<4,4> H1={},
118 OptionalJacobian<4,4> H2={}) const;
119
120 /// @}
121 /// @name Lie Group
122 /// @{
123
124 /// Exponential map at identity - create a rotation from canonical coordinates
125 static Pose3Upright Expmap(const Vector& xi);
126
127 /// Log map at identity - return the canonical coordinates of this rotation
128 static Vector Logmap(const Pose3Upright& p);
129
130 /// @}
131
132private:
133
134#if GTSAM_ENABLE_BOOST_SERIALIZATION //
135 // Serialization function
136 friend class boost::serialization::access;
137 template<class Archive>
138 void serialize(Archive & ar, const unsigned int /*version*/) {
139 ar & BOOST_SERIALIZATION_NVP(T_);
140 ar & BOOST_SERIALIZATION_NVP(z_);
141 }
142#endif
143
144}; // \class Pose3Upright
145
146template<>
147struct traits<Pose3Upright> : public internal::Manifold<Pose3Upright> {};
148
149
150} // \namespace gtsam
151