已知两个位姿和两个位姿间的时间，计算角速度。并根据此角速度，通过时间推算新的位姿

using namespace std;
using namespace Eigen;
//预测矫正帧位姿
Eigen::Vector3d angular_velocity_from_poses(Eigen::Matrix4d pose1, Eigen::Matrix4d pose2, double t) {
    // Extract rotation matrices
    Eigen::Matrix3d R1 = pose1.block<3,3>(0,0);
    Eigen::Matrix3d R2 = pose2.block<3,3>(0,0);

    // Compute rotation from pose1 to pose2
    Eigen::Matrix3d R = R1.transpose() * R2;

    // Convert to axis-angle representation
    Eigen::AngleAxisd angle_axis(R);

    // Compute angular velocity
    Eigen::Vector3d omega = angle_axis.axis() * angle_axis.angle() / t;

    return omega;
}
Eigen::Matrix4d update_pose_with_angular_velocity(Eigen::Matrix4d pose, Eigen::Vector3d omega, double t) {
    double theta = omega.norm() * t;
    if (theta == 0) {
        return pose;
    }
    Eigen::Vector3d axis = omega / omega.norm();
    Eigen::Matrix3d K;
    K << 0, -axis.z(), axis.y(),
         axis.z(), 0, -axis.x(),
         -axis.y(), axis.x(), 0;
    Eigen::Matrix3d R = cos(theta) * Eigen::Matrix3d::Identity() + sin(theta) * K + (1 - cos(theta)) * K * K;

    Eigen::Matrix4d delta_pose = Eigen::Matrix4d::Identity();
    delta_pose.block<3,3>(0,0) = R;

    return pose * delta_pose;
}

int main() {
    Eigen::Matrix4d pose1 = Eigen::Matrix4d::Identity();  // Initial pose
    Eigen::Matrix4d pose2 = Eigen::Matrix4d::Identity();  // Final pose
    pose2.block<3,3>(0,0) = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()).toRotationMatrix();  // Rotate 90 degrees around Z axis
    double t1 = 1.0;  // 两个已知位姿间的时间
    Eigen::Vector3d omega = angular_velocity_from_poses(pose1, pose2, t);

    double t2 = M_PI / 2;  // 给定任意时间
    Eigen::Matrix4d new_pose = update_pose_with_angular_velocity(pose1, omega, t);
    std::cout << new_pose << std::endl;
    return 0;
}