双目视觉三角测量C语言代码实现不使用OpenCV
在双目视觉系统中，三角测量是一种基于几何原理的三维重建技术，通过分析同一场景在两个不同视角下的二维图像来确定物体的三维坐标。这种方法的核心在于利用摄像机的内参和相对位姿（由旋转矩阵和平移向量描述），将图像中的像素坐标转换为三维空间中的射线，并通过求解这些射线的交点来确定物体的空间位置。
代码方面，利用相机内参和对极几何关系，从二维像素坐标(u, v)解算出三维世界坐标(X, Y, Z)。通过相机内参矩阵，二维图像点被转换为相机坐标系下的射线。算法通过求解两条射线的交点来确定对应于两幅图像上同一场景点的三维位置。在存在噪声和误差的情况下，通过最小二乘法从可能的解中选择最合适的三维坐标。
三角测量算法的运算过程依据如下步骤：
首先，左相机的投影矩阵Pl为
Pl = Kl·[I|0]
右相机的投影矩阵Pr为
Pr = Kr·[R|T]
Kl和Kr分别是左右相机的内参矩阵，[I|0]表示左相机在世界坐标系中的位置，这里以左相机为参考坐标系，R是右相机相对于左相机的旋转矩阵，T是右相机相对于左相机的平移向量。
在图像中，二维像素坐标可以通过归一化得到左右相机图像坐标pl=(ul, vl)和pr=(ur, vr)。三维点X=(X, Y, Z, 1)T投影到左右相机上的坐标pl和pr满足:
pl = Pl·X = Kl·[I|0]·X
pr = Pr·X = Kr·[R|T]·X
根据左右相机的投影矩阵Pl、Pr和对应的坐标点pl、pr，可以建立如下线性方程。
(ul·Pl[2,:] – Pl[0,:])·X = 0