SLAM实战解析：从坐标系转换到PnP算法的工程实现

1. SLAM中的坐标系转换基础

第一次接触SLAM时，我被各种坐标系绕得头晕眼花——世界坐标系、相机坐标系、图像坐标系、像素坐标系，它们之间到底怎么转换？后来在机器人导航项目中踩过几次坑才明白，坐标系转换就像在不同语言国家间问路，必须建立统一的"翻译规则"。

世界坐标系相当于地球的经纬度系统，是一个固定不变的参考系。比如我们要构建商场导航地图，可以把商场正门中心点设为世界坐标系原点。当扫地机器人在商场移动时，它的位置(Xw,Yw,Zw)就是相对于这个原点的坐标。

相机坐标系则是以相机镜头为中心的局部坐标系。就像你拿着手机拍照时，手机镜头的位置和角度决定了它看到的画面。这个世界到相机的转换需要两个关键参数：旋转矩阵R（决定相机朝向）和平移向量T（决定相机位置）。用数学表示就是P_c = R·P_w + T，其中P_w是世界坐标，P_c是相机坐标。

图像坐标系已经进入了2D领域。通过相机内参矩阵K，我们可以将3D相机坐标投影到2D成像平面。这个步骤就像把立体的景物"压扁"到照片上。相机内参包括焦距f、主点坐标(cx,cy)等，通常通过相机标定获得。

像素坐标系则是我们最终看到的数字图像。这里有个容易混淆的概念：图像坐标的单位是毫米等物理单位，而像素坐标的单位是像素。转换时需要知道每个像素的物理尺寸（例如0.1mm/pixel）。假设图像坐标系中点坐标是(x,y)，对应的像素坐标(u,v)计算公式为：

u = x/dx + cx
v = y/dy + cy 

其中dx,dy表示每个像素的物理尺寸。

实际工程中，我常用OpenCV的projectPoints函数一次性完成这些转换。但理解背后的数学原理非常重要，当出现定位偏差时，能快速判断是坐标系转换的哪个环节出了问题。

2. PnP算法原理深度解析

在视觉SLAM项目中，我们经常遇到这样的场景：已知地图中某些