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
Matrix.h
typedef and functions to augment Eigen's MatrixXd
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Vector.h
typedef and functions to augment Eigen's VectorXd
Manifold.h
Base class and basic functions for Manifold types.
VectorSerialization.h
serialization for Vectors
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:195
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:81
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