Revision history [back]

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;

}

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;
}