Basalt Headers
Sophus Namespace Reference

Functions

template<typename Scalar >
SE3< Scalar >::Tangent se3_logd (const SE3< Scalar > &se3)
 Decoupled version of logmap for SE(3) More...
 
template<typename Derived >
SE3< typename Derived::Scalar > se3_expd (const Eigen::MatrixBase< Derived > &upsilon_omega)
 Decoupled version of expmap for SE(3) More...
 
template<typename Scalar >
Sim3< Scalar >::Tangent sim3_logd (const Sim3< Scalar > &sim3)
 Decoupled version of logmap for Sim(3) More...
 
template<typename Derived >
Sim3< typename Derived::Scalar > sim3_expd (const Eigen::MatrixBase< Derived > &upsilon_omega_sigma)
 Decoupled version of expmap for Sim(3) More...
 
template<typename Derived1 , typename Derived2 >
void rightJacobianSO3 (const Eigen::MatrixBase< Derived1 > &phi, const Eigen::MatrixBase< Derived2 > &J_phi)
 Right Jacobian for SO(3) More...
 
template<typename Derived1 , typename Derived2 >
void rightJacobianInvSO3 (const Eigen::MatrixBase< Derived1 > &phi, const Eigen::MatrixBase< Derived2 > &J_phi)
 Right Inverse Jacobian for SO(3) More...
 
template<typename Derived1 , typename Derived2 >
void leftJacobianSO3 (const Eigen::MatrixBase< Derived1 > &phi, const Eigen::MatrixBase< Derived2 > &J_phi)
 Left Jacobian for SO(3) More...
 
template<typename Derived1 , typename Derived2 >
void leftJacobianInvSO3 (const Eigen::MatrixBase< Derived1 > &phi, const Eigen::MatrixBase< Derived2 > &J_phi)
 Left Inverse Jacobian for SO(3) More...
 
template<typename Derived1 , typename Derived2 >
void rightJacobianSE3Decoupled (const Eigen::MatrixBase< Derived1 > &phi, const Eigen::MatrixBase< Derived2 > &J_phi)
 Right Jacobian for decoupled SE(3) More...
 
template<typename Derived1 , typename Derived2 >
void rightJacobianInvSE3Decoupled (const Eigen::MatrixBase< Derived1 > &phi, const Eigen::MatrixBase< Derived2 > &J_phi)
 Right Inverse Jacobian for decoupled SE(3) More...
 
template<typename Derived1 , typename Derived2 >
void rightJacobianSim3Decoupled (const Eigen::MatrixBase< Derived1 > &phi, const Eigen::MatrixBase< Derived2 > &J_phi)
 Right Jacobian for decoupled Sim(3) More...
 
template<typename Derived1 , typename Derived2 >
void rightJacobianInvSim3Decoupled (const Eigen::MatrixBase< Derived1 > &phi, const Eigen::MatrixBase< Derived2 > &J_phi)
 Right Inverse Jacobian for decoupled Sim(3) More...
 

Function Documentation

◆ leftJacobianInvSO3()

template<typename Derived1 , typename Derived2 >
void Sophus::leftJacobianInvSO3 ( const Eigen::MatrixBase< Derived1 > &  phi,
const Eigen::MatrixBase< Derived2 > &  J_phi 
)
inline

Left Inverse Jacobian for SO(3)

For \( \exp(x) \in SO(3) \) provides an inverse Jacobian that approximates the logmap of the left multiplication of expmap of the arguments with a sum for small \( \epsilon \). Can be used to compute: \( \log (\exp(\epsilon) \exp(\phi)) \approx \phi + J_{\phi} \epsilon\)

Parameters
[in]phi(3x1 vector)
[out]J_phi(3x3 matrix)

◆ leftJacobianSO3()

template<typename Derived1 , typename Derived2 >
void Sophus::leftJacobianSO3 ( const Eigen::MatrixBase< Derived1 > &  phi,
const Eigen::MatrixBase< Derived2 > &  J_phi 
)
inline

Left Jacobian for SO(3)

For \( \exp(x) \in SO(3) \) provides a Jacobian that approximates the sum under expmap with a left multiplication of expmap for small \( \epsilon \). Can be used to compute: \( \exp(\phi + \epsilon) \approx \exp(J_{\phi} \epsilon) \exp(\phi) \)

Parameters
[in]phi(3x1 vector)
[out]J_phi(3x3 matrix)

◆ rightJacobianInvSE3Decoupled()

template<typename Derived1 , typename Derived2 >
void Sophus::rightJacobianInvSE3Decoupled ( const Eigen::MatrixBase< Derived1 > &  phi,
const Eigen::MatrixBase< Derived2 > &  J_phi 
)
inline

Right Inverse Jacobian for decoupled SE(3)

For \( \exp(x) \in SE(3) \) provides an inverse Jacobian that approximates the decoupled logmap of the right multiplication of the decoupled expmap of the arguments with a sum for small \( \epsilon \). Can be used to compute: \( \log (\exp(\phi) \exp(\epsilon)) \approx \phi + J_{\phi} \epsilon\)

Parameters
[in]phi(6x1 vector)
[out]J_phi(6x6 matrix)

◆ rightJacobianInvSim3Decoupled()

template<typename Derived1 , typename Derived2 >
void Sophus::rightJacobianInvSim3Decoupled ( const Eigen::MatrixBase< Derived1 > &  phi,
const Eigen::MatrixBase< Derived2 > &  J_phi 
)
inline

Right Inverse Jacobian for decoupled Sim(3)

