161 lines
3.6 KiB
C++
161 lines
3.6 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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/Point3.h>
|
|
#include <gtsam/base/DerivedValue.h>
|
|
#include <boost/random/mersenne_twister.hpp>
|
|
|
|
namespace gtsam {
|
|
|
|
/// Represents a 3D point on a unit sphere.
|
|
class Unit3: public DerivedValue<Unit3> {
|
|
|
|
private:
|
|
|
|
Point3 p_; ///< The location of the point on the unit sphere
|
|
mutable Matrix B_; ///< Cached basis
|
|
|
|
public:
|
|
|
|
/// @name Constructors
|
|
/// @{
|
|
|
|
/// Default constructor
|
|
Unit3() :
|
|
p_(1.0, 0.0, 0.0) {
|
|
}
|
|
|
|
/// Construct from point
|
|
explicit Unit3(const Point3& p) :
|
|
p_(p / p.norm()) {
|
|
}
|
|
|
|
/// Construct from x,y,z
|
|
Unit3(double x, double y, double z) :
|
|
p_(x, y, z) {
|
|
p_ = p_ / p_.norm();
|
|
}
|
|
|
|
/// Named constructor from Point3 with optional Jacobian
|
|
static Unit3 FromPoint3(const Point3& point, boost::optional<Matrix&> H =
|
|
boost::none);
|
|
|
|
/// Random direction, using boost::uniform_on_sphere
|
|
static Unit3 Random(boost::mt19937 & rng);
|
|
|
|
/// @}
|
|
|
|
/// @name Testable
|
|
/// @{
|
|
|
|
/// The print fuction
|
|
void print(const std::string& s = std::string()) const;
|
|
|
|
/// The equals function with tolerance
|
|
bool equals(const Unit3& s, double tol = 1e-9) const {
|
|
return p_.equals(s.p_, tol);
|
|
}
|
|
/// @}
|
|
|
|
/// @name Other functionality
|
|
/// @{
|
|
|
|
/**
|
|
* Returns the local coordinate frame to tangent plane
|
|
* It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
|
|
* tangent to the sphere at the current direction.
|
|
*/
|
|
Matrix basis() const;
|
|
|
|
/// Return skew-symmetric associated with 3D point on unit sphere
|
|
Matrix skew() const;
|
|
|
|
/// Return unit-norm Point3
|
|
const Point3& point3(boost::optional<Matrix&> H = boost::none) const {
|
|
if (H)
|
|
*H = basis();
|
|
return p_;
|
|
}
|
|
|
|
/// Return scaled direction as Point3
|
|
friend Point3 operator*(double s, const Unit3& d) {
|
|
return s * d.p_;
|
|
}
|
|
|
|
/// Signed, vector-valued error between two directions
|
|
Vector error(const Unit3& q,
|
|
boost::optional<Matrix&> H = boost::none) const;
|
|
|
|
/// Distance between two directions
|
|
double distance(const Unit3& q,
|
|
boost::optional<Matrix&> H = boost::none) const;
|
|
|
|
/// @}
|
|
|
|
/// @name Manifold
|
|
/// @{
|
|
|
|
/// Dimensionality of tangent space = 2 DOF
|
|
inline static size_t Dim() {
|
|
return 2;
|
|
}
|
|
|
|
/// Dimensionality of tangent space = 2 DOF
|
|
inline size_t dim() const {
|
|
return 2;
|
|
}
|
|
|
|
enum CoordinatesMode {
|
|
EXPMAP, ///< Use the exponential map to retract
|
|
RENORM ///< Retract with vector addtion and renormalize.
|
|
};
|
|
|
|
/// The retract function
|
|
Unit3 retract(const Vector& v) const;
|
|
|
|
/// The local coordinates function
|
|
Vector localCoordinates(const Unit3& s) const;
|
|
|
|
/// @}
|
|
|
|
private:
|
|
|
|
/// @name Advanced Interface
|
|
/// @{
|
|
|
|
/** Serialization function */
|
|
friend class boost::serialization::access;
|
|
template<class ARCHIVE>
|
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
ar & boost::serialization::make_nvp("Unit3",
|
|
boost::serialization::base_object<Value>(*this));
|
|
ar & BOOST_SERIALIZATION_NVP(p_);
|
|
ar & BOOST_SERIALIZATION_NVP(B_);
|
|
}
|
|
|
|
/// @}
|
|
|
|
};
|
|
|
|
} // namespace gtsam
|
|
|