鄂尔多斯北京网站建设,网站建设使用哪种语言好,沈阳男科医院在线咨询免费,网上注册公司的网址文章目录0 写在前面1 数学背景对比2 Eigen 实现差异3 Isometry3d 是不是 4 4 矩阵#xff1f;4 核心 API 速查5 实战示例5.1 SLAM 位姿链#xff1a;相机点 → 世界点5.2 体素滤波#xff1a;各向异性缩放#xff08;X/Y → 5 cm#xff0c;Z → 10 cm#xff09;5.3 把…
文章目录0 写在前面1 数学背景对比2 Eigen 实现差异3 Isometry3d 是不是 4 × 4 矩阵4 核心 API 速查5 实战示例5.1 SLAM 位姿链相机点 → 世界点5.2 体素滤波各向异性缩放X/Y → 5 cmZ → 10 cm5.3 把通用 4×4 矩阵 M “正交化”为刚体变换6 性能与数值建议7 常见坑 · 排雷8 Cheat Sheet (贴工位)9 总结参考资料目录0 写在前面1 数学背景对比2 Eigen 实现差异3 Isometry3d 是不是 4 × 4 矩阵4 核心 API 速查5 实战示例5.1 SLAM 位姿链相机点 → 世界点5.2 体素滤波各向异性缩放X/Y → 5 cmZ → 10 cm5.3 把通用 4×4 矩阵 M “正交化”为刚体变换6 性能与数值建议7 常见坑 · 排雷8 Cheat Sheet (贴工位)9 总结参考资料0 写在前面
在 vSLAM / VIO / 机器人定位等工程实践中90 % 的变换只是旋转 平移的刚体运动只有不到 10 % 的场景点云体素缩放、图像几何映射、CAD 变形等才会用到带缩放或剪切的仿射变换。Eigen 针对这两类需求提供了
类别Eigen 类型数学对应刚体变换Eigen::Isometry3dSE(3)一般仿射Eigen::Affine3dA(3)
本文将从 数学原理 → 内部结构 → 常用 API → 实战示例 → 性能建议 → 易错排查 全方位梳理它们并回答一个高频疑惑Isometry3d 究竟是不是 4 × 4 矩阵 1 数学背景对比
刚体变换 SE(3)仿射变换 A(3)定义保持点间距离不变的变换线性变换 A 平移 t可带缩放/剪切矩阵结构[Rt01]\displaystyle\begin{bmatrix}Rt\\01\end{bmatrix}[R0t1]R∈SO(3)R∈SO(3)R∈SO(3)[At01]\displaystyle\begin{bmatrix}At\\01\end{bmatrix}[A0t1]AAA 任意 3 × 3自由度6 (3 旋转 3 平移)≤ 12 (9 线性 3 平移)典型应用传感器外参、SLAM 位姿、TF 广播图像/点云缩放、CAD 模型变形2 Eigen 实现差异
特性Isometry3dAffine3d旋转部分强制正交det 1任意 3 × 3存储封装 4 × 4 矩阵刚体特化封装 4 × 4 矩阵求逆只需 R⊤,−R⊤tR^\top,\,-R^\top tR⊤,−R⊤t通用 4 × 4 逆内存12 double (9 R 3 t)16 double语义保障保证组合后仍为刚体可能引入缩放/剪切3 Isometry3d 是不是 4 × 4 矩阵 结论是 Isometry3d 是 4 × 4 齐次刚体矩阵的封装类。 数学结构 T[Rt01],R∈SO(3)T\begin{bmatrix}Rt\\01\end{bmatrix},\;R∈SO(3) T[R0t1],R∈SO(3) 代码验证 Eigen::Isometry3d T Eigen::Isometry3d::Identity();
Eigen::Matrix4d H T.matrix(); // H 是标准 4x4类型本质维度刚体约束典型用途Matrix4d纯 4×4 矩阵4×4❌渲染 / 存盘Isometry3d刚体 SE(3)4×4 (封装)✅SLAM / VIOAffine3d仿射 A(3)4×4 (封装)❌图像/点云缩放4 核心 API 速查
#include Eigen/Geometry// ── Isometry3d ───────────────────────────
Isometry3d T Isometry3d::Identity();
T.rotate(q.toRotationMatrix()); // 写入旋转
T.pretranslate(t); // 左乘平移
Matrix4d H T.matrix(); // 取 4×4// 组合与逆
Isometry3d Tab Ta * Tb; // 右侧先执行
Isometry3d Tinv T.inverse(); // 快速刚体逆
Vector3d pw T * pc; // 点变换// ── Affine3d ────────────────────────────
Affine3d A Affine3d::Identity();
A.linear() 0.5, 0, 0,0, 0.5, 0,0, 0, 1; // 含缩放
A.translation() Vector3d(1,2,3);5 实战示例
5.1 SLAM 位姿链相机点 → 世界点
// T_w_b : body 坐标系 → world 坐标系 车辆/IMU Pose
Isometry3d T_w_b; // T_b_c : camera 坐标系 → body 坐标系 相机外参
Isometry3d T_b_c; // p_c : 相机坐标系下的点坐标单位 m
Vector3d p_c(0.1, 0.2, 1.0);// 右乘先算p_c 先映射到 body再映射到 world
Vector3d p_w T_w_b * T_b_c * p_c; 要点 乘法写成 “外层坐标系 * 内层坐标系 * 点”代码顺序 实际执行的坐标系级联最右边先计算 5.2 体素滤波各向异性缩放X/Y → 5 cmZ → 10 cm
Affine3d voxel Affine3d::Identity();// 设置线性部分对 (x,y,z) 轴分别乘以 0.05, 0.05, 0.10
voxel.linear() 0.05, 0, 0,0, 0.05, 0,0, 0, 0.10;// 若还需平移可再设置 voxel.translation()// 直接对点或点云做缩放
point voxel * point;要点 取 Affine3d因为含 缩放 ⇒ 非刚体.linear() 修改 3 × 3 线性块.translation() 负责平移 5.3 把通用 4×4 矩阵 M “正交化”为刚体变换
// ① 读进来时先用 Affine3d 保存保留所有信息
Affine3d A(M); // ② 提取线性部分做极分解 / 四元数归一化得到最接近的正交矩阵
Quaterniond q(A.linear()); // 自动正交化
Matrix3d R q.toRotationMatrix();// ③ 重新组装为 Isometry3d刚体平移直接拷贝
Isometry3d T;
T.linear() R; // 旋转 (正交)
T.translation() A.translation(); // 平移要点 外部矩阵不保证正交 → 先用 Affine3d 接收利用 Quaterniond 把 3 × 3 线性块正交化重新封装成 Isometry3d之后即可安全用于 SLAM 位姿累乘 6 性能与数值建议
场景推荐类型大量位姿累乘 / 求逆Isometry3d含尺度 / 剪切Affine3d不确定线性部分是否正交先 Affine3d再正交化与 Sophus / g2o 联合始终保持 Isometry3d7 常见坑 · 排雷
乘法顺序与注释不符 T_a_b * T_b_c 表示“先 c→b再 b→a”——注释别写反误往 Isometry3d.linear() 写入非正交矩阵 写完请确保 R.col(i).norm()1 且 det≈1。角度单位 Eigen 全部使用 弧度杜绝度→弧度混用。存盘丢语义 存 T.matrix()读后用 Isometry3d(H) 恢复。 8 Cheat Sheet (贴工位)
#include Eigen/GeometryIsometry3d T Isometry3d::Identity();
T.rotate(q); // 写旋转
T.pretranslate(t); // 写平移
Vector3d p T * pc; // 变换点
Matrix4d H T.matrix(); // 导出 4×4
T Isometry3d(H); // 从矩阵恢复9 总结
类型选它的理由典型任务Isometry3d强制刚体、逆运算 O(1)SLAM 位姿、外参、TFAffine3d允许缩放/剪切点云/图像几何、CADMatrix4d无语义最通用统一存盘 / 可视化
牢记 “右乘先算左乘决定结果坐标系”你的姿态链将稳固无坑。祝开发顺利 参考资料
Eigen 官方文档https://eigen.tuxfamily.org/dox/group__Geometry__Module.html《视觉 SLAM 十四讲》第 3 章
