点+法向量 计算旋转平移矩阵

这个是我之前做的一个项目，现在总结一下

一、问题本质

已知：

一个点：P = (x, y, z)

一个法向量：n = (nx, ny, nz)

求：

刚体变换矩阵 T（4×4）：
[ R  t ]
[ 0  1 ]
一个 X YZ +法向量 如何转换成一个旋转平移矩阵

二、核心思想

约束：

• z轴 = 法向量
• x/y轴 = 在平面上的两个正交方向

这就是一个 SO(3) 构造问题

其中：

• z = 法向
• x = 任意垂直于 z 的方向
• y = 保证右手系

本质：从一个方向 → 补全成一个正交坐标系

ẑ = n / ||n||

三、PCL使用和Matlab

PCL

#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.block<3,3>(0,0) = R;
    T.block<3,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);

好处：

姿态稳定（不会随机旋转）

Matlab

clc; 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 z）

2、构造变换矩阵

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 ]