gtsam 4.2.0
gtsam
Unit3.h
1/* ----------------------------------------------------------------------------
2
3 * Atlanta, Georgia 30332-0415
4 * All Rights Reserved
5 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/*
13 * @file Unit3.h
14 * @date Feb 02, 2011
15 * @author Can Erdogan
16 * @author Frank Dellaert
17 * @author Alex Trevor
18 * @brief Develop a Unit3 class - basically a point on a unit sphere
19 */
20
21#pragma once
22
25#include <gtsam/base/Manifold.h>
26#include <gtsam/base/Vector.h>
28#include <gtsam/base/Matrix.h>
29#include <gtsam/dllexport.h>
30
31#include <boost/optional.hpp>
32
33#include <random>
34#include <string>
35
36#ifdef GTSAM_USE_TBB
37#include <mutex> // std::mutex
38#endif
39
40namespace gtsam {
41
43class GTSAM_EXPORT Unit3 {
44
45private:
46
47 Vector3 p_;
48 mutable boost::optional<Matrix32> B_;
49 mutable boost::optional<Matrix62> H_B_;
50
51#ifdef GTSAM_USE_TBB
52 mutable std::mutex B_mutex_;
53#endif
54
55public:
56
57 enum {
58 dimension = 2
59 };
60
63
66 p_(1.0, 0.0, 0.0) {
67 }
68
70 explicit Unit3(const Vector3& p);
71
73 Unit3(double x, double y, double z);
74
77 explicit Unit3(const Point2& p, double f);
78
80 Unit3(const Unit3& u) {
81 p_ = u.p_;
82 }
83
85 Unit3& operator=(const Unit3 & u) {
86 p_ = u.p_;
87 B_ = u.B_;
88 H_B_ = u.H_B_;
89 return *this;
90 }
91
93 static Unit3 FromPoint3(const Point3& point, //
94 OptionalJacobian<2, 3> H = boost::none);
95
102 static Unit3 Random(std::mt19937 & rng);
103
105
108
109 friend std::ostream& operator<<(std::ostream& os, const Unit3& pair);
110
112 void print(const std::string& s = std::string()) const;
113
115 bool equals(const Unit3& s, double tol = 1e-9) const {
116 return equal_with_abs_tol(p_, s.p_, tol);
117 }
119
122
129 const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
130
132 Matrix3 skew() const;
133
135 Point3 point3(OptionalJacobian<3, 2> H = boost::none) const;
136
138 Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const;
139
141 friend Point3 operator*(double s, const Unit3& d) {
142 return Point3(s * d.p_);
143 }
144
146 double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
147 OptionalJacobian<1,2> H2 = boost::none) const;
148
151 Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
152
155 Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
156 OptionalJacobian<2, 2> H_q = boost::none) const;
157
159 double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
160
162 Unit3 cross(const Unit3& q) const {
163 return Unit3(p_.cross(q.p_));
164 }
165
167 Point3 cross(const Point3& q) const {
168 return point3().cross(q);
169 }
170
172
175
177 inline static size_t Dim() {
178 return 2;
179 }
180
182 inline size_t dim() const {
183 return 2;
184 }
185
188 RENORM
189 };
190
192 Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const;
193
195 Vector2 localCoordinates(const Unit3& s) const;
196
198
199private:
200
203
204 friend class boost::serialization::access;
205 template<class ARCHIVE>
206 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
207 ar & BOOST_SERIALIZATION_NVP(p_);
208 }
209
211
212public:
214};
215
216// Define GTSAM traits
217template<> struct traits<Unit3> : public internal::Manifold<Unit3> {
218};
219
220template<> struct traits<const Unit3> : public internal::Manifold<Unit3> {
221};
222
223} // namespace gtsam
224
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:308
serialization for Vectors
typedef and functions to augment Eigen's VectorXd
typedef and functions to augment Eigen's MatrixXd
Base class and basic functions for Manifold types.
3D Point
2D Point
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition: Point2.h:27
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:36
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:195
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition: Matrix.h:81
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:41
Represents a 3D point on a unit sphere.
Definition: Unit3.h:43
Unit3(const Unit3 &u)
Copy constructor.
Definition: Unit3.h:80
Unit3()
Default constructor.
Definition: Unit3.h:65
friend Point3 operator*(double s, const Unit3 &d)
Return scaled direction as Point3.
Definition: Unit3.h:141
static size_t Dim()
Dimensionality of tangent space = 2 DOF.
Definition: Unit3.h:177
size_t dim() const
Dimensionality of tangent space = 2 DOF.
Definition: Unit3.h:182
bool equals(const Unit3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition: Unit3.h:115
CoordinatesMode
Definition: Unit3.h:186
@ EXPMAP
Use the exponential map to retract.
Definition: Unit3.h:187
Unit3 cross(const Unit3 &q) const
Cross-product between two Unit3s.
Definition: Unit3.h:162
Point3 cross(const Point3 &q) const
Cross-product w Point3.
Definition: Unit3.h:167
Unit3 & operator=(const Unit3 &u)
Copy assignment.
Definition: Unit3.h:85