gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
serialization for Vectors
Base class and basic functions for Manifold types.
typedef and functions to augment Eigen's MatrixXd
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
typedef and functions to augment Eigen's VectorXd
2D Point
3D Point
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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