Loading...
Searching...
No Matches
SO3.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/geometry/SOn.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/dllexport.h>
#include <cmath>
#include <vector>
namespace gtsam {
using SO3 = SO<3>;
// Below are all declarations of SO<3> specializations.
// They are *defined* in SO3.cpp.
template <>
GTSAM_EXPORT
SO3 SO3::AxisAngle(const Vector3& axis, double theta);
template <>
GTSAM_EXPORT
SO3 SO3::ClosestTo(const Matrix3& M);
template <>
GTSAM_EXPORT
SO3 SO3::ChordalMean(const std::vector<SO3>& rotations);
template <>
GTSAM_EXPORT
Matrix3 SO3::Hat(const Vector3& xi);
template <>
GTSAM_EXPORT
Vector3 SO3::Vee(const Matrix3& X);
template <>
Matrix3 SO3::AdjointMap() const;
template <>
GTSAM_EXPORT
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
template <>
GTSAM_EXPORT
Matrix3 SO3::ExpmapDerivative(const Vector3& omega);
template <>
GTSAM_EXPORT
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H);
template <>
GTSAM_EXPORT
Matrix3 SO3::LogmapDerivative(const Vector3& omega);
// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
template <>
GTSAM_EXPORT
SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H);
template <>
GTSAM_EXPORT
Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H);
template <>
GTSAM_EXPORT
Vector9 SO3::vec(OptionalJacobian<9, 3> H) const;
template <class Archive>
void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) {
Matrix3& M = R.matrix_;
ar& boost::serialization::make_nvp("R11", M(0, 0));
ar& boost::serialization::make_nvp("R12", M(0, 1));
ar& boost::serialization::make_nvp("R13", M(0, 2));
ar& boost::serialization::make_nvp("R21", M(1, 0));
ar& boost::serialization::make_nvp("R22", M(1, 1));
ar& boost::serialization::make_nvp("R23", M(1, 2));
ar& boost::serialization::make_nvp("R31", M(2, 0));
ar& boost::serialization::make_nvp("R32", M(2, 1));
ar& boost::serialization::make_nvp("R33", M(2, 2));
}
namespace so3 {
GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
OptionalJacobian<9, 9> H = boost::none);
GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
// Below are two functors that allow for saving computation when exponential map
// and its derivatives are needed at the same location in so<3>. The second
// functor also implements dedicated methods to apply dexp and/or inv(dexp).
class GTSAM_EXPORT ExpmapFunctor {
protected:
const double theta2;
Matrix3 W, K, KK;
bool nearZero;
double theta, sin_theta, one_minus_cos; // only defined if !nearZero
void init(bool nearZeroApprox = false);
public:
explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
SO3 expmap() const;
};
class DexpFunctor : public ExpmapFunctor {
const Vector3 omega;
double a, b;
Matrix3 dexp_;
public:
GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
// NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
// (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
// Information Theory, and Lie Groups", Volume 2, 2008.
// expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
// This maps a perturbation v in the tangent space to
// a perturbation on the manifold Expmap(dexp * v) */
const Matrix3& dexp() const { return dexp_; }
GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
};
} // namespace so3
/*
* Define the traits. internal::LieGroup provides both Lie group and Testable
*/
template <>
struct traits<SO3> : public internal::LieGroup<SO3> {};
template <>
struct traits<const SO3> : public internal::LieGroup<SO3> {};
} // end namespace gtsam
Matrix.h
typedef and functions to augment Eigen's MatrixXd
Lie.h
Base class and basic functions for Lie types.
SOn.h
N*N matrix representation of SO(N).
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
gtsam::ChartJacobian
gtsam::LieGroup< SO< N >, internal::DimensionSO(N)>::Retract
static SO< N > Retract(const TangentVector &v)
Definition Lie.h:111
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
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::so3::ExpmapFunctor::ExpmapFunctor
ExpmapFunctor(const Vector3 &omega, bool nearZeroApprox=false)
Constructor with element of Lie algebra so(3).
Definition SO3.cpp:60
gtsam::so3::ExpmapFunctor::expmap
SO3 expmap() const
Rodrigues formula.
Definition SO3.cpp:83
gtsam::so3::DexpFunctor::applyDexp
GTSAM_EXPORT Vector3 applyDexp(const Vector3 &v, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Multiplies with dexp(), with optional derivatives.
Definition SO3.cpp:101
gtsam::so3::DexpFunctor::DexpFunctor
GTSAM_EXPORT DexpFunctor(const Vector3 &omega, bool nearZeroApprox=false)
Constructor with element of Lie algebra so(3).
Definition SO3.cpp:90
gtsam::so3::DexpFunctor::applyInvDexp
GTSAM_EXPORT Vector3 applyInvDexp(const Vector3 &v, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Multiplies with dexp().inverse(), with optional derivatives.
Definition SO3.cpp:120
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition SOn.h:52
gtsam::SO< 3 >::Expmap
static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none)
gtsam::SO< 3 >::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
gtsam::SO< 3 >::ChordalMean
static SO ChordalMean(const std::vector< SO > &rotations)
gtsam::SO< 3 >::Vee
static TangentVector Vee(const MatrixNN &X)
gtsam::SO< 3 >::Logmap
static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none)
gtsam::SO< 3 >::AxisAngle
static SO AxisAngle(const Vector3 &axis, double theta)
Definition SO3.cpp:138
gtsam::SO< 3 >::AdjointMap
MatrixDD AdjointMap() const
Definition SO3.cpp:193
gtsam::SO< 3 >::Hat
static MatrixNN Hat(const TangentVector &xi)
gtsam::SO< 3 >::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
gtsam::SO< 3 >::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)
gtsam::SO< 3 >::ClosestTo
static SO ClosestTo(const MatrixNN &M)