# Interpolation for between two poses

Hey all,

I am looking for a function which interpolate between two poseStamped poses for a given time. I know there has to be something withing the tf/tf2 API but I could not find it. Something along the line of

poseStamped = poseInterpolation (poseStamp_t1,poseStamp_t2, t) where t_1 < t < t_2

Would be glad for some pointers.

Cheers

edit retag close merge delete

Sort by ยป oldest newest most voted

Here is a solution using Eigen.

The stamped poses (poseStamp_t1 and poseStamp_t2 in your notation) have a timestamp and a pose. The timestamps can be saved to variables t1 and t2, such as t1 = poseStamp_t1.header.stamp.toSec().

The poses can be converted to Eigen with the function tf::poseMsgToEigen (const geometry_msgs::Pose &m, Eigen::Affine3d &e), obtaining Eigen:Affine poses aff1 and aff2. From here, the rotation and the translation part can be interpolated separately. Here is how:

// C++ code

#include <Eigen/Geometry>

// Take poses at times t1 and t2. Interpolate the pose at time t.
Eigen::Affine3d pose_interp(double t, double t1, double t2,
Eigen::Affine3d const& aff1, Eigen::Affine3d const& aff2) {
// assume here t1 <= t <= t2
double alpha = 0.0;
if (t2 != t1)
alpha = (t - t1) / (t2 - t1);

Eigen::Quaternion<double> rot1(aff1.linear());
Eigen::Quaternion<double> rot2(aff2.linear());

Eigen::Vector3d trans1 = aff1.translation();
Eigen::Vector3d trans2 = aff2.translation();

Eigen::Affine3d result;
result.translation() = (1.0 - alpha) * trans1 + alpha * trans2;
result.linear()      = rot1.slerp(alpha, rot2).toRotationMatrix();

return result;
}

more

I'd recommend looking at: https://github.com/ros/geometry2/blob... which uses the internal fork of the Bullet Linear Math library.

I'd recommend you implement your own version using the pure bullet API.

more