8 #ifndef META_OCEAN_MATH_SOPHUS_UTILITIES_H
9 #define META_OCEAN_MATH_SOPHUS_UTILITIES_H
14 #include <sophus/se3.hpp>
15 #include <sophus/sim3.hpp>
35 template <
typename TSource,
typename TTarget>
45 template <
typename TSource,
typename TTarget>
55 template <
typename TSource,
typename TTarget>
59 template <
typename TSource,
typename TTarget>
62 const Sophus::Matrix4<TSource> sophusMatrix = sophusSE.matrix();
69 const VectorT3<TTarget> xAxis(TTarget(sophusMatrix(0, 0)), TTarget(sophusMatrix(1, 0)), TTarget(sophusMatrix(2, 0)));
70 const VectorT3<TTarget> yAxis(TTarget(sophusMatrix(0, 1)), TTarget(sophusMatrix(1, 1)), TTarget(sophusMatrix(2, 1)));
71 const VectorT3<TTarget> zAxis(TTarget(sophusMatrix(0, 2)), TTarget(sophusMatrix(1, 2)), TTarget(sophusMatrix(2, 2)));
72 const VectorT3<TTarget> translation(TTarget(sophusMatrix(0, 3)), TTarget(sophusMatrix(1, 3)), TTarget(sophusMatrix(2, 3)));
77 template <
typename TSource,
typename TTarget>
80 const Sophus::Matrix<TSource, 4, 4> matrix(sophusSim.matrix());
85 template <
typename TSource,
typename TTarget>
91 const Eigen::Quaternion<TTarget> eigenQuaternion(TTarget(quaternion.
w()), TTarget(quaternion.
x()), TTarget(quaternion.
y()), TTarget(quaternion.
z()));
92 const Eigen::Matrix<TTarget, 3, 1, 0> eigenTranslation(TTarget(translation.
x()), TTarget(translation.
y()), TTarget(translation.
z()));
94 return Sophus::SE3<TTarget>(eigenQuaternion, eigenTranslation);
This class implements a 4x4 homogeneous transformation matrix using floating point values with the pr...
Definition: HomogenousMatrix4.h:110
VectorT3< T > translation() const
Returns the translation of the transformation.
Definition: HomogenousMatrix4.h:1381
QuaternionT< T > rotation() const
Returns the rotation of the transformation as quaternion.
Definition: HomogenousMatrix4.h:1388
This class provides basic numeric functionalities.
Definition: Numeric.h:57
This class implements a unit quaternion rotation.
Definition: Quaternion.h:100
const T & x() const
Returns the x value of the quaternion.
Definition: Quaternion.h:917
const T & w() const
Returns the w value of the quaternion.
Definition: Quaternion.h:905
const T & y() const
Returns the y value of the quaternion.
Definition: Quaternion.h:929
const T & z() const
Returns the z value of the quaternion.
Definition: Quaternion.h:941
This class implements utilitiy functions for Sophus.
Definition: SophusUtilities.h:25
static Sophus::SE3< TTarget > toSE3(const HomogenousMatrixT4< TSource > &homogenousMatrix)
Converts an Ocean HomogenousMatrix4 object to a Sophus::SE3 object.
Definition: SophusUtilities.h:86
static HomogenousMatrixT4< TTarget > toHomogenousMatrix4(const Sophus::SE3< TSource > &sophusSE)
Converts a Sophus::SE3 object to an Ocean HomogenousMatrix4 object.
Definition: SophusUtilities.h:60
This class implements a vector with three elements.
Definition: Vector3.h:97
const T & y() const noexcept
Returns the y value.
Definition: Vector3.h:812
const T & x() const noexcept
Returns the x value.
Definition: Vector3.h:800
const T & z() const noexcept
Returns the z value.
Definition: Vector3.h:824
The namespace covering the entire Ocean framework.
Definition: Accessor.h:15