原理
下图表示了小孔成像模型(图片及公式参考opencv官方资料)
这个图里涉及4个坐标系:
世界坐标系:其坐标原点可视情况而定,可以表示空间的物体,单位为长度单位,比如mm,用矩阵
表示;
相机坐标系:以摄像机光心为原点(在针孔模型中也就是针孔为中心),z轴与光轴重合,也就是z轴指向相机的前方(与成像平面垂直),x轴与y轴的正方向与世界坐标系平行,单位为长度单位,比如mm,用矩阵
表示;
图像物理坐标系(也叫成像平面坐标系):用物理长度单位表示像素的位置,坐标原点为摄像机光轴与图像物理坐标系的交点位置。坐标系为图上o-xy,单位为长度单位,比如mm,用矩阵
表示。
像素坐标系:坐标原点在左上角,以像素为单位,有明显的范围限制,即用于表示全画面的像素长和像素长宽,矩阵
表示。
以下公式描述了
、
、
和
之间的转换关系。
以上公式中,
和
表示1个像素有多少长度,即用传感器的尺寸除以像素数量,比如2928.384umx2205.216um的传感的分辨率为2592x1944,每个像素的大小即约1.12um。
表示焦距,在上图中根据相似三角形,P点和p点具有以下关系:
即
,可见:
越大,
和
越大,
越大,
和
越小。
和
表示中心点在像素坐标系中的位置。
要求像素坐标系中某像素点对应在世界坐标系中的位置,需要知道相机的内参、外参,相机的内参可以通过标定获得,外参可以人为设定。
第一步,将像素坐标变换到相机坐标系:
两边乘以K的逆后推导出:
第二步,从相机坐标系变换到世界坐标系:
将方程乘以
,可以推导出:
代码
通过输入相机的内参,旋转向量,平移向量和像素坐标,可以通过以下函数求出对应的世界坐标点。
以下代码中需求注意要对平移向量取转置,将1x3矩阵变为3x1矩阵后,才能实现3x3矩阵和3x1矩阵的乘法运算。
void cameraToWorld(InputArray cameraMatrix, InputArray rV, InputArray tV, vector imgPoints, vector &worldPoints)
{
Mat invK64, invK;
invK64 = cameraMatrix.getMat().inv();
invK64.convertTo(invK, CV