diff --git a/VirtualRobot/math/Helpers.cpp b/VirtualRobot/math/Helpers.cpp
index c02886445f7820c25bef97dc3a6b4d79312e0cc3..cef7a18d65a3494f5b5237a87d384b91236aee12 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 8793efebb9ece92a6ae858710cac6791af117c4f..1cead4ab1278f662e396f1d2ac62d53e371fa43e 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;
+    }
+    
 }