这个是我之前做的一个项目现在总结一下一、问题本质已知一个点P (x, y, z)一个法向量n (nx, ny, nz)求刚体变换矩阵 T4×4 [ R t ] [ 0 1 ] 一个 X YZ 法向量 如何转换成一个旋转平移矩阵二、核心思想约束z轴 法向量x/y轴 在平面上的两个正交方向这就是一个 SO(3) 构造问题其中z 法向x 任意垂直于 z 的方向y 保证右手系本质从一个方向 → 补全成一个正交坐标系ẑ n / ||n||三、PCL使用和MatlabPCL#include Eigen/Dense Eigen::Matrix4f poseFromPointNormal( const Eigen::Vector3f point, const Eigen::Vector3f normal) { Eigen::Vector3f z normal.normalized(); // 选择辅助向量避免共线 Eigen::Vector3f aux(0, 0, 1); if (fabs(z.dot(aux)) 0.9) aux Eigen::Vector3f(1, 0, 0); // 构造正交基 Eigen::Vector3f x aux.cross(z).normalized(); Eigen::Vector3f y z.cross(x); // 旋转矩阵 Eigen::Matrix3f R; R.col(0) x; R.col(1) y; R.col(2) z; // 变换矩阵 Eigen::Matrix4f T Eigen::Matrix4f::Identity(); T.block3,3(0,0) R; T.block3,1(0,3) point; return T; }使用pcl::PointNormal pt; Eigen::Vector3f P(pt.x, pt.y, pt.z); Eigen::Vector3f n(pt.normal_x, pt.normal_y, pt.normal_z); Eigen::Matrix4f T poseFromPointNormal(P, n);注意点1、法向量方向不稳定n 和 -n 都是合法法向会导致姿态翻转抓取方向反了解决if (n.dot(view_direction) 0) n -n;注意点12、自由度缺失核心理解一个法向量只确定2个自由度还缺绕法向旋转你现在的 x 轴是“人为选的”注意点13、工业更稳定的写法 ⭐如果你有参考方向推荐比如重力方向 g (0,0,1)更优写法Eigen::Vector3f z normal.normalized(); Eigen::Vector3f x g.cross(z).normalized(); Eigen::Vector3f y z.cross(x);好处姿态稳定不会随机旋转Matlabclc; clear; close all; % 原始点云模拟一个斜平面 [X,Y] meshgrid(-1:0.2:1, -1:0.2:1); Z X Y; % 平面 z x y pts [X(:), Y(:), Z(:)]; % 给定点和法向 P [1,2,3]; n [1,1,1]; n n / norm(n); % 构造坐标系 z n; if abs(dot(z,[0 0 1])) 0.9 a [0 0 1]; else a [1 0 0]; end x cross(a, z); x x / norm(x); y cross(z, x); R [x(:), y(:), z(:)]; % 构造T T eye(4); T(1:3,1:3) R; T(1:3,4) P(:); % 变换点云展平 pts_h [pts, ones(size(pts,1),1)]; pts_trans inv(T) * pts_h; pts_trans pts_trans(1:3,:); % 可视化 figure; subplot(1,2,1); scatter3(pts(:,1), pts(:,2), pts(:,3), .); title(原始点云); axis equal; subplot(1,2,2); scatter3(pts_trans(:,1), pts_trans(:,2), pts_trans(:,3), .); title(变换后平面被拉平); axis equal;使用平面矫正法向 → 构造坐标系 → 变换点云 → 展平抓取姿态机器人z轴 法向接触方向x轴 抓取方向点云对齐初始化局部坐标系 → ICP 初值四、案例假设你有一个平面上的点P (1, 2, 3) 法向量n (0.577, 0.577, 0.577) // ≈ (1,1,1) 归一化 目标 构造一个坐标系 z轴 法向 并把这个平面“摆正”对齐到XY平面1、构建正交基1、 z轴 z n (0.577, 0.577, 0.577) 2、选辅助向量 a (0, 0, 1)不共线 3、 x轴 叉乘 x a × z (0,0,1) × (0.577,0.577,0.577) (-0.577, 0.577, 0) 归一化 x ≈ (-0.707, 0.707, 0) 4、 y轴 y z × x 计算 y ≈ (-0.408, -0.408, 0.816) 5、 旋转矩阵 R [ -0.707 -0.408 0.577 0.707 -0.408 0.577 0 0.816 0.577 ] 列向量x y z2、构造变换矩阵T [ R P ] [ 0 1 ] T [ -0.707 -0.408 0.577 1 0.707 -0.408 0.577 2 0 0.816 0.577 3 0 0 0 1 ]
点+法向量 计算旋转平移矩阵
这个是我之前做的一个项目现在总结一下一、问题本质已知一个点P (x, y, z)一个法向量n (nx, ny, nz)求刚体变换矩阵 T4×4 [ R t ] [ 0 1 ] 一个 X YZ 法向量 如何转换成一个旋转平移矩阵二、核心思想约束z轴 法向量x/y轴 在平面上的两个正交方向这就是一个 SO(3) 构造问题其中z 法向x 任意垂直于 z 的方向y 保证右手系本质从一个方向 → 补全成一个正交坐标系ẑ n / ||n||三、PCL使用和MatlabPCL#include Eigen/Dense Eigen::Matrix4f poseFromPointNormal( const Eigen::Vector3f point, const Eigen::Vector3f normal) { Eigen::Vector3f z normal.normalized(); // 选择辅助向量避免共线 Eigen::Vector3f aux(0, 0, 1); if (fabs(z.dot(aux)) 0.9) aux Eigen::Vector3f(1, 0, 0); // 构造正交基 Eigen::Vector3f x aux.cross(z).normalized(); Eigen::Vector3f y z.cross(x); // 旋转矩阵 Eigen::Matrix3f R; R.col(0) x; R.col(1) y; R.col(2) z; // 变换矩阵 Eigen::Matrix4f T Eigen::Matrix4f::Identity(); T.block3,3(0,0) R; T.block3,1(0,3) point; return T; }使用pcl::PointNormal pt; Eigen::Vector3f P(pt.x, pt.y, pt.z); Eigen::Vector3f n(pt.normal_x, pt.normal_y, pt.normal_z); Eigen::Matrix4f T poseFromPointNormal(P, n);注意点1、法向量方向不稳定n 和 -n 都是合法法向会导致姿态翻转抓取方向反了解决if (n.dot(view_direction) 0) n -n;注意点12、自由度缺失核心理解一个法向量只确定2个自由度还缺绕法向旋转你现在的 x 轴是“人为选的”注意点13、工业更稳定的写法 ⭐如果你有参考方向推荐比如重力方向 g (0,0,1)更优写法Eigen::Vector3f z normal.normalized(); Eigen::Vector3f x g.cross(z).normalized(); Eigen::Vector3f y z.cross(x);好处姿态稳定不会随机旋转Matlabclc; clear; close all; % 原始点云模拟一个斜平面 [X,Y] meshgrid(-1:0.2:1, -1:0.2:1); Z X Y; % 平面 z x y pts [X(:), Y(:), Z(:)]; % 给定点和法向 P [1,2,3]; n [1,1,1]; n n / norm(n); % 构造坐标系 z n; if abs(dot(z,[0 0 1])) 0.9 a [0 0 1]; else a [1 0 0]; end x cross(a, z); x x / norm(x); y cross(z, x); R [x(:), y(:), z(:)]; % 构造T T eye(4); T(1:3,1:3) R; T(1:3,4) P(:); % 变换点云展平 pts_h [pts, ones(size(pts,1),1)]; pts_trans inv(T) * pts_h; pts_trans pts_trans(1:3,:); % 可视化 figure; subplot(1,2,1); scatter3(pts(:,1), pts(:,2), pts(:,3), .); title(原始点云); axis equal; subplot(1,2,2); scatter3(pts_trans(:,1), pts_trans(:,2), pts_trans(:,3), .); title(变换后平面被拉平); axis equal;使用平面矫正法向 → 构造坐标系 → 变换点云 → 展平抓取姿态机器人z轴 法向接触方向x轴 抓取方向点云对齐初始化局部坐标系 → ICP 初值四、案例假设你有一个平面上的点P (1, 2, 3) 法向量n (0.577, 0.577, 0.577) // ≈ (1,1,1) 归一化 目标 构造一个坐标系 z轴 法向 并把这个平面“摆正”对齐到XY平面1、构建正交基1、 z轴 z n (0.577, 0.577, 0.577) 2、选辅助向量 a (0, 0, 1)不共线 3、 x轴 叉乘 x a × z (0,0,1) × (0.577,0.577,0.577) (-0.577, 0.577, 0) 归一化 x ≈ (-0.707, 0.707, 0) 4、 y轴 y z × x 计算 y ≈ (-0.408, -0.408, 0.816) 5、 旋转矩阵 R [ -0.707 -0.408 0.577 0.707 -0.408 0.577 0 0.816 0.577 ] 列向量x y z2、构造变换矩阵T [ R P ] [ 0 1 ] T [ -0.707 -0.408 0.577 1 0.707 -0.408 0.577 2 0 0.816 0.577 3 0 0 0 1 ]
