对应点齐了，相机位姿还是不对？用 Kornia 把 PnP 算对的最小路径

对应点齐了，相机位姿还是不对？用 Kornia 把 PnP 算对的最小路径

【免费下载链接】kornia🐍 空间人工智能的几何计算机视觉库项目地址: https://gitcode.com/kornia/kornia

3D 重建对不上、相机放偏了——你有 2D-3D 对应，差的是那把位姿。这篇文章给一条能跑通、能验证的 PnP 求解路径，外加四个最容易翻车的点。Kornia 是 PyTorch 上的几何视觉库，这里用它可微的相机模型和 PnP 求解器把"从对应点推位姿"这件事做扎实。

先搞懂两件事

针孔投影：把 3D 压扁成 2D 的那一步。类比：手机拍照时，镜头把房间里的每一点按"远近"压到感光元件上，近的大、远的小。压扁由两个参数控制：焦距 f（压多狠）和主点 c（画面中心偏哪）。

方向问题：矩阵指向哪边。PnP 求出来的不是"相机在哪"，而是"世界点怎么进相机坐标"——世界到相机的变换。方向拿反，位姿一定反。记住这一句，后文最后一个坑就是它。

最小可跑流程

四步：内参、对应点、求解、验证。每步只留核心调用。

把内参填进矩阵

内参 K 是 3×3，决定"像素坐标和相机坐标怎么换算"：

import torch # 800x600 画面，主点放像素中心，f 取图像长边量级 K = torch.tensor([[600.0, 0.0, 399.5], [0.0, 600.0, 299.5], [0.0, 0.0, 1.0]], dtype=torch.float64).unsqueeze(0)

unsqueeze加 batch 维不是装饰——库的接口全是批量的，形状不对直接拒收。

造出带 batch 维的 2D-3D 对应

模拟"先有真值、再被检测出带噪声的像素点"这个真实过程：

world = torch.tensor([[[0.0, 0.0, 2.0], [1.0, 0.0, 2.0], [0.0, 1.0, 2.0], [1.0, 1.0, 2.0], [0.5, 0.5, 3.0], [0.2, 0.8, 2.5], [0.0, 1.5, 2.2], [1.2, 0.3, 2.8]]], dtype=torch.float64) R_t = torch.tensor([[[1, 0, 0, -0.3], [0, 1, 0, 0.1], [0, 0, 1, 1.2]]], dtype=torch.float64) pts_cam = (R_t[..., :3, :3] @ world.transpose(1, 2) + R_t[..., :3, 3:4]).transpose(1, 2) from kornia.geometry.camera import project_points # 2 像素噪声，模拟真实检测水平 img = project_points(pts_cam, K[0]) + torch.randn(img_shape, dtype=torch.float64) * 2.0

注意world的 8 个点故意不共面：DLT 在点共面时会直接报错，后面说为什么。

让 DLT 把 [R|t] 交出来

求解器入口：kornia/geometry/calibration/pnp.py（solve_pnp_dlt，DLT 法 PnP）：

from kornia.geometry.calibration import solve_pnp_dlt # 最少 6 点，返回 (B,3,4) 的世界到相机矩阵 W2C = solve_pnp_dlt(world, img, K)

预期输出：torch.Size([1, 3, 4])，前 3 列是旋转，第 4 列是平移。

用重投影把答案验一遍

别信解出来的数，信重投影误差：

pts_cam2 = (W2C[..., :3, :3] @ world.transpose(1, 2) + W2C[..., :3, 3:4]).transpose(1, 2) reproj = project_points(pts_cam2, K[0]) err = (reproj - img).norm(dim=-1).mean() print(f"平均重投影误差: {err:.2f} px")

噪声是 2px，误差落在 1~2px 就是对的，链路通。

求解涉及的所有量长这样，调参前先对表：

为什么这么算

上面能跑通，但你得知道它在干什么。DLT 把每对对应点写成两行线性方程，12 个未知数一起列成一个大矩阵，用 SVD 找最小奇异值方向，拿到一个"近似旋转平移"的解。点数少时它不挑、不分支，一次 SVD 出结果；EPnP 那类 4 点最小解法要构造辅助四面体，点少时更省，点多时差距拉不开。

求解内部还做了两件保命的事：两路点各自中心化缩放（防大数压小数），QR 分解把旋转部分扭成正交。代价是——点共面时方程组秩亏，SVD 最小奇异值贴零，函数直接抛错。

这些坑我替你踩了

• 传了 4×4 内参，形状报错。大概率你把PinholeCamera.camera_matrix整个塞进去了，求解器只认 3×3。改：K[:3, :3]切片再传。
• 一跑就抛 singular value 的 AssertionError。3D 点共面或共线，方程组解不唯一。改：加点让深度散开——本例的world第 7、8 个点就是干这个用的。
• 只凑出 5 个对应点。DLT 需要 ≥6，少一个就报错。改：补点，或者换支持 4 点的最小解法。
• 位姿方向是反的，物体跑到相机后面。返回值是世界到相机：拿它"把相机摆到世界里"自然反。改：求相机到世界就取逆，torch.linalg.inv(W2C)（补成 4×4 后）。

再往远处看

• 鲁棒化：真实匹配有外点，可结合kornia/geometry/ransac.py（RANSAC 估计框架）做采样一致性筛选
• 学习融合：整条链路在 PyTorch 图里，位姿可参与反向传播，做端到端标定或位姿网络

git clone https://gitcode.com/kornia/kornia

想读源码，从kornia/geometry/calibration/pnp.py开始，294 行，DLT 的每一步注释都在。

【免费下载链接】kornia🐍 空间人工智能的几何计算机视觉库项目地址: https://gitcode.com/kornia/kornia