gtsam  4.1.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 
23 #include <gtsam/geometry/Point2.h>
24 #include <gtsam/geometry/Point3.h>
25 #include <gtsam/base/Manifold.h>
26 #include <gtsam/base/Matrix.h>
27 #include <gtsam/dllexport.h>
28 
29 #include <boost/optional.hpp>
30 #include <boost/serialization/nvp.hpp>
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 Unit3 {
43 
44 private:
45 
46  Vector3 p_;
47  mutable boost::optional<Matrix32> B_;
48  mutable boost::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  p_(p.normalized()) {
71  }
72 
74  Unit3(double x, double y, double z) :
75  p_(x, y, z) {
76  p_.normalize();
77  }
78 
81  explicit Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) {
82  p_.normalize();
83  }
84 
86  Unit3(const Unit3& u) {
87  p_ = u.p_;
88  }
89 
91  Unit3& operator=(const Unit3 & u) {
92  p_ = u.p_;
93  B_ = u.B_;
94  H_B_ = u.H_B_;
95  return *this;
96  }
97 
99  GTSAM_EXPORT static Unit3 FromPoint3(const Point3& point, //
100  OptionalJacobian<2, 3> H = boost::none);
101 
108  GTSAM_EXPORT static Unit3 Random(std::mt19937 & rng);
109 
111 
114 
115  friend std::ostream& operator<<(std::ostream& os, const Unit3& pair);
116 
118  GTSAM_EXPORT void print(const std::string& s = std::string()) const;
119 
121  bool equals(const Unit3& s, double tol = 1e-9) const {
122  return equal_with_abs_tol(p_, s.p_, tol);
123  }
125 
128 
135  GTSAM_EXPORT const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
136 
138  GTSAM_EXPORT Matrix3 skew() const;
139 
141  GTSAM_EXPORT Point3 point3(OptionalJacobian<3, 2> H = boost::none) const;
142 
144  GTSAM_EXPORT Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const;
145 
147  friend Point3 operator*(double s, const Unit3& d) {
148  return Point3(s * d.p_);
149  }
150 
152  GTSAM_EXPORT double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
153  OptionalJacobian<1,2> H2 = boost::none) const;
154 
157  GTSAM_EXPORT Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
158 
161  GTSAM_EXPORT Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
162  OptionalJacobian<2, 2> H_q = boost::none) const;
163 
165  GTSAM_EXPORT double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
166 
168  Unit3 cross(const Unit3& q) const {
169  return Unit3(p_.cross(q.p_));
170  }
171 
173  Point3 cross(const Point3& q) const {
174  return point3().cross(q);
175  }
176 
178 
181 
183  inline static size_t Dim() {
184  return 2;
185  }
186 
188  inline size_t dim() const {
189  return 2;
190  }
191 
194  RENORM
195  };
196 
198  GTSAM_EXPORT Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const;
199 
201  GTSAM_EXPORT Vector2 localCoordinates(const Unit3& s) const;
202 
204 
205 private:
206 
209 
211  template<class ARCHIVE>
212  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
213  ar & BOOST_SERIALIZATION_NVP(p_);
214  }
215 
217 
218 public:
220 };
221 
222 // Define GTSAM traits
223 template<> struct traits<Unit3> : public internal::Manifold<Unit3> {
224 };
225 
226 template<> struct traits<const Unit3> : public internal::Manifold<Unit3> {
227 };
228 
229 } // namespace gtsam
230 
gtsam::Unit3::Unit3
Unit3(const Point2 &p, double f)
Construct from 2D point in plane at focal length f Unit3(p,1) can be viewed as normalized homogeneous...
Definition: Unit3.h:81
gtsam::Unit3::Unit3
Unit3(double x, double y, double z)
Construct from x,y,z.
Definition: Unit3.h:74
gtsam::Unit3::retract
GTSAM_EXPORT Unit3 retract(const Vector2 &v, OptionalJacobian< 2, 2 > H=boost::none) const
The retract function.
Definition: Unit3.cpp:247
gtsam::Unit3::dot
GTSAM_EXPORT double dot(const Unit3 &q, OptionalJacobian< 1, 2 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none) const
Return dot product with q.
Definition: Unit3.cpp:166
gtsam::equal_with_abs_tol
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:84
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Unit3::cross
Point3 cross(const Point3 &q) const
Cross-product w Point3.
Definition: Unit3.h:173
gtsam::Unit3::Dim
static size_t Dim()
Dimensionality of tangent space = 2 DOF.
Definition: Unit3.h:183
gtsam::Unit3::cross
Unit3 cross(const Unit3 &q) const
Cross-product between two Unit3s.
Definition: Unit3.h:168
gtsam::Unit3::EXPMAP
@ EXPMAP
Use the exponential map to retract.
Definition: Unit3.h:193
gtsam::Unit3::error
GTSAM_EXPORT Vector2 error(const Unit3 &q, OptionalJacobian< 2, 2 > H_q=boost::none) const
Signed, vector-valued error between two directions.
Definition: Unit3.cpp:191
gtsam::Unit3::access
friend class boost::serialization::access
Serialization function.
Definition: Unit3.h:210
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Unit3::Random
static GTSAM_EXPORT Unit3 Random(std::mt19937 &rng)
Random direction, using boost::uniform_on_sphere Example: std::mt19937 engine(42); Unit3 unit = Unit3...
Definition: Unit3.cpp:47
gtsam::Unit3::point3
GTSAM_EXPORT Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Point3.
Definition: Unit3.cpp:136
gtsam::Unit3::Unit3
Unit3()
Default constructor.
Definition: Unit3.h:64
gtsam::Unit3::localCoordinates
GTSAM_EXPORT Vector2 localCoordinates(const Unit3 &s) const
The local coordinates function.
Definition: Unit3.cpp:277
Point3.h
3D Point
gtsam::Point2
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
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
gtsam::Unit3::print
GTSAM_EXPORT void print(const std::string &s=std::string()) const
The print fuction.
Definition: Unit3.cpp:156
gtsam::Unit3::operator*
friend Point3 operator*(double s, const Unit3 &d)
Return scaled direction as Point3.
Definition: Unit3.h:147
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Matrix.h
typedef and functions to augment Eigen's MatrixXd
Point2.h
2D Point
gtsam::Unit3::CoordinatesMode
CoordinatesMode
Definition: Unit3.h:192
gtsam::Unit3::equals
bool equals(const Unit3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition: Unit3.h:121
gtsam::Point3
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:35
gtsam::Unit3::unitVector
GTSAM_EXPORT Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition: Unit3.cpp:143
gtsam::Unit3::Unit3
Unit3(const Vector3 &p)
Construct from point.
Definition: Unit3.h:69
gtsam::Unit3::distance
GTSAM_EXPORT double distance(const Unit3 &q, OptionalJacobian< 1, 2 > H=boost::none) const
Distance between two directions.
Definition: Unit3.cpp:237
gtsam::Unit3::RENORM
@ RENORM
Retract with vector addition and renormalize.
Definition: Unit3.h:194
gtsam::Unit3::skew
GTSAM_EXPORT Matrix3 skew() const
Return skew-symmetric associated with 3D point on unit sphere.
Definition: Unit3.cpp:161
Manifold.h
Base class and basic functions for Manifold types.
gtsam::Unit3::Unit3
Unit3(const Unit3 &u)
Copy constructor.
Definition: Unit3.h:86
gtsam::Unit3::FromPoint3
static GTSAM_EXPORT Unit3 FromPoint3(const Point3 &point, OptionalJacobian< 2, 3 > H=boost::none)
Named constructor from Point3 with optional Jacobian.
Definition: Unit3.cpp:36
gtsam::Unit3::dim
size_t dim() const
Dimensionality of tangent space = 2 DOF.
Definition: Unit3.h:188
gtsam::Unit3::operator=
Unit3 & operator=(const Unit3 &u)
Copy assignment.
Definition: Unit3.h:91
gtsam::Unit3::basis
GTSAM_EXPORT const Matrix32 & basis(OptionalJacobian< 6, 2 > H=boost::none) const
Returns the local coordinate frame to tangent plane It is a 3*2 matrix [b1 b2] composed of two orthog...
Definition: Unit3.cpp:76
gtsam::Unit3::errorVector
GTSAM_EXPORT Vector2 errorVector(const Unit3 &q, OptionalJacobian< 2, 2 > H_p=boost::none, OptionalJacobian< 2, 2 > H_q=boost::none) const
Signed, vector-valued error between two directions NOTE(hayk): This method has zero derivatives if th...
Definition: Unit3.cpp:201