SE.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/core/optional_ref.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/CVectorFixed.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/TPose3D.h>
#include <mrpt/poses/poses_frwds.h>

/** \defgroup mrpt_poses_lie_grp Lie Algebra methods for SO(2),SO(3),SE(2),SE(3)
 * \ingroup poses_grp */

namespace mrpt::poses::Lie
{
/** \addtogroup mrpt_poses_lie_grp
 *  @{ */

/** Traits for SE(n), rigid-body transformations in R^n space.
 * \ingroup mrpt_poses_lie_grp
 */
template <unsigned int n>
struct SE;

/** Traits for SE(3), rigid-body transformations in R^3 space.
 * See indidual members for documentation, or \cite blanco_se3_tutorial for a
 * general overview.
 * \ingroup mrpt_poses_lie_grp
 */
template <>
struct SE<3>
{
    /** Number of actual degrees of freedom for this transformation */
    constexpr static size_t DOFs = 6;

    /** Dimensionality of the matrix manifold (3x4=12 upper part of the 4x4) */
    constexpr static size_t MANIFOLD_DIM = 3 * 4;

    using tangent_vector = mrpt::math::CVectorFixedDouble<DOFs>;
    using manifold_vector = mrpt::math::CVectorFixedDouble<MANIFOLD_DIM>;

    using type = CPose3D;
    using light_type = mrpt::math::TPose3D;

    /** Type for Jacobian: tangent space -> SO(n) matrix */
    using tang2mat_jacob = mrpt::math::CMatrixDouble12_6;

    /** Type for Jacobian: SO(n) matrix -> tangent space */
    using mat2tang_jacob = mrpt::math::CMatrixDouble6_12;

    /** Type for Jacobians between so(n) vectors on the tangent space */
    using matrix_TxT = mrpt::math::CMatrixDouble66;

    /** Type for Jacobians between SO(n) 3x4 (sub)matrices in the manifold */
    using matrix_MxM = mrpt::math::CMatrixFixed<double, 12, 12>;

    /** Retraction to SE(3), a **pseudo-exponential** map \f$ x \rightarrow
     * PseudoExp(x^\wedge) \f$ and its Jacobian.
     * - Input: 6-len vector in Lie algebra se(3) [x,y,z, rx,ry,rz]
     * - Output: translation and rotation in SE(3) as CPose3D
     * Note that this method implements retraction via a **pseudo-exponential**,
     * where only the rotational part undergoes a real matrix exponential,
     * while the translation is left unmodified. This is done for computational
     * efficiency, and does not change the results of optimizations as long
     * as the corresponding local coordinates (pseudo-logarithm) are used as
     * well.
     *
     * See section 9.4.2 in \cite blanco_se3_tutorial
     */
    static type exp(const tangent_vector& x);

    /** SE(3) **pseudo-logarithm** map \f$ \mathbf{R} \rightarrow
     * \log(\mathbf{R}^\vee)\f$
     * - Input: translation and rotation in SE(3) as CPose3D
     * - Output: 6-len vector in Lie algebra se(3) [x,y,z, rx,ry,rz]
     *
     * See exp() for the explanation about the "pseudo" name.
     * For the formulas, see section 9.4.2 in \cite blanco_se3_tutorial
     */
    static tangent_vector log(const type& P);

    /** Returns a vector with all manifold matrix elements in column-major
     * order. For SE(3), it is a 3x4=12 vector. */
    static manifold_vector asManifoldVector(const type& pose);

    /** The inverse operation of asManifoldVector() */
    static type fromManifoldVector(const manifold_vector& v);

    /** Jacobian for the pseudo-exponential exp().
     * See 10.3.1 in \cite blanco_se3_tutorial
     *
     * - Input: 6-len vector in Lie algebra se(3)
     * - Jacobian: Jacobian of the 3x4 matrix (stacked as a column major
     * 12-vector) wrt the vector in the tangent space.
     */
    static tang2mat_jacob jacob_dexpe_de(const tangent_vector& x);

    /** Jacobian for the pseudo-logarithm log()
     * See 10.3.11 in \cite blanco_se3_tutorial
     *
     * - Input: a SE(3) pose as a CPose3D object
     * - Jacobian: Jacobian of the tangent space vector wrt the 3x4 matrix
     * elements (stacked as a column major 12-vector).
     */
    static mat2tang_jacob jacob_dlogv_dv(const type& P);

    /** Jacobian d (e^eps * D) / d eps , with eps=increment in Lie Algebra.
     * \note Section 10.3.3 in \cite blanco_se3_tutorial
     */
    static tang2mat_jacob jacob_dexpeD_de(const CPose3D& D);

    /** Jacobian d (D * e^eps) / d eps , with eps=increment in Lie Algebra.
     * \note Section 10.3.4 in \cite blanco_se3_tutorial
     */
    static tang2mat_jacob jacob_dDexpe_de(const CPose3D& D);

