2. 光学定位原理：立体视觉、标定、标记点与坐标系
各位同学，今天我们来聊聊光学定位。说白了，就是让机器“看见”手术器械在哪儿。
我刚开始接触这个领域时，觉得不就是两个摄像头拍个照嘛，能有多难？后来真上手做项目才发现，这里面的坑，一个比一个深。嗯，咱们一个一个来拆解。
2.1 立体视觉原理：为什么两只眼睛比一只强？
你想想看，如果你闭上一只眼睛，去拿桌上的水杯，是不是容易抓偏？
立体视觉的核心，就是利用两个相机之间的“视差”，来推算物体的深度信息。说白了，就是三角测量。
核心公式（简化版）：
深度 Z = (f * B) / d
其中：f 是焦距，B 是基线距离（两个相机光心之间的距离），d 是视差（同一个点在左右图像中的像素差）。
我在项目中遇到过一个问题：基线距离B设得太大，近处物体倒是看得清楚，但远处物体视差太小，深度误差大得离谱。后来我学乖了，基线距离要根据手术操作空间来定。腹腔镜手术，操作深度一般在5-20cm，基线取3-5cm比较合适。
我的经验： 别迷信高分辨率相机。在立体视觉里，同步性比分辨率更重要。两个相机如果曝光时间差了几毫秒，器械一动，视差就全乱了。我吃过这个亏，后来一律用硬件触发同步。
2.2 双目相机标定：给相机“配眼镜”
相机标定，说白了就是找出相机的“内参”和“外参”。
- 内参： 焦距、主点坐标、畸变系数。这是相机自己的“脾气”。
- 外参： 两个相机之间的旋转矩阵R和平移向量T。这是两个相机之间的“关系”。
标定方法，最经典的就是张正友标定法。用棋盘格，拍个十几二十张不同角度的照片，然后让算法去算。
注意： 标定板一定要平整！我曾经用一张打印在A4纸上的棋盘格，贴在硬纸板上，结果纸板有点弯曲。标定出来的畸变参数全是错的，追踪精度直接崩了。后来换了陶瓷标定板，问题才解决。
标定流程，我习惯这样做：
- 固定好双目相机，确保两个相机相对位置不动。
- 手持标定板，在相机视野内变换姿态（倾斜、旋转、远近）。
- 采集20-30对图像，确保棋盘格覆盖整个视野。
- 运行标定算法，检查重投影误差，一般要小于0.5像素。
// 伪代码示例：双目标定核心步骤
// 1. 检测棋盘格角点
corners_left = findChessboardCorners(image_left, pattern_size);
corners_right = findChessboardCorners(image_right, pattern_size);
// 2. 单目标定（分别标定左右相机）
calibrateCamera(object_points, corners_left, image_size, K_left, dist_left);
calibrateCamera(object_points, corners_right, image_size, K_right, dist_right);
// 3. 双目标定（计算R和T）
stereoCalibrate(object_points, corners_left, corners_right,
K_left, dist_left, K_right, dist_right,
image_size, R, T);
2.3 光学标记点追踪：让机器“盯住”器械
手术器械上贴几个反光小球，或者主动发光的LED，相机捕捉到这些光点，然后算出它们的三维位置。这就是光学标记点追踪。
我建议，标记点至少用4个，而且不能共面。为什么？你想想看，如果4个点都在一个平面上，从某个角度看过去，它们可能“缩”成一条线，算法就分不清谁是谁了。
标记点设计原则：
- 非对称分布：避免旋转对称导致的歧义
- 最小间距：标记点之间至少间隔2cm，否则容易混淆
- 红外波段：手术室灯光干扰大，用850nm或940nm的红外LED，配合带通滤光片，效果最好
追踪算法，我常用的是“最近邻匹配 + 迭代最近点（ICP）”。先根据上一帧的位置，预测当前帧标记点的大概位置，然后匹配，最后用ICP算刚体变换。
避坑指南： 我曾经遇到一个情况，标记点被手术器械遮挡了1个，结果追踪直接丢了。后来我加了“容错机制”：只要看到3个点，就能根据刚体约束，推算出第4个点的位置。这样遮挡时也能继续追踪。
2.4 坐标系转换基础：把“相机看到的”变成“医生需要的”
坐标系转换，说白了就是“搬家”。
内窥镜手术里，至少涉及这几个坐标系：
- 相机坐标系： 以左相机光心为原点
- 标记点坐标系： 以器械上的标记点组为基准
- 器械尖端坐标系： 以手术器械的末端执行器为原点
- 患者坐标系： 以患者解剖结构为基准（通常由术前CT/MRI定义）
转换关系，就是一连串的矩阵乘法：
// 从标记点坐标系到相机坐标系的转换
P_camera = R_marker_to_camera * P_marker + T_marker_to_camera
// 从相机坐标系到器械尖端坐标系的转换
P_tip = R_camera_to_tip * P_camera + T_camera_to_tip
// 最终，标记点坐标系下的一个点，转换到器械尖端坐标系
P_tip = R_camera_to_tip * (R_marker_to_camera * P_marker + T_marker_to_camera) + T_camera_to_tip
看着复杂，其实就是一个链式变换。我习惯用齐次坐标，把旋转和平移合并成一个4x4矩阵，这样写起来清爽多了。
重要提醒： 坐标系转换最容易出错的地方，就是“左乘”和“右乘”搞反了。我建议，每次做转换前，先在纸上画个坐标系关系图，标清楚每个变换的方向。别问我为什么知道——我调试过整整两天，最后发现是乘反了。
知识体系总览
下面这张图，是我自己总结的光学定位知识框架。你看一眼，心里就有谱了。
好了，这一章的内容就是这些。光学定位这块，理论不难，难的是工程实现中的各种细节。我上面提到的那些坑，都是真金白银换来的教训，你们记下来，以后能少走很多弯路。