adding function to interpolate poses.

although this functionality is available in some components there in no common place.

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
 
@@ -220,6 +221,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

SimoxUtility/math/pose/interpolate.cpp

+#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

SimoxUtility/math/pose/interpolate.h

+#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