diff --git a/SimoxUtility/CMakeLists.txt b/SimoxUtility/CMakeLists.txt
index 8e5c50a6de8de92b71c932d438c71de064fda6c6..dd3b2d65174ce122bb0c2e9d761b9ec94c597351 100644
--- a/SimoxUtility/CMakeLists.txt
+++ b/SimoxUtility/CMakeLists.txt
@@ -81,6 +81,7 @@ SET(SOURCES
     math/pose/check_rotation_matrix.cpp
     math/pose/invert.cpp
     math/pose/orthogonalize.cpp
+    math/pose/interpolate.cpp
 
     math/statistics/Histogram1D.cpp
 
@@ -221,6 +222,7 @@ SET(INCLUDES
     math/pose/orthogonalize.h
     math/pose/pose.h
     math/pose/transform.h
+    math/pose/interpolate.h
 
     math/similarity/cosine_similarity.h
     math/similarity/angular_similarity.h
diff --git a/SimoxUtility/math/pose/interpolate.cpp b/SimoxUtility/math/pose/interpolate.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..61dec29db6f41490a909e3c65477a95c085a799d
--- /dev/null
+++ b/SimoxUtility/math/pose/interpolate.cpp
@@ -0,0 +1,25 @@
+#include "interpolate.h"
+
+
+namespace simox::math {
+
+
+Eigen::Affine3f interpolatePose(const Eigen::Affine3f &posePre, const Eigen::Affine3f &poseNext, float t) {
+    
+    assert(0 <= t <= 1);
+    
+    Eigen::Affine3f pose = Eigen::Affine3f::Identity();
+
+    pose.translation() = (1 - t) * posePre.translation() + t * poseNext.translation();
+
+    Eigen::Quaternionf rotPrev(posePre.linear().matrix());
+    Eigen::Quaternionf rotNext(poseNext.linear().matrix());
+
+    Eigen::Quaternionf rotNew = rotPrev.slerp(t, rotNext);
+
+    pose.linear() = rotNew.toRotationMatrix();
+
+    return pose;
+}
+
+}  // namespace simox::math
\ No newline at end of file
diff --git a/SimoxUtility/math/pose/interpolate.h b/SimoxUtility/math/pose/interpolate.h
new file mode 100644
index 0000000000000000000000000000000000000000..f31e8dbe730afa2ec933967cf5dd04bc73e6f75c
--- /dev/null
+++ b/SimoxUtility/math/pose/interpolate.h
@@ -0,0 +1,9 @@
+#pragma once
+
+#include <Eigen/Geometry>
+
+namespace simox::math {
+
+Eigen::Affine3f interpolatePose(const Eigen::Affine3f &posePre, const Eigen::Affine3f &poseNext, float t);
+
+} // namespace simox::math
\ No newline at end of file