Unit3.h
Go to the documentation of this file.
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 
23 #include <gtsam/geometry/Point2.h>
24 #include <gtsam/geometry/Point3.h>
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 
32 #include <random>
33 #include <string>
34 
35 #ifdef GTSAM_USE_TBB
36 #include <mutex> // std::mutex
37 #endif
38 
39 namespace gtsam {
40 
42 class GTSAM_EXPORT Unit3 {
43 
44 private:
45 
47  mutable std::optional<Matrix32> B_;
48  mutable std::optional<Matrix62> H_B_;
49 
50 #ifdef GTSAM_USE_TBB
51  mutable std::mutex B_mutex_;
52 #endif
53 
54 public:
55 
56  enum {
57  dimension = 2
58  };
59 
62 
64  Unit3() :
65  p_(1.0, 0.0, 0.0) {
66  }
67 
69  explicit Unit3(const Vector3& p);
70 
72  Unit3(double x, double y, double z);
73 
76  explicit Unit3(const Point2& p, double f);
77 
79  Unit3(const Unit3& u) {
80  p_ = u.p_;
81  }
82 
84  Unit3& operator=(const Unit3 & u) {
85  p_ = u.p_;
86  B_ = u.B_;
87  H_B_ = u.H_B_;
88  return *this;
89  }
90 
92  static Unit3 FromPoint3(const Point3& point, //
94 
101  static Unit3 Random(std::mt19937 & rng);
102 
104 
107 
108  friend std::ostream& operator<<(std::ostream& os, const Unit3& pair);
109 
111  void print(const std::string& s = std::string()) const;
112 
114  bool equals(const Unit3& s, double tol = 1e-9) const {
115  return equal_with_abs_tol(p_, s.p_, tol);
116  }
118 
121 
128  const Matrix32& basis(OptionalJacobian<6, 2> H = {}) const;
129 
131  Matrix3 skew() const;
132 
135 
137  Vector3 unitVector(OptionalJacobian<3, 2> H = {}) const;
138 
140  friend Point3 operator*(double s, const Unit3& d) {
141  return Point3(s * d.p_);
142  }
143 
145  double dot(const Unit3& q, OptionalJacobian<1,2> H1 = {}, //
146  OptionalJacobian<1,2> H2 = {}) const;
147 
150  Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = {}) const;
151 
154  Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = {}, //
155  OptionalJacobian<2, 2> H_q = {}) const;
156 
158  double distance(const Unit3& q, OptionalJacobian<1, 2> H = {}) const;
159 
161  Unit3 cross(const Unit3& q) const {
162  return Unit3(p_.cross(q.p_));
163  }
164 
166  Point3 cross(const Point3& q) const {
167  return point3().cross(q);
168  }
169 
171 
174 
176  inline static size_t Dim() {
177  return 2;
178  }
179 
181  inline size_t dim() const {
182  return 2;
183  }
184 
187  RENORM
188  };
189 
191  Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = {}) const;
192 
194  Vector2 localCoordinates(const Unit3& s) const;
195 
197 
198 private:
199 
202 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
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 #endif
210 
212 
213 public:
215 };
216 
217 // Define GTSAM traits
218 template<> struct traits<Unit3> : public internal::Manifold<Unit3> {
219 };
220 
221 template<> struct traits<const Unit3> : public internal::Manifold<Unit3> {
222 };
223 
224 } // namespace gtsam
225 
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Vector3 p_
The location of the point on the unit sphere.
Definition: Unit3.h:46
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
Unit3(const Unit3 &u)
Copy constructor.
Definition: Unit3.h:79
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
Scalar * y
Eigen::Vector3d Vector3
Definition: Vector.h:43
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
std::string serialize(const T &input)
serializes to a string
std::optional< Matrix32 > B_
Cached basis.
Definition: Unit3.h:47
Vector2 Point2
Definition: Point2.h:32
static std::mt19937 rng
bool equals(const Unit3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition: Unit3.h:114
size_t dim() const
Dimensionality of tangent space = 2 DOF.
Definition: Unit3.h:181
Double_ distance(const OrientedPlane3_ &p)
std::optional< Matrix62 > H_B_
Cached basis derivative.
Definition: Unit3.h:48
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Point3 cross(const Point3 &q) const
Cross-product w Point3.
Definition: Unit3.h:166
Base class and basic functions for Manifold types.
Unit3 & operator=(const Unit3 &u)
Copy assignment.
Definition: Unit3.h:84
Array< int, Dynamic, 1 > v
static size_t Dim()
Dimensionality of tangent space = 2 DOF.
Definition: Unit3.h:176
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
friend Point3 operator*(double s, const Unit3 &d)
Return scaled direction as Point3.
Definition: Unit3.h:140
RealScalar s
EIGEN_DEVICE_FUNC const Scalar & q
Unit3()
Default constructor.
Definition: Unit3.h:64
CoordinatesMode
Definition: Unit3.h:185
Point3_ point3(const Unit3_ &v)
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
Unit3 cross(const Unit3 &q) const
Cross-product between two Unit3s.
Definition: Unit3.h:161
ofstream os("timeSchurFactors.csv")
3D Point
float * p
serialization for Vectors
Use the exponential map to retract.
Definition: Unit3.h:186
static double error
Definition: testRot3.cpp:37
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:284
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
2D Point


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:41