Loading...
Searching...
No Matches
Unit3.h
/* ----------------------------------------------------------------------------
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* @file Unit3.h
* @date Feb 02, 2011
* @author Can Erdogan
* @author Frank Dellaert
* @author Alex Trevor
* @brief Develop a Unit3 class - basically a point on a unit sphere
*/
#pragma once
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/VectorSerialization.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/dllexport.h>
#include <boost/optional.hpp>
#include <random>
#include <string>
#ifdef GTSAM_USE_TBB
#include <mutex> // std::mutex
#endif
namespace gtsam {
class GTSAM_EXPORT Unit3 {
private:
Vector3 p_;
mutable boost::optional<Matrix32> B_;
mutable boost::optional<Matrix62> H_B_;
#ifdef GTSAM_USE_TBB
mutable std::mutex B_mutex_;
#endif
public:
enum {
dimension = 2
};
Unit3() :
p_(1.0, 0.0, 0.0) {
}
explicit Unit3(const Vector3& p);
Unit3(double x, double y, double z);
explicit Unit3(const Point2& p, double f);
Unit3(const Unit3& u) {
p_ = u.p_;
}
Unit3& operator=(const Unit3 & u) {
p_ = u.p_;
B_ = u.B_;
H_B_ = u.H_B_;
return *this;
}
static Unit3 FromPoint3(const Point3& point, //
OptionalJacobian<2, 3> H = boost::none);
static Unit3 Random(std::mt19937 & rng);
friend std::ostream& operator<<(std::ostream& os, const Unit3& pair);
void print(const std::string& s = std::string()) const;
bool equals(const Unit3& s, double tol = 1e-9) const {
return equal_with_abs_tol(p_, s.p_, tol);
}
const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
Matrix3 skew() const;
Point3 point3(OptionalJacobian<3, 2> H = boost::none) const;
Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const;
friend Point3 operator*(double s, const Unit3& d) {
return Point3(s * d.p_);
}
double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
OptionalJacobian<1,2> H2 = boost::none) const;
Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
OptionalJacobian<2, 2> H_q = boost::none) const;
double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
Unit3 cross(const Unit3& q) const {
return Unit3(p_.cross(q.p_));
}
Point3 cross(const Point3& q) const {
return point3().cross(q);
}
inline static size_t Dim() {
return 2;
}
inline size_t dim() const {
return 2;
}
enum CoordinatesMode {
EXPMAP,
RENORM
};
Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const;
Vector2 localCoordinates(const Unit3& s) const;
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(p_);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// Define GTSAM traits
template<> struct traits<Unit3> : public internal::Manifold<Unit3> {
};
template<> struct traits<const Unit3> : public internal::Manifold<Unit3> {
};
} // namespace gtsam
Manifold.h
Base class and basic functions for Manifold types.
VectorSerialization.h
serialization for Vectors
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Matrix.h
typedef and functions to augment Eigen's MatrixXd
Vector.h
typedef and functions to augment Eigen's VectorXd
Point2.h
2D Point
Point3.h
3D Point
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
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::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:36
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:205
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:108
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::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
gtsam::Unit3::Unit3
Unit3(const Unit3 &u)
Copy constructor.
Definition Unit3.h:80
gtsam::Unit3::Unit3
Unit3()
Default constructor.
Definition Unit3.h:65
gtsam::Unit3::operator*
friend Point3 operator*(double s, const Unit3 &d)
Return scaled direction as Point3.
Definition Unit3.h:141
gtsam::Unit3::Dim
static size_t Dim()
Dimensionality of tangent space = 2 DOF.
Definition Unit3.h:177
gtsam::Unit3::point3
Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Point3.
Definition Unit3.cpp:144
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::Unit3::CoordinatesMode
CoordinatesMode
Definition Unit3.h:186
gtsam::Unit3::EXPMAP
@ EXPMAP
Use the exponential map to retract.
Definition Unit3.h:187
gtsam::Unit3::RENORM
@ RENORM
Retract with vector addition and renormalize.
Definition Unit3.h:188
gtsam::Unit3::cross
Unit3 cross(const Unit3 &q) const
Cross-product between two Unit3s.
Definition Unit3.h:162
gtsam::Unit3::cross
Point3 cross(const Point3 &q) const
Cross-product w Point3.
Definition Unit3.h:167
gtsam::Unit3::operator=
Unit3 & operator=(const Unit3 &u)
Copy assignment.
Definition Unit3.h:85