From 3bd3e0c96472fdf1be5d010473fad054c92afc61 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@student.kit.edu>
Date: Tue, 22 Jan 2019 09:20:39 +0100
Subject: [PATCH] Added invertPose/invertedPose
---
VirtualRobot/math/Helpers.cpp | 4 ++++
VirtualRobot/math/Helpers.h | 17 +++++++++++++++++
2 files changed, 21 insertions(+)
diff --git a/VirtualRobot/math/Helpers.cpp b/VirtualRobot/math/Helpers.cpp
index c02886445..cef7a18d6 100644
--- a/VirtualRobot/math/Helpers.cpp
+++ b/VirtualRobot/math/Helpers.cpp
@@ -258,6 +258,10 @@ Eigen::Matrix3f Helpers::GetOrientation(const Eigen::Matrix4f& pose)
return oriBlock(pose);
}
+void Helpers::InvertPose(Eigen::Matrix4f& pose)
+{
+ oriBlock(pose).transposeInPlace();
+ posBlock(pose) = - oriBlock(pose) * posBlock(pose);
}
Eigen::VectorXf Helpers::LimitVectorLength(const Eigen::VectorXf& vec, const Eigen::VectorXf& maxLen)
diff --git a/VirtualRobot/math/Helpers.h b/VirtualRobot/math/Helpers.h
index 8793efebb..1cead4ab1 100644
--- a/VirtualRobot/math/Helpers.h
+++ b/VirtualRobot/math/Helpers.h
@@ -62,6 +62,12 @@ namespace math
static Eigen::Vector3f CreateVectorFromCylinderCoords(float r, float angle, float z);
static Eigen::Matrix4f TranslatePose(const Eigen::Matrix4f &pose, const Eigen::Vector3f& offset);
+ /// Invert the given pose in-place.
+ static void InvertPose(Eigen::Matrix4f& pose);
+ /// Return the inverted of the given pose.
+ template <typename Derived>
+ static Eigen::Matrix4f InvertedPose(const Eigen::MatrixBase<Derived>& pose);
+
static Eigen::Vector3f TransformPosition(const Eigen::Matrix4f& transform, const Eigen::Vector3f &pos);
static Eigen::Vector3f TransformDirection(const Eigen::Matrix4f& transform, const Eigen::Vector3f &dir);
static Eigen::Matrix3f TransformOrientation(const Eigen::Matrix4f& transform, const Eigen::Matrix3f &ori);
@@ -69,7 +75,9 @@ namespace math
static Eigen::Vector3f GetPosition(const Eigen::Matrix4f& pose);
static Eigen::Matrix3f GetOrientation(const Eigen::Matrix4f& pose);
+
static Eigen::VectorXf LimitVectorLength(const Eigen::VectorXf& vec, const Eigen::VectorXf& maxLen);
+
static float rad2deg(float rad);
static float deg2rad(float deg);
@@ -160,5 +168,14 @@ namespace math
return toPose(pos, ori.toRotationMatrix());
}
+
+ template <typename Derived>
+ Eigen::Matrix4f Helpers::InvertedPose(const Eigen::MatrixBase<Derived>& pose)
+ {
+ Eigen::Matrix4f inv = pose;
+ InvertPose(inv);
+ return inv;
+ }
+
}
--
GitLab