template class sophus::RxSo3Base¶
Overview¶
RxSo3 base type - implements RxSo3 class but is storage agnostic. More…
#include <rxso3.h> template <class TDerived> class RxSo3Base { public: // typedefs using Scalar = typename Eigen::internal::traits<TDerived>::Scalar ; using QuaternionType = typename Eigen::internal::traits<TDerived>::QuaternionType ; using QuaternionTemporaryType = Eigen::Quaternion<Scalar, kOptions> ; using Transformation = Eigen::Matrix<Scalar, kMatrixDim, kMatrixDim> ; using Point = Eigen::Vector3<Scalar> ; using HomogeneousPoint = Eigen::Vector4<Scalar> ; using Line = Eigen::ParametrizedLine<Scalar, 3> ; using Hyperplane = Eigen::Hyperplane<Scalar, 3> ; using Tangent = Eigen::Vector<Scalar, kDoF> ; using Adjoint = Eigen::Matrix<Scalar, kDoF, kDoF> ; using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<Scalar, typename TOtherDerived::Scalar>::ReturnType ; using RxSo3Product = RxSo3<ReturnScalar<TOtherDerived>> ; using PointProduct = Eigen::Vector3<ReturnScalar<TPointDerived>> ; using HomogeneousPointProduct = Eigen::Vector4<ReturnScalar<THPointDerived>> ; // structs struct TangentAndTheta; // fields static constexpr int kOptions = Eigen::internal::traits<TDerived>::kOptions; static constexpr int kDoF = 4; static constexpr int kNumParameters = 4; static constexpr int kMatrixDim = 3; static constexpr int kPointDim = 3; // methods SOPHUS_FUNC Adjoint adj() const; template <class TNewScalarType> SOPHUS_FUNC RxSo3<TNewScalarType> cast() const; SOPHUS_FUNC Scalar* data(); SOPHUS_FUNC Scalar const* data() const; SOPHUS_FUNC RxSo3<Scalar> inverse() const; SOPHUS_FUNC Tangent log() const; SOPHUS_FUNC TangentAndTheta logAndTheta() const; SOPHUS_FUNC Transformation matrix() const; template <class TOtherDerived> SOPHUS_FUNC RxSo3Base<TDerived>& operator=(RxSo3Base<TOtherDerived> const& other); template <class TOtherDerived> SOPHUS_FUNC RxSo3Product<TOtherDerived> operator*(RxSo3Base<TOtherDerived> const& other) const; template < typename TPointDerived, typename = typename std::enable_if<IsFixedSizeVector<TPointDerived, 3>::value>::type > SOPHUS_FUNC PointProduct<TPointDerived> operator*(Eigen::MatrixBase<TPointDerived> const& p) const; template < typename THPointDerived, typename = typename std::enable_if<IsFixedSizeVector<THPointDerived, 4>::value>::type > SOPHUS_FUNC HomogeneousPointProduct<THPointDerived> operator*(Eigen::MatrixBase<THPointDerived> const& p) const; SOPHUS_FUNC Line operator*(Line const& l) const; SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const; template < typename TOtherDerived, typename = typename std::enable_if<std::is_same<Scalar, ReturnScalar<TOtherDerived>>::value>::type > SOPHUS_FUNC RxSo3Base<TDerived>& operator*=(RxSo3Base<TOtherDerived> const& other); SOPHUS_FUNC Eigen::Vector<Scalar, kNumParameters> params() const; SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat); SOPHUS_FUNC QuaternionType const& quaternion() const; SOPHUS_FUNC Transformation rotationMatrix() const; SOPHUS_FUNC Scalar scale() const; SOPHUS_FUNC void setRotationMatrix(Transformation const& r); SOPHUS_FUNC void setScale(Scalar const& scale); SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& s_r); SOPHUS_FUNC void setSO3(So3<Scalar> const& so3); SOPHUS_FUNC So3<Scalar> so3() const; SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxThisMulExpXAt0() const; SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kNumParameters> dxLogThisInvTimesXAtThis() const; }; // direct descendants template <class TScalar, int kOptions> class Map<sophus::RxSo3<TScalar> const, kOptions>; template <class TScalar, int kOptions> class Map<sophus::RxSo3<TScalar>, kOptions>;
Detailed Documentation¶
RxSo3 base type - implements RxSo3 class but is storage agnostic.
This class implements the group R+ x SO(3)
, the direct product of the group of positive scalar 3x3 matrices (= isomorph to the positive real numbers) and the three-dimensional special orthogonal group SO(3). Geometrically, it is the group of rotation and scaling in three dimensions. As a matrix groups, RxSo3 consists of matrices of the form s * R
where R
is an orthogonal matrix with det(R)=1
and s > 0
being a positive real number.
Internally, RxSo3 is represented by the group of non-zero quaternions. In particular, the scale equals the squared(!) norm of the quaternion q
, s = |q|^2
. This is a most compact representation since the degrees of freedom (kDoF) of RxSo3 (=4) equals the number of internal parameters (=4).
This class has the explicit class invariant that the scale s
is not too close to either zero or infinity. Strictly speaking, it must hold that:
quaternion().squaredNorm() >= Constants::epsilon()
and 1. / quaternion().squaredNorm() >= Constants::epsilon()
.
In order to obey this condition, group multiplication is implemented with saturation such that a product always has a scale which is equal or greater this threshold.
Typedefs¶
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<Scalar, typename TOtherDerived::Scalar>::ReturnType
For binary operations the return type is determined with the ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map types, as well as other compatible scalar types such as Ceres::Jet and double scalars with RxSo3 operations.
Fields¶
static constexpr int kDoF = 4
Degrees of freedom of manifold, number of dimensions in tangent space (three for rotation and one for scaling).
static constexpr int kNumParameters = 4
Number of internal parameters used (quaternion is a 4-tuple).
static constexpr int kMatrixDim = 3
Group transformations are 3x3 matrices.
static constexpr int kPointDim = 3
Points are 3-dimensional.
Methods¶
SOPHUS_FUNC Adjoint adj() const
Adjoint transformation.
This function return the adjoint transformation Ad
of the group element A
such that for all x
it holds that hat(Ad_A * x) = A * hat(x) A^{-1}
. See hat-operator below.
For RxSO(3), it simply returns the rotation matrix corresponding to A
.
template <class TNewScalarType> SOPHUS_FUNC RxSo3<TNewScalarType> cast() const
Returns copy of instance casted to NewScalarType.
SOPHUS_FUNC Scalar* data()
This provides unsafe read/write access to internal data. RxSO(3) is represented by an Eigen::Quaternion (four parameters). When using direct write access, the user needs to take care of that the quaternion is not set close to zero.
Note: The first three Scalars represent the imaginary parts, while the forth Scalar represent the real part.
SOPHUS_FUNC Scalar const* data() const
Const version of data() above.
SOPHUS_FUNC RxSo3<Scalar> inverse() const
Returns group inverse.
SOPHUS_FUNC Tangent log() const
Logarithmic map.
Computes the logarithm, the inverse of the group exponential which maps element of the group (scaled rotation matrices) to elements of the tangent space (rotation-vector plus logarithm of scale factor).
To be specific, this function computes vee(logmat(.))
with logmat(.)
being the matrix logarithm and vee(.)
the vee-operator of RxSo3.
SOPHUS_FUNC TangentAndTheta logAndTheta() const
As above, but also returns theta = |omega|
.
SOPHUS_FUNC Transformation matrix() const
Returns 3x3 matrix representation of the instance.
For RxSo3, the matrix representation is an scaled orthogonal matrix sR
with det(R)=s^3
, thus a scaled rotation matrix R
with scale s
.
template <class TOtherDerived> SOPHUS_FUNC RxSo3Base<TDerived>& operator=(RxSo3Base<TOtherDerived> const& other)
Assignment-like operator from OtherDerived.
template <class TOtherDerived> SOPHUS_FUNC RxSo3Product<TOtherDerived> operator*(RxSo3Base<TOtherDerived> const& other) const
Group multiplication, which is rotation concatenation and scale multiplication.
Note: This function performs saturation for products close to zero in order to ensure the class invariant.
template < typename TPointDerived, typename = typename std::enable_if<IsFixedSizeVector<TPointDerived, 3>::value>::type > SOPHUS_FUNC PointProduct<TPointDerived> operator*(Eigen::MatrixBase<TPointDerived> const& p) const
Group action on 3-points.
This function rotates a 3 dimensional point p
by the So3 element bar_R_foo
(= rotation matrix) and scales it by the scale factor s
:
p_bar = s * (bar_R_foo * p_foo)
.
template < typename THPointDerived, typename = typename std::enable_if<IsFixedSizeVector<THPointDerived, 4>::value>::type > SOPHUS_FUNC HomogeneousPointProduct<THPointDerived> operator*(Eigen::MatrixBase<THPointDerived> const& p) const
Group action on homogeneous 3-points. See above for more details.
SOPHUS_FUNC Line operator*(Line const& l) const
Group action on lines.
This function rotates a parametrized line l(t) = o + t * d
by the So3 element ans scales it by the scale factor:
Origin o
is rotated and scaled Direction d
is rotated (preserving it’s norm)
SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const
Group action on planes.
This function rotates parametrized plane n.x + d = 0
by the So3 element and scales it by the scale factor:
Normal vector n
is rotated Offset d
is scaled
template < typename TOtherDerived, typename = typename std::enable_if<std::is_same<Scalar, ReturnScalar<TOtherDerived>>::value>::type > SOPHUS_FUNC RxSo3Base<TDerived>& operator*=(RxSo3Base<TOtherDerived> const& other)
In-place group multiplication. This method is only valid if the return type of the multiplication is compatible with this So3 ‘s Scalar type.
Note: This function performs saturation for products close to zero in order to ensure the class invariant.
SOPHUS_FUNC Eigen::Vector<Scalar, kNumParameters> params() const
Returns internal parameters of RxSO(3).
It returns (q.imag[0], q.imag[1], q.imag[2], q.real), with q being the quaternion.
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat)
Sets non-zero quaternion.
Precondition: quat
must not be close to either zero or infinity
SOPHUS_FUNC QuaternionType const& quaternion() const
Accessor of quaternion.
SOPHUS_FUNC Transformation rotationMatrix() const
Returns rotation matrix.
SOPHUS_FUNC Scalar scale() const
Returns scale.
SOPHUS_FUNC void setRotationMatrix(Transformation const& r)
Setter of quaternion using rotation matrix R
, leaves scale as is.
SOPHUS_FUNC void setScale(Scalar const& scale)
Sets scale and leaves rotation as is.
Note: This function as a significant computational cost, since it has to call the square root twice.
SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& s_r)
Setter of quaternion using scaled rotation matrix sR
.
Precondition: The 3x3 matrix must be “scaled orthogonal” and have a positive determinant.
SOPHUS_FUNC void setSO3(So3<Scalar> const& so3)
Setter of SO(3) rotations, leaves scale as is.
SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxThisMulExpXAt0() const
Returns derivative of this * RxSo3::exp(x) wrt. x at x=0.
SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kNumParameters> dxLogThisInvTimesXAtThis() const
Returns derivative of log(this^{-1} * x) by x at x=this.