namespace mrpt::graphs::detail

Overview

Internal functions for MRPT.

namespace detail {

// structs

template <class GRAPH_T>
struct THypothesis;

struct TNodeAnnotations;
struct TNodeAnnotationsEmpty;

template <>
struct TPosePDFHelper<CPose3D>;

template <>
struct TPosePDFHelper<CPose2D>;

template <class POSE_PDF>
struct TPosePDFHelper;

struct edge_annotations_empty;

template <class graph_t>
struct graph_ops;

// classes

class NotConnectedGraph;

// global functions

template <class MAT>
MAT congruenceTransform(const MAT& J, const MAT& M);

mrpt::math::CMatrixDouble33 jacobPoseParamsWrtLocal(const mrpt::poses::CPose2D& Z);
mrpt::math::CMatrixDouble33 jacobLocalWrtPoseParams(const mrpt::poses::CPose2D& Z);
mrpt::math::CMatrixDouble66 jacobPoseParamsWrtLocal(const mrpt::poses::CPose3D& Z);
mrpt::math::CMatrixDouble66 jacobLocalWrtPoseParams(const mrpt::poses::CPose3D& Z);

template <class POSE, class MAT>
MAT infoParamsToLocal(const POSE& Z, const MAT& infoParams);

template <class POSE, class MAT>
MAT infoLocalToParams(const POSE& Z, const MAT& infoLocal);

} // namespace detail

Global Functions

template <class MAT>
MAT congruenceTransform(const MAT& J, const MAT& M)

Returns J^T * M * J, for square matrices of the same size.

mrpt::math::CMatrixDouble33 jacobPoseParamsWrtLocal(const mrpt::poses::CPose2D& Z)

Jacobian of the (x,y,phi) parameters of Z (+) exp(e) with respect to the local increment e=[x,y,phi], at e=0.

mrpt::math::CMatrixDouble33 jacobLocalWrtPoseParams(const mrpt::poses::CPose2D& Z)

Inverse of jacobPoseParamsWrtLocal()

mrpt::math::CMatrixDouble66 jacobPoseParamsWrtLocal(const mrpt::poses::CPose3D& Z)

Jacobian of the (x,y,z,yaw,pitch,roll) parameters of Z (+) exp(e) with respect to the local increment e=[x,y,z,rx,ry,rz] (translation in the frame of Z, body-frame rotation vector), at e=0.

Singular at pitch=+-90 deg.

mrpt::math::CMatrixDouble66 jacobLocalWrtPoseParams(const mrpt::poses::CPose3D& Z)

Inverse of jacobPoseParamsWrtLocal()

template <class POSE, class MAT>
MAT infoParamsToLocal(
    const POSE& Z,
    const MAT& infoParams
    )

Converts the information matrix of the pose parameters of a relative pose with mean Z (MRPT convention) into that of its local residual.

template <class POSE, class MAT>
MAT infoLocalToParams(
    const POSE& Z,
    const MAT& infoLocal
    )

Inverse of infoParamsToLocal()