template class sophus::So3¶
Overview¶
So3 using default storage; derived from So3Base. More…
#include <so3.h> template <class TScalar, int kOptions> class So3 { public: // typedefs using Base = So3Base<So3<TScalar, kOptions>> ; using Scalar = TScalar ; using Transformation = typename Base::Transformation ; using Point = typename Base::Point ; using HomogeneousPoint = typename Base::HomogeneousPoint ; using Tangent = typename Base::Tangent ; using Adjoint = typename Base::Adjoint ; using QuaternionMember = Eigen::Quaternion<Scalar, kOptions> ; // structs struct So3AndTheta; // fields static constexpr int kDoF = Base::kDoF; static constexpr int kNumParameters = Base::kNumParameters; // construction So3(); So3(So3 const& other); template <class TOtherDerived> So3(So3Base<TOtherDerived> const& other); So3(Transformation const& r); template <class TD> So3(Eigen::QuaternionBase<TD> const& quat); // methods SOPHUS_FUNC So3& operator=(So3 const& other); SOPHUS_FUNC QuaternionMember const& unitQuaternion() const; SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kDoF> dxThisMulX() const; template <class TOtherDerived> SOPHUS_FUNC So3Base<TDerived>& operator=(So3Base<TOtherDerived> const& other); static SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kDoF> leftJacobian( Tangent const& omega, std::optional<Scalar> const& maybe_theta = std::nullopt ); static SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kDoF> leftJacobianInverse( Tangent const& omega, std::optional<Scalar> const& maybe_theta = std::nullopt ); static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpX(Tangent const& omega); static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpXAt0(); static SOPHUS_FUNC Eigen::Matrix<Scalar, 3, kDoF> dxExpXTimesPointAt0(Point const& point); static SOPHUS_FUNC Transformation dxiExpmatXAt0(int i); static SOPHUS_FUNC So3<Scalar> exp(Tangent const& omega); static SOPHUS_FUNC So3AndTheta expAndTheta(Tangent const& omega); template <class TS = Scalar> static SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, So3> fitToSo3(Transformation const& r); static SOPHUS_FUNC Transformation generator(int i); static SOPHUS_FUNC Transformation hat(Tangent const& omega); static SOPHUS_FUNC Tangent lieBracket(Tangent const& omega1, Tangent const& omega2); static SOPHUS_FUNC So3 rotX(Scalar const& x); static SOPHUS_FUNC So3 rotY(Scalar const& y); static SOPHUS_FUNC So3 rotZ(Scalar const& z); template <class TUniformRandomBitGenerator> static So3 sampleUniform(TUniformRandomBitGenerator& generator); static SOPHUS_FUNC Tangent vee(Transformation const& omega); protected: // fields QuaternionMember unit_quaternion_; // methods SOPHUS_FUNC QuaternionMember& mutUnitQuaternion(); };
Detailed Documentation¶
So3 using default storage; derived from So3Base.
Construction¶
So3()
Default constructor initializes unit quaternion to identity rotation.
So3(So3 const& other)
Copy constructor.
template <class TOtherDerived> So3(So3Base<TOtherDerived> const& other)
Copy-like constructor from OtherDerived.
So3(Transformation const& r)
Constructor from rotation matrix.
Precondition: rotation matrix needs to be orthogonal with determinant of 1.
template <class TD> So3(Eigen::QuaternionBase<TD> const& quat)
Constructor from quaternion.
Precondition: quaternion must not be close to zero.
Methods¶
SOPHUS_FUNC So3& operator=(So3 const& other)
Define copy-assignment operator explicitly. The definition of implicit copy assignment operator is deprecated in presence of a user-declared copy constructor (-Wdeprecated-copy in clang >= 13).
SOPHUS_FUNC QuaternionMember const& unitQuaternion() const
Accessor of unit quaternion.
SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kDoF> dxThisMulX() const
Returns derivative of this * x wrt x at x=0.
template <class TOtherDerived> SOPHUS_FUNC So3Base<TDerived>& operator=(So3Base<TOtherDerived> const& other)
Assignment-like operator from OtherDerived.
static SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kDoF> leftJacobian( Tangent const& omega, std::optional<Scalar> const& maybe_theta = std::nullopt )
Returns the left Jacobian on lie group. See 1st entry in rightmost column in: http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf.
A precomputed theta
can be optionally passed in
Warning: Not to be confused with dxExpX(), which is derivative of the internal quaternion representation of So3 wrt the tangent vector
static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpX(Tangent const& omega)
Returns derivative of exp(x) wrt. x.
static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpXAt0()
Returns derivative of exp(x) wrt. x_i at x=0.
static SOPHUS_FUNC Eigen::Matrix<Scalar, 3, kDoF> dxExpXTimesPointAt0(Point const& point)
Returns derivative of exp(x) * p wrt. x at x=0.
static SOPHUS_FUNC Transformation dxiExpmatXAt0(int i)
Returns derivative of expmat(x)
wrt. x_i at x=0
, with expmat(.)
being the matrix exponential.
static SOPHUS_FUNC So3<Scalar> exp(Tangent const& omega)
Group exponential.
This functions takes in an element of tangent space (= rotation vector omega
) and returns the corresponding element of the group SO(3).
To be more specific, this function computes expmat(hat(omega))
with expmat(.)
being the matrix exponential and hat(.)
being the hat() -operator of SO(3).
static SOPHUS_FUNC So3AndTheta expAndTheta(Tangent const& omega)
As above, but also returns theta = |omega|
as out-parameter.
Precondition: theta
must not be nullptr
.
template <class TS = Scalar> static SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, So3> fitToSo3(Transformation const& r)
Returns closest So3 given arbitrary 3x3 matrix.
static SOPHUS_FUNC Transformation generator(int i)
Returns the ith infinitesimal generators of SO(3).
The infinitesimal generators of SO(3) are:
| 0 0 0 | G_0 = | 0 0 -1 | | 0 1 0 | | 0 0 1 | G_1 = | 0 0 0 | | -1 0 0 | | 0 -1 0 | G_2 = | 1 0 0 | | 0 0 0 |
Precondition: i
must be 0, 1 or 2.
static SOPHUS_FUNC Transformation hat(Tangent const& omega)
hat-operator
It takes in the 3-vector representation omega
(= rotation vector) and returns the corresponding matrix representation of Lie algebra element.
Formally, the hat() -operator of SO(3) is defined as
hat(.): R^3 -> R^{3x3}, hat(omega) = sum_i omega_i * G_i
(for i=0,1,2)
with G_i
being the ith infinitesimal generator of SO(3).
The corresponding inverse is the vee() -operator, see below.
static SOPHUS_FUNC Tangent lieBracket(Tangent const& omega1, Tangent const& omega2)
Lie bracket.
It computes the Lie bracket of SO(3). To be more specific, it computes
[omega_1, omega_2]_so3 := vee([hat(omega_1), hat(omega_2)])
with [A,B] := AB-BA
being the matrix commutator, hat(.)
the hat() -operator and vee(.)
the vee() -operator of So3.
For the Lie algebra so3, the Lie bracket is simply the cross product:
[omega_1, omega_2]_so3 = omega_1 x omega_2.
static SOPHUS_FUNC So3 rotX(Scalar const& x)
Construct x-axis rotation.
static SOPHUS_FUNC So3 rotY(Scalar const& y)
Construct y-axis rotation.
static SOPHUS_FUNC So3 rotZ(Scalar const& z)
Construct z-axis rotation.
template <class TUniformRandomBitGenerator> static So3 sampleUniform(TUniformRandomBitGenerator& generator)
Draw uniform sample from SO(3) manifold. Based on: http://planning.cs.uiuc.edu/node198.html.
static SOPHUS_FUNC Tangent vee(Transformation const& omega)
vee-operator
It takes the 3x3-matrix representation Omega
and maps it to the corresponding vector representation of Lie algebra.
This is the inverse of the hat() -operator, see above.
Precondition: Omega
must have the following structure:
| 0 -c b |
| c 0 -a |
| -b a 0 |
SOPHUS_FUNC QuaternionMember& mutUnitQuaternion()
Mutator of unit_quaternion is protected to ensure class invariant.