计算原理

角度计算

1. 相机CCD大小固定，即相机成像平面大小固定，相机视场角(FOV)仅由相机焦距F决定；

   • 因此，定焦相机的FOV大小固定，通过上图可以看出相机视场角的计算公式为：
   FOV = 2*atan（w/2f）
   • 注意，这里w/2f代表的是w/2除以f；

2. 因此，要想获取图像上任意点(x0,x0)到图像中心的旋转角度，只需求出任意点在成像画面中与图像中心(x1,x2)在x、y方向上的距离差作为新的w即可，但需注意x轴方向的差对应的是y轴的旋转角度，其余类似，如下式：

α_y=2*atan（（x_1-x_0）/2f_x） α_x=2*atan（（y_1-y_0）/2f_x）

机械臂位姿计算

1. 已知两个旋转角度，将其转化为旋转矩阵R_x、R_y;
   
2. 左乘机械臂当前位姿矩阵，即可获取机械臂到达中心点的机械臂位姿即：

targetPose = currPose * R_x * R_y