eigenTransformOfPoses

gistfile1.txt

Eigen::Transform<double,3,Eigen::Affine> eigenTransformOfPoses(geometry_msgs::Pose from, geometry_msgs::Pose to)
{
    Eigen::Quaternionf fromQuat = rosQuatToEigenQuat(from.orientation);
    Eigen::Quaternionf toQuat = rosQuatToEigenQuat(to.orientation);

    Eigen::Quaternionf quaternionRotation = transFromQuatToQuat(fromQuat, toQuat);
    Eigen::Transform<float,3,Eigen::Affine> rotation(quaternionRotation);

    Eigen::Translation<double,3> translation(vectorOfPoints(from.position, to.position));
    Eigen::Transform<float,3,Eigen::Affine> T = rotation * translation.cast<float>();

    return T.cast<double>();
}