    /** Jacobian d (A * e^eps * D) / d eps , with eps=increment in Lie
     * Algebra.
     * \note Eq. 10.3.7 in \cite blanco_se3_tutorial
     */
    static tang2mat_jacob jacob_dAexpeD_de(const CPose3D& A, const CPose3D& D);

    /** Jacobian of the pose composition A*B for SE(3) 3x4 (sub)matrices,
     * with respect to A.
     * \note See section 7.3.1 of in \cite blanco_se3_tutorial
     */
    static matrix_MxM jacob_dAB_dA(const type& A, const type& B);

    /** Jacobian of the pose composition A*B for SE(3) 3x4 (sub)matrices,
     * with respect to B.
     * \note See section 7.3.1 of in \cite blanco_se3_tutorial
     */
    static matrix_MxM jacob_dAB_dB(const type& A, const type& B);

    /** Return one or both of the following 6x6 Jacobians, useful in
     * graph-slam problems:
     * \f[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \f]
     * \f[  \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1}\f]
     * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ increments in the
     * linearized manifold for P1 and P2.
     * \note Section 10.3.10 in \cite blanco_se3_tutorial
     */
    static void jacob_dDinvP1invP2_de1e2(
        const type& Dinv, const type& P1, const type& P2,
        mrpt::optional_ref<matrix_TxT> df_de1 = std::nullopt,
        mrpt::optional_ref<matrix_TxT> df_de2 = std::nullopt);
};

/** Traits for SE(2), rigid-body transformations in R^2 space.
 * See indidual members for documentation, or \cite blanco_se3_tutorial for a
 * general overview.
 * \ingroup mrpt_poses_lie_grp
 */
template <>
struct SE<2>
{
    /** Number of actual degrees of freedom for this transformation */
    constexpr static size_t DOFs = 3;

    /** Dimensionality of the matrix manifold; this should be 3x3=9 for SE(2),
     * but for efficiency, we define it as simply 3x1=3 and use the (x,y,phi)
     * representation as well as the "manifold". This is done for API
     * consistency with SE(3), where the actual matrix is used instead. */
    constexpr static size_t MANIFOLD_DIM = 3;

    using tangent_vector = mrpt::math::CVectorFixedDouble<DOFs>;
    using manifold_vector = mrpt::math::CVectorFixedDouble<MANIFOLD_DIM>;

    using type = CPose2D;
    using light_type = mrpt::math::TPose2D;

    /** Type for Jacobians between SO(n) transformations */
    using matrix_TxT = mrpt::math::CMatrixDouble33;

    /** In SE(3), this type represents Jacobians between SO(n) (sub)matrices in
     * the manifold, but in SE(2) it simply models Jacobians between (x,y,phi)
     * vectors. */
    using matrix_MxM = mrpt::math::CMatrixDouble33;

    /** Type for Jacobian: tangent space -> SO(n) matrix */
    using tang2mat_jacob = mrpt::math::CMatrixDouble33;
    /** Type for Jacobian: SO(n) matrix -> tangent space */
    using mat2tang_jacob = mrpt::math::CMatrixDouble33;

    /** Exponential map in SE(2), takes [x,y,phi] and returns a CPose2D */
    static type exp(const tangent_vector& x);

    /** Logarithm map in SE(2), takes a CPose2D and returns [X,Y, phi] */
    static tangent_vector log(const type& P);

    /** Returns a vector with all manifold matrix elements in column-major
     * order. For SE(2), though, it directly returns the vector [x,y,phi] for
     * efficiency in comparison to 3x3 homogeneous coordinates. */
    static manifold_vector asManifoldVector(const type& pose);

    /** The inverse operation of asManifoldVector() */
    static type fromManifoldVector(const manifold_vector& v);

    /** Jacobian of the pose composition A*B for SE(2) with respect to A.
     * \note See appendix B of \cite blanco_se3_tutorial
     */
    static matrix_MxM jacob_dAB_dA(const type& A, const type& B);

    /** Jacobian of the pose composition A*B for SE(2) with respect to B.
     * \note See appendix B of \cite blanco_se3_tutorial
     */
    static matrix_MxM jacob_dAB_dB(const type& A, const type& B);

    /** Jacobian d (D * e^eps) / d eps , with eps=increment in Lie Algebra.
     * \note See appendix B in \cite blanco_se3_tutorial
     */
    static tang2mat_jacob jacob_dDexpe_de(const type& D);

    /** Return one or both of the following 3x3 Jacobians, useful in
     * graph-slam problems:
     * \f[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \f]
     * \f[  \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1}\f]
     * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ increments in the
     * linearized manifold for P1 and P2.
     * \note See appendix B in \cite blanco_se3_tutorial
     */
    static void jacob_dDinvP1invP2_de1e2(
        const type& Dinv, const type& P1, const type& P2,
        mrpt::optional_ref<matrix_TxT> df_de1 = std::nullopt,
        mrpt::optional_ref<matrix_TxT> df_de2 = std::nullopt);
};

}  // namespace mrpt::poses::Lie