29#include <gtsam/dllexport.h>
31#include <boost/optional.hpp>
48 mutable boost::optional<Matrix32> B_;
49 mutable boost::optional<Matrix62> H_B_;
52 mutable std::mutex B_mutex_;
70 explicit Unit3(
const Vector3& p);
73 Unit3(
double x,
double y,
double z);
102 static Unit3 Random(std::mt19937 & rng);
109 friend std::ostream& operator<<(std::ostream& os,
const Unit3& pair);
112 void print(
const std::string& s = std::string())
const;
132 Matrix3 skew()
const;
163 return Unit3(p_.cross(q.p_));
168 return point3().cross(q);
177 inline static size_t Dim() {
182 inline size_t dim()
const {
195 Vector2 localCoordinates(
const Unit3& s)
const;
204 friend class boost::serialization::access;
205 template<
class ARCHIVE>
206 void serialize(ARCHIVE & ar,
const unsigned int ) {
207 ar & BOOST_SERIALIZATION_NVP(p_);
Base class and basic functions for Manifold types.
typedef and functions to augment Eigen's MatrixXd
typedef and functions to augment Eigen's VectorXd
serialization for Vectors
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
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