a/math: Add multiple isometry-related matrix functions

An isometry is a rigid body transform. In this context I'm using the term to
refer to 4x4 homogeneous matrices in SE(3). I.e., matrices comprised of a
3x3 rotation, a 3x1 translation, and a [0,0,0,1] last row.
This matrix represent both rigid body transforms.
This commit is contained in:
Mateo de Mayo 2022-05-23 10:42:24 -03:00
parent 8b2fa955b9
commit 05b8e320a5
3 changed files with 186 additions and 0 deletions

View file

@ -163,6 +163,15 @@ math_vec3_cross(const struct xrt_vec3 *l, const struct xrt_vec3 *r, struct xrt_v
void
math_vec3_f64_cross(const struct xrt_vec3_f64 *l, const struct xrt_vec3_f64 *r, struct xrt_vec3_f64 *result);
/*!
* Get translation vector from isometry matrix (col-major).
*
* @relates xrt_vec3
* @ingroup aux_math
*/
void
math_vec3_translation_from_isometry(const struct xrt_matrix_4x4 *isometry, struct xrt_vec3 *result);
/*!
* Normalize a vec3 in place.
*
@ -417,6 +426,15 @@ math_matrix_3x3_identity(struct xrt_matrix_3x3 *mat);
void
math_matrix_3x3_f64_identity(struct xrt_matrix_3x3_f64 *mat);
/*!
* Print a 3x3 (col-major) matrix to stdout
*
* @see xrt_matrix_3x3
* @ingroup aux_math
*/
void
math_matrix_3x3_print(const struct xrt_matrix_3x3 *mat);
/*!
* Transform a vec3 by a 3x3 matrix
*
@ -459,6 +477,15 @@ math_matrix_3x3_multiply(const struct xrt_matrix_3x3 *left,
void
math_matrix_3x3_inverse(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result);
/*!
* Transpose Matrix3x3
*
* @relates xrt_matrix_3x3
* @ingroup aux_math
*/
void
math_matrix_3x3_transpose(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result);
/*!
* Create a rotation from two vectors plus x and z, by
* creating a rotation matrix by crossing z and x to
@ -474,6 +501,15 @@ math_matrix_3x3_f64_from_plus_x_z(const struct xrt_vec3_f64 *plus_x,
const struct xrt_vec3_f64 *plus_z,
struct xrt_matrix_3x3_f64 *result);
/*!
* Get the rotation matrix from an isomertry matrix (col-major).
*
* @relates xrt_matrix_4x4
* @ingroup aux_math
*/
void
math_matrix_3x3_rotation_from_isometry(const struct xrt_matrix_4x4 *isometry, struct xrt_matrix_3x3 *result);
/*!
* Initialize Matrix4x4 with identity.
*
@ -483,6 +519,15 @@ math_matrix_3x3_f64_from_plus_x_z(const struct xrt_vec3_f64 *plus_x,
void
math_matrix_4x4_identity(struct xrt_matrix_4x4 *result);
/*!
* Prints a Matrix4x4 (col-major).
*
* @relates xrt_matrix_4x4
* @ingroup aux_math
*/
void
math_matrix_4x4_print(const struct xrt_matrix_4x4 *mat);
/*!
* Multiply Matrix4x4.
*
@ -494,6 +539,33 @@ math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left,
const struct xrt_matrix_4x4 *right,
struct xrt_matrix_4x4 *result);
/*!
* Invert Matrix4x4.
*
* @relates xrt_matrix_4x4
* @ingroup aux_math
*/
void
math_matrix_4x4_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result);
/*!
* Invert a homogeneous isometry 4x4 (col-major) matrix in SE(3).
*
* @relates xrt_matrix_4x4
* @ingroup aux_math
*/
void
math_matrix_4x4_isometry_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result);
/*!
* Transpose Matrix4x4
*
* @relates xrt_matrix_4x4
* @ingroup aux_math
*/
void
math_matrix_4x4_transpose(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result);
/*!
* Compute view matrix from xrt_pose.
*
@ -503,6 +575,18 @@ math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left,
void
math_matrix_4x4_view_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result);
/*!
* Get an isometry matrix in SE(3) from a rotation matrix SO(3) and a
* translation vector. All col-major matrices.
*
* @relates xrt_matrix_4x4
* @ingroup aux_math
*/
void
math_matrix_4x4_isometry_from_rt(const struct xrt_matrix_3x3 *rotation,
const struct xrt_vec3 *translation,
struct xrt_matrix_4x4 *result);
/*!
* Compute quad layer model matrix from xrt_pose and xrt_vec2 size.
*

View file

@ -12,6 +12,7 @@
#include "math/m_api.h"
#include "math/m_eigen_interop.hpp"
#include "util/u_logging.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
@ -418,6 +419,17 @@ math_matrix_3x3_f64_identity(struct xrt_matrix_3x3_f64 *mat)
map_matrix_3x3_f64(*mat) = Eigen::Matrix3d::Identity();
}
extern "C" void
math_matrix_3x3_print(const struct xrt_matrix_3x3 *mat)
{
const auto &m = mat->v;
U_LOG_RAW("[\n");
U_LOG_RAW("\t%f, %f, %f,\n", m[0], m[3], m[6]);
U_LOG_RAW("\t%f, %f, %f,\n", m[1], m[4], m[7]);
U_LOG_RAW("\t%f, %f, %f \n", m[2], m[5], m[8]);
U_LOG_RAW("]\n");
}
extern "C" void
math_matrix_3x3_f64_transform_vec3_f64(const struct xrt_matrix_3x3_f64 *left,
const struct xrt_vec3_f64 *right,
@ -450,6 +462,13 @@ math_matrix_3x3_f64_from_plus_x_z(const struct xrt_vec3_f64 *plus_x,
result->v[8] = plus_z->z;
}
extern "C" void
math_matrix_3x3_rotation_from_isometry(const struct xrt_matrix_4x4 *isometry, struct xrt_matrix_3x3 *result)
{
Eigen::Isometry3f transform{map_matrix_4x4(*isometry)};
map_matrix_3x3(*result) = transform.linear();
}
extern "C" void
math_matrix_3x3_transform_vec3(const struct xrt_matrix_3x3 *left,
const struct xrt_vec3 *right,
@ -493,12 +512,31 @@ math_matrix_3x3_inverse(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *
map_matrix_3x3(*result) = m.inverse();
}
extern "C" void
math_matrix_3x3_transpose(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result)
{
Eigen::Matrix3f m = copy(in);
map_matrix_3x3(*result) = m.transpose();
}
extern "C" void
math_matrix_4x4_identity(struct xrt_matrix_4x4 *result)
{
map_matrix_4x4(*result) = Eigen::Matrix4f::Identity();
}
extern "C" void
math_matrix_4x4_print(const struct xrt_matrix_4x4 *mat)
{
const auto &m = mat->v;
U_LOG_RAW("[\n");
U_LOG_RAW("\t%f, %f, %f, %f,\n", m[0], m[4], m[8], m[12]);
U_LOG_RAW("\t%f, %f, %f, %f,\n", m[1], m[5], m[9], m[13]);
U_LOG_RAW("\t%f, %f, %f, %f,\n", m[2], m[6], m[10], m[14]);
U_LOG_RAW("\t%f, %f, %f, %f \n", m[3], m[7], m[11], m[15]);
U_LOG_RAW("]\n");
}
extern "C" void
math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left,
const struct xrt_matrix_4x4 *right,
@ -507,6 +545,27 @@ math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left,
map_matrix_4x4(*result) = copy(left) * copy(right);
}
extern "C" void
math_matrix_4x4_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result)
{
Eigen::Matrix4f m = copy(in);
map_matrix_4x4(*result) = m.inverse();
}
extern "C" void
math_matrix_4x4_transpose(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result)
{
Eigen::Matrix4f m = copy(in);
map_matrix_4x4(*result) = m.transpose();
}
extern "C" void
math_matrix_4x4_isometry_inverse(const struct xrt_matrix_4x4 *in, struct xrt_matrix_4x4 *result)
{
Eigen::Isometry3f m{copy(in)};
map_matrix_4x4(*result) = m.inverse().matrix();
}
extern "C" void
math_matrix_4x4_view_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result)
{
@ -519,6 +578,17 @@ math_matrix_4x4_view_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x
map_matrix_4x4(*result) = transformation.inverse().matrix();
}
extern "C" void
math_matrix_4x4_isometry_from_rt(const struct xrt_matrix_3x3 *rotation,
const struct xrt_vec3 *translation,
struct xrt_matrix_4x4 *result)
{
Eigen::Isometry3f transformation = Eigen::Isometry3f::Identity();
transformation.linear() = map_matrix_3x3(*rotation);
transformation.translation() = map_vec3(*translation);
map_matrix_4x4(*result) = transformation.matrix();
}
extern "C" void
math_matrix_4x4_model(const struct xrt_pose *pose, const struct xrt_vec3 *size, struct xrt_matrix_4x4 *result)
{
@ -582,6 +652,13 @@ math_vec3_f64_cross(const struct xrt_vec3_f64 *l, const struct xrt_vec3_f64 *r,
map_vec3_f64(*result) = map_vec3_f64(*l).cross(map_vec3_f64(*r));
}
extern "C" void
math_vec3_translation_from_isometry(const struct xrt_matrix_4x4 *transform, struct xrt_vec3 *result)
{
Eigen::Isometry3f isometry{map_matrix_4x4(*transform)};
map_vec3(*result) = isometry.translation();
}
extern "C" void
m_mat4_f64_orientation(const struct xrt_quat *quat, struct xrt_matrix_4x4_f64 *result)
{

View file

@ -98,6 +98,18 @@ map_vec3_f64(struct xrt_vec3_f64 &v)
return Eigen::Map<Eigen::Vector3d>{&v.x};
}
/*!
* @brief Wrap an internal 3x3 matrix struct in an Eigen type, const overload.
*
* Permits zero-overhead manipulation of `xrt_matrix_3x3&` by Eigen routines as
* if it were a `Eigen::Matrix3f&`.
*/
static inline Eigen::Map<const Eigen::Matrix3f>
map_matrix_3x3(const struct xrt_matrix_3x3 &m)
{
return Eigen::Map<const Eigen::Matrix3f>(m.v);
}
/*!
* @brief Wrap an internal 3x3 matrix struct in an Eigen type, non-const
* overload.
@ -124,6 +136,19 @@ map_matrix_3x3_f64(struct xrt_matrix_3x3_f64 &m)
return Eigen::Map<Eigen::Matrix3d>(m.v);
}
/*!
* @brief Wrap an internal 4x4 matrix struct in an Eigen type, const
* overload.
*
* Permits zero-overhead manipulation of `xrt_matrix_4x4&` by Eigen routines as
* if it were a `Eigen::Matrix4f&`.
*/
static inline Eigen::Map<const Eigen::Matrix4f>
map_matrix_4x4(const struct xrt_matrix_4x4 &m)
{
return Eigen::Map<const Eigen::Matrix4f>(m.v);
}
/*!
* @brief Wrap an internal 4x4 matrix struct in an Eigen type, non-const
* overload.