Ocean
SophusUtilities.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) Meta Platforms, Inc. and affiliates.
3  *
4  * This source code is licensed under the MIT license found in the
5  * LICENSE file in the root directory of this source tree.
6  */
7 
8 #ifndef META_OCEAN_MATH_SOPHUS_UTILITIES_H
9 #define META_OCEAN_MATH_SOPHUS_UTILITIES_H
10 
12 #include "ocean/math/Quaternion.h"
13 
14 #include <sophus/se3.hpp>
15 #include <sophus/sim3.hpp>
16 
17 namespace Ocean
18 {
19 
20 /**
21  * This class implements utilitiy functions for Sophus.
22  * @ingroup math
23  */
25 {
26  public:
27 
28  /**
29  * Converts a Sophus::SE3 object to an Ocean HomogenousMatrix4 object.
30  * @param sophusSE The Sophus object to be converted
31  * @return The resulting Ocean object
32  * @tparam TSource The data type of the source SE elements, e.g., 'float', or 'double'
33  * @tparam TTarget The data type of the target matrix elements, e.g., 'float', or 'double'
34  */
35  template <typename TSource, typename TTarget>
36  static inline HomogenousMatrixT4<TTarget> toHomogenousMatrix4(const Sophus::SE3<TSource>& sophusSE);
37 
38  /**
39  * Converts a Sophus::Sim3 object to an Ocean HomogenousMatrix4 object.
40  * @param sophusSim The Sophus object to be converted
41  * @return The resulting Ocean object
42  * @tparam TSource The data type of the source SE elements, e.g., 'float', or 'double'
43  * @tparam TTarget The data type of the target matrix elements, e.g., 'float', or 'double'
44  */
45  template <typename TSource, typename TTarget>
46  static inline HomogenousMatrixT4<TTarget> toHomogenousMatrix4(const Sophus::Sim3<TSource>& sophusSim);
47 
48  /**
49  * Converts an Ocean HomogenousMatrix4 object to a Sophus::SE3 object.
50  * @param homogenousMatrix The Ocean homgenous matrix object to convert, must be valid
51  * @return The resulting Sophus::SE3 object
52  * @tparam TSource The data type of the source matrix elements, e.g., 'float', or 'double'
53  * @tparam TTarget The data type of the target SE elements, e.g., 'float', or 'double'
54  */
55  template <typename TSource, typename TTarget>
56  static inline Sophus::SE3<TTarget> toSE3(const HomogenousMatrixT4<TSource>& homogenousMatrix);
57 };
58 
59 template <typename TSource, typename TTarget>
60 inline HomogenousMatrixT4<TTarget> SophusUtilities::toHomogenousMatrix4(const Sophus::SE3<TSource>& sophusSE)
61 {
62  const Sophus::Matrix4<TSource> sophusMatrix = sophusSE.matrix();
63 
64  ocean_assert(NumericT<TSource>::isEqualEps(sophusMatrix(3, 0)));
65  ocean_assert(NumericT<TSource>::isEqualEps(sophusMatrix(3, 1)));
66  ocean_assert(NumericT<TSource>::isEqualEps(sophusMatrix(3, 2)));
67  ocean_assert(NumericT<TSource>::isEqual(sophusMatrix(3, 3), TSource(1)));
68 
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)));
73 
74  return HomogenousMatrixT4<TTarget>(xAxis, yAxis, zAxis, translation);
75 }
76 
77 template <typename TSource, typename TTarget>
78 inline HomogenousMatrixT4<TTarget> SophusUtilities::toHomogenousMatrix4(const Sophus::Sim3<TSource>& sophusSim)
79 {
80  const Sophus::Matrix<TSource, 4, 4> matrix(sophusSim.matrix());
81 
82  return HomogenousMatrixT4<TTarget>(matrix.data(), matrix.IsRowMajor);
83 }
84 
85 template <typename TSource, typename TTarget>
86 inline Sophus::SE3<TTarget> SophusUtilities::toSE3(const HomogenousMatrixT4<TSource>& homogenousMatrix)
87 {
88  const QuaternionT<TSource> quaternion = homogenousMatrix.rotation();
89  const VectorT3<TSource> translation = homogenousMatrix.translation();
90 
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()));
93 
94  return Sophus::SE3<TTarget>(eigenQuaternion, eigenTranslation);
95 }
96 
97 }
98 
99 #endif // META_OCEAN_MATH_SOPHUS_UTILITIES_H
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