For \( \exp(x) \in Sim(3) \) provides an inverse Jacobian that approximates the decoupled logmap of the right multiplication of the decoupled expmap of the arguments with a sum for small \( \epsilon \). Can be used to compute: \( \log (\exp(\phi) \exp(\epsilon)) \approx \phi + J_{\phi} \epsilon\)

Parameters
[in]phi(7x1 vector)
[out]J_phi(7x7 matrix)

◆ rightJacobianInvSO3()

template<typename Derived1 , typename Derived2 >
void Sophus::rightJacobianInvSO3 ( const Eigen::MatrixBase< Derived1 > &  phi,
const Eigen::MatrixBase< Derived2 > &  J_phi 
)
inline

Right Inverse Jacobian for SO(3)

For \( \exp(x) \in SO(3) \) provides an inverse Jacobian that approximates the logmap of the right multiplication of expmap of the arguments with a sum for small \( \epsilon \). Can be used to compute: \( \log (\exp(\phi) \exp(\epsilon)) \approx \phi + J_{\phi} \epsilon\)

Parameters
[in]phi(3x1 vector)
[out]J_phi(3x3 matrix)

◆ rightJacobianSE3Decoupled()

template<typename Derived1 , typename Derived2 >
void Sophus::rightJacobianSE3Decoupled ( const Eigen::MatrixBase< Derived1 > &  phi,
const Eigen::MatrixBase< Derived2 > &  J_phi 
)
inline

Right Jacobian for decoupled SE(3)

For \( \exp(x) \in SE(3) \) provides a Jacobian that approximates the sum under decoupled expmap with a right multiplication of decoupled expmap for small \( \epsilon \). Can be used to compute: \( \exp(\phi + \epsilon) \approx \exp(\phi) \exp(J_{\phi} \epsilon)\)

Parameters
[in]phi(6x1 vector)
[out]J_phi(6x6 matrix)

◆ rightJacobianSim3Decoupled()

template<typename Derived1 , typename Derived2 >
void Sophus::rightJacobianSim3Decoupled ( const Eigen::MatrixBase< Derived1 > &  phi,
const Eigen::MatrixBase< Derived2 > &  J_phi 
)
inline

Right Jacobian for decoupled Sim(3)

For \( \exp(x) \in Sim(3) \) provides a Jacobian that approximates the sum under decoupled expmap with a right multiplication of decoupled expmap for small \( \epsilon \). Can be used to compute: \( \exp(\phi + \epsilon) \approx \exp(\phi) \exp(J_{\phi} \epsilon)\)

Parameters
[in]phi(7x1 vector)
[out]J_phi(7x7 matrix)

◆ rightJacobianSO3()

template<typename Derived1 , typename Derived2 >
void Sophus::rightJacobianSO3 ( const Eigen::MatrixBase< Derived1 > &  phi,
const Eigen::MatrixBase< Derived2 > &  J_phi 
)
inline

Right Jacobian for SO(3)

For \( \exp(x) \in SO(3) \) provides a Jacobian that approximates the sum under expmap with a right multiplication of expmap for small \( \epsilon \). Can be used to compute: \( \exp(\phi + \epsilon) \approx \exp(\phi) \exp(J_{\phi} \epsilon)\)

Parameters
[in]phi(3x1 vector)
[out]J_phi(3x3 matrix)

◆ se3_expd()

template<typename Derived >
SE3<typename Derived::Scalar> Sophus::se3_expd ( const Eigen::MatrixBase< Derived > &  upsilon_omega)
inline

Decoupled version of expmap for SE(3)

For tangent vector \( (\upsilon, \omega) \in \mathbb{R}^6 \) returns

\[ \begin{pmatrix} \exp(\omega) & \upsilon \\ 0 & 1 \end{pmatrix} \in SE(3), \]

where \( \exp(\omega) \in SO(3) \). Here rotation is not coupled with translation.

Parameters
[in]tangentvector (6x1 vector)
Returns
SE(3) member

◆ se3_logd()

template<typename Scalar >
SE3<Scalar>::Tangent Sophus::se3_logd ( const SE3< Scalar > &  se3)
inline

Decoupled version of logmap for SE(3)

For SE(3) element vector

\[ \begin{pmatrix} R & t \\ 0 & 1 \end{pmatrix} \in SE(3), \]

returns \( (t, \log(R)) \in \mathbb{R}^6 \). Here rotation is not coupled with translation.

Parameters
[in]SE(3)member
Returns
tangent vector (6x1 vector)

◆ sim3_expd()

template<typename Derived >
Sim3<typename Derived::Scalar> Sophus::sim3_expd ( const Eigen::MatrixBase< Derived > &  upsilon_omega_sigma)
inline

Decoupled version of expmap for Sim(3)

For tangent vector \( (\upsilon, \omega, \sigma) \in \mathbb{R}^7 \) returns

\[ \begin{pmatrix} \exp(\sigma)\exp(\omega) & \upsilon \\ 0 & 1 \end{pmatrix} \in Sim(3), \]

where \( \exp(\omega) \in SO(3) \). Here rotation and scale are not coupled with translation. Rotation and scale are commutative anyway.

Parameters
[in]tangentvector (7x1 vector)
Returns
Sim(3) member

◆ sim3_logd()

template<typename Scalar >
Sim3<Scalar>::Tangent Sophus::sim3_logd ( const Sim3< Scalar > &  sim3)
inline

Decoupled version of logmap for Sim(3)

For Sim(3) element vector

\[ \begin{pmatrix} sR & t \\ 0 & 1 \end{pmatrix} \in SE(3), \]

returns \( (t, \log(R), log(s)) \in \mathbb{R}^3 \). Here rotation and scale are not coupled with translation. Rotation and scale are commutative anyway.

Parameters
[in]Sim(3)member
Returns
tangent vector (7x1 vector)