点云库C++中如何将4×4变换矩阵转换为Eigen::Quaternionf四元数

具体操作步骤 #

你只需要从4×4变换矩阵中提取左上角3×3的旋转分量，再直接构造四元数即可，完整实现代码如下：

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

int main() {
    // 你的4×4点云变换矩阵，这里替换为你实际获取到的数值
    Eigen::Matrix4f transform_matrix;
    transform_matrix <<  0.707, -0.707, 0, 1,
                         0.707,  0.707, 0, 2,
                         0,      0,     1, 3,
                         0,      0,     0, 1;

    // 第一步：提取左上角3×3纯旋转矩阵
    Eigen::Matrix3f rotation_matrix = transform_matrix.block<3,3>(0,0);

    // 第二步：构造Eigen::Quaternionf对象
    Eigen::Quaternionf quaternion(rotation_matrix);

    // 可选：浮点误差较大时执行归一化，保证四元数模长为1
    quaternion.normalize();

    // 测试输出四元数分量，顺序为w x y z
    std::cout << "Quaternion: w=" << quaternion.w() 
              << ", x=" << quaternion.x() 
              << ", y=" << quaternion.y() 
              << ", z=" << quaternion.z() << std::endl;
    return 0;
}

注意事项 #

• 请确保你的4×4矩阵是刚体变换矩阵，左上角3×3部分满足旋转矩阵正交性要求，否则生成的四元数会存在数值偏差。
• Eigen的Quaternionf内部存储顺序为[x,y,z,w]，不要直接用下标访问原始数据，调用w()/x()/y()/z()方法获取对应分量更稳妥。
• 如果你的变换矩阵是Eigen::Isometry3f类型，可以直接调用rotation()方法获取旋转矩阵，无需手动切分block，示例如下：

Eigen::Isometry3f transform_isom;
// 给transform_isom赋值的逻辑省略
Eigen::Quaternionf quaternion(transform_isom.rotation());

内容的提问来源于stack exchange，提问作者tarek ali abd allah teama