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  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
109  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 = {}) const;
130 
132  Matrix3 skew() const;
133 
135  Point3 point3(OptionalJacobian<3, 2> H = {}) const;
136 
138  Vector3 unitVector(OptionalJacobian<3, 2> H = {}) 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 = {}, //
147  OptionalJacobian<1,2> H2 = {}) const;
148 
151  Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = {}) const;
152 
155  Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = {}, //
156  OptionalJacobian<2, 2> H_q = {}) const;
157 
159  double distance(const Unit3& q, OptionalJacobian<1, 2> H = {}) 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 = {}) const;
193 
195  Vector2 localCoordinates(const Unit3& s) const;
196 
198 
199 private:
200 
203 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
204 
205  friend class boost::serialization::access;
206  template<class ARCHIVE>
207  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
208  ar & BOOST_SERIALIZATION_NVP(p_);
209  }
210 #endif
211 
213 
214 public:
216 };
217 
218 // Define GTSAM traits
219 template<> struct traits<Unit3> : public internal::Manifold<Unit3> {
220 };
221 
222 template<> struct traits<const Unit3> : public internal::Manifold<Unit3> {
223 };
224 
225 } // namespace gtsam
226 
H
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
Definition: gnuplot_common_settings.hh:74
gtsam::Unit3::operator*
friend Point3 operator*(double s, const Unit3 &d)
Return scaled direction as Point3.
Definition: Unit3.h:141
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
Vector.h
typedef and functions to augment Eigen's VectorXd
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
VectorSerialization.h
serialization for Vectors
Matrix.h
typedef and functions to augment Eigen's MatrixXd
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::point3
Point3_ point3(const Unit3_ &v)
Definition: slam/expressions.h:101
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
gtsam::Unit3::H_B_
std::optional< Matrix62 > H_B_
Cached basis derivative.
Definition: Unit3.h:48
gtsam::Unit3::B_
std::optional< Matrix32 > B_
Cached basis.
Definition: Unit3.h:47
Point3.h
3D Point
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
os
ofstream os("timeSchurFactors.csv")
Point2.h
2D Point
gtsam::Unit3::Unit3
Unit3()
Default constructor.
Definition: Unit3.h:64
gtsam::Unit3::operator=
Unit3 & operator=(const Unit3 &u)
Copy assignment.
Definition: Unit3.h:84
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::Unit3::Dim
static size_t Dim()
Dimensionality of tangent space = 2 DOF.
Definition: Unit3.h:177
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam::Unit3::cross
Point3 cross(const Point3 &q) const
Cross-product w Point3.
Definition: Unit3.h:167
gtsam::dot
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
Manifold.h
Base class and basic functions for Manifold types.
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
Vector2
Definition: test_operator_overloading.cpp:18
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
y
Scalar * y
Definition: level1_cplx_impl.h:124
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam
traits
Definition: chartTesting.h:28
error
static double error
Definition: testRot3.cpp:37
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Unit3::dim
size_t dim() const
Dimensionality of tangent space = 2 DOF.
Definition: Unit3.h:182
gtsam::Unit3::equals
bool equals(const Unit3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition: Unit3.h:115
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::distance
Double_ distance(const OrientedPlane3_ &p)
Definition: slam/expressions.h:117
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::Unit3::cross
Unit3 cross(const Unit3 &q) const
Cross-product between two Unit3s.
Definition: Unit3.h:162
gtsam::Unit3::CoordinatesMode
CoordinatesMode
Definition: Unit3.h:186
gtsam::Unit3::Unit3
Unit3(const Unit3 &u)
Copy constructor.
Definition: Unit3.h:79
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279
gtsam::Unit3::p_
Vector3 p_
The location of the point on the unit sphere.
Definition: Unit3.h:46
gtsam::Unit3::EXPMAP
@ EXPMAP
Use the exponential map to retract.
Definition: Unit3.h:187


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:11:29