基于多摄像头二维信息的三维定位
2026-08-31 21:35:30 2
二维图像只能告诉我们目标位于哪条视线上，真正的深度信息来自多视角之间的几何关系。
这次实践使用一台云台相机，在三个固定预置位分别拍摄目标，通过相机标定、像素反投影和多视角射线交会，已经成功跑通了从二维观测到三维世界坐标输出的完整链路。
这里的“跑通”指算法流程和数据链路已经闭环；当前标定误差还需要继续优化，才能进入正式精度验收。
一、先把世界坐标系定义清楚
现场空间实际尺寸为：
长度：1980 mm，定义为世界 X 方向
宽度：1200 mm，定义为世界 Y 方向
高度：800 mm，定义为世界 Z 方向三个拍摄位置的相机光心分别为：
位置 1：[0, 50, 800] mm
位置 2：[0, 1150, 800] mm
位置 3：[1980, 50, 800] mm标定棋盘左上第一个内角点在世界坐标中的位置分别为：(每个位置对应的标定板的位置不一样)
位置 1：[853, 50, 0] mm
位置 2：[780, 770, 0] mm
位置 3：[825, 60, 0] mm目标纸巾的人工测量中心为：
[930, 524, 30] mm所有计算内部统一使用米，最终同时输出米和毫米，避免在矩阵运算中混用单位。
下面三张图片的纸巾位置都是一样的, 只是云台拍摄角度不一样
注意
1. 10个格子的是Y轴, 7个格子的是X轴, 标定板放在地面上, 平行于Z轴. 相机更换了位置, 标定板的位置可以变动, 方向不用变动
2. 最靠近原点的是第一个内角点
二、二维检测点不等于物体三维中心
三角测量恢复的是“同一个物理点”，而不是整个检测框。
如果检测框格式为：
[xmin, ymin, xmax, ymax]框中心可以写成：
u = (xmin + xmax) / 2.0
v = (ymin + ymax) / 2.0对于放在地面上的目标，更合理的参考点通常是底部中心：
u = (xmin + xmax) / 2.0
v = ymax这次测量使用的是检测框中心，因此结果应理解为“目标视觉中心”的近似值。不同视角下的框中心不一定对应物体上的同一个物理点，这也是普通目标定位精度受限的原因之一。
三、先标定相机自身的成像模型
相机输出图像分辨率为：
2304 x 1296使用 9 x 6 个棋盘内角点, 实际是10 x 7个格子，方格实际边长为：
24 mm通过 33 张标定图像计算得到内参矩阵：
K = np.array([
[1320.1717, 0.0, 1187.5307],
[ 0.0, 1319.2484, 708.4240],
[ 0.0, 0.0, 1.0]
])畸变参数为：
dist_coeffs = np.array([
-0.1889467,
-0.1578081,
-0.0055605,
-0.0019354,
0.1654711
])标定整体重投影 RMS 为：
0.7653 px内参标定的核心过程是：
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
ok, corners = cv2.findChessboardCorners(
gray,
(9, 6)
)
corners = cv2.cornerSubPix(
gray,
corners,
(11, 11),
(-1, -1),
criteria
)
rms, K, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(
object_points,
image_points,
image_size,
None,
None
)这一阶段只回答一个问题：
相机如何把三维世界投影成二维图像？
它还不能告诉我们相机在房间中的位置。
四、通过棋盘得到相机外参
外参描述相机相对于世界坐标系的位置和方向。
棋盘角点在相机中的位姿可以通过 PnP 求解：
ok, rvec, tvec = cv2.solvePnP(
object_points_board,
image_points_camera,
K,
dist_coeffs,
flags=cv2.SOLVEPNP_ITERATIVE
)solvePnP 得到的是：
P_camera = T_camera_board * P_board而三维定位需要的是相机坐标到世界坐标的变换：
P_world = T_world_camera * P_camera因此需要先构造：
R_camera_board, _ = cv2.Rodrigues(rvec)
T_camera_board = np.eye(4)
T_camera_board[:3, :3] = R_camera_board
T_camera_board[:3, 3] = tvec.reshape(3)
T_world_camera = (
T_world_board @ np.linalg.inv(T_camera_board)
)这里最容易出错的是矩阵方向。T_camera_board 和 T_world_camera 不能混用，否则最终得到的坐标可能数值看似合理，但物理位置完全错误。
五、二维像素如何变成空间射线
对于目标像素 (u, v)，第一步是去除镜头畸变：
point = np.array([[[u, v]]], dtype=np.float64)
normalized = cv2.undistortPoints(
point,
K,
dist_coeffs
)[0, 0]得到归一化相机坐标后，构造相机坐标系中的方向：
ray_camera = np.array([
normalized[0],
normalized[1],
1.0
])
ray_camera /= np.linalg.norm(ray_camera)再通过外参旋转到世界坐标系：
R_world_camera = T_world_camera[:3, :3]
camera_center_world = T_world_camera[:3, 3]
ray_world = R_world_camera @ ray_camera
ray_world /= np.linalg.norm(ray_world)这样，一个二维点就被转换成了世界坐标系中的射线：
L(lambda) = C + lambda * d其中：
● C 是相机光心；
● d 是世界坐标中的射线方向；
● lambda > 0 表示目标位于相机前方。
单张普通 RGB 图像只能得到一条射线，所以无法独立恢复任意目标的三维坐标
六、两条或多条射线交会得到三维点
对于每条射线，使用正交投影矩阵：
projector = (
np.eye(3) -
np.outer(direction, direction)
)
A += projector
b += projector @ origin最后通过最小二乘求解：
point_world, _, _, _ = np.linalg.lstsq(
A,
b,
rcond=None
)这实际上是在求一个三维点，使它到所有观测射线的正交距离之和最小：
P* = argmin Σ ||(I - dᵢdᵢᵀ)(P - Cᵢ)||²理想情况下，多条射线应该交于同一点。现实中由于像素误差、标定误差和目标框语义误差，它们通常不会完全相交，因此需要输出射线间距作为质量指标。
七、定位结果
以人工测量中心 [930, 524, 30] mm 作为参考，三视角计算得到：
三视角结果：
[931.75, 548.68, 52.52] mm与人工测量值的空间距离约为：
33.5 mm使用两个视角进行复核，得到：
双视角结果：
[940.67, 565.81, 39.86] mm与人工测量值的空间距离约为：
44.3 mm从结果上看，X 方向已经比较接近，主要误差集中在 Y 和 Z 方向。这说明从二维像素、相机标定到三维交会的计算链路已经能够工作，当前精度瓶颈主要来自外参质量和二维参考点定义。
