像素坐标转世界坐标的计算

原理

下图表示了小孔成像模型(图片及公式参考opencv官方资料


这个图里涉及4个坐标系:

  1. 世界坐标系:其坐标原点可视情况而定,可以表示空间的物体,单位为长度单位,比如mm,用矩阵\begin{bmatrix}X_W \\ Y_W \\Z_W \end{bmatrix}表示;
  2. 相机坐标系:以摄像机光心为原点(在针孔模型中也就是针孔为中心),z轴与光轴重合,也就是z轴指向相机的前方(与成像平面垂直),x轴与y轴的正方向与世界坐标系平行,单位为长度单位,比如mm,用矩阵\begin{bmatrix}X_c \\ Y_c \\ Z_c\end{bmatrix}表示;
  3. 图像物理坐标系(也叫成像平面坐标系):用物理长度单位表示像素的位置,坐标原点为摄像机光轴与图像物理坐标系的交点位置。坐标系为图上o-xy,单位为长度单位,比如mm,用矩阵\begin{bmatrix}x \\ y \end{bmatrix}表示。
  4. 像素坐标系:坐标原点在左上角,以像素为单位,有明显的范围限制,即用于表示全画面的像素长和像素长宽,矩阵\begin{bmatrix}u \\ v \end{bmatrix}表示。

以下公式描述了\begin{bmatrix}u & v \end{bmatrix}^T\begin{bmatrix}x & y \end{bmatrix}^T\begin{bmatrix}X_c & Y_c & Z_c\end{bmatrix}^T\begin{bmatrix}X_W & Y_W & Z_W \end{bmatrix}^T之间的转换关系。
z\begin{bmatrix}u \\ v\\ 1 \end{bmatrix}= \begin{bmatrix}1/d_x&0&c_x\\0&1/d_y&c_y\\0&0&1 \end{bmatrix} \begin{bmatrix}f&0&0\\ 0&f&0\\ 0&0&1 \end{bmatrix} \begin{bmatrix}r11&r12&r13&t1\\ r21&r22&r23&t2\\ r31&r32&r33&t3 \end{bmatrix} \begin{bmatrix}X_W \\ Y_W \\Z_W \\ 1\end{bmatrix}
以上公式中,d_xd_y表示1个像素有多少长度,即用传感器的尺寸除以像素数量,比如2928.384umx2205.216um的传感的分辨率为2592x1944,每个像素的大小即约1.12um。
f表示焦距,在上图中根据相似三角形,P点和p点具有以下关系:
\frac{X_c}{x} = \frac{Y_c}{y} = \frac{Z_c}{f}
x=X_c/(\frac{Z_c}{f}) y=Y_c/(\frac{Z_c}{f}),可见:f越大,xy越大,Z_c越大,xy越小。
c_xc_y表示中心点在像素坐标系中的位置。
要求像素坐标系中某像素点对应在世界坐标系中的位置,需要知道相机的内参、外参,相机的内参可以通过标定获得,外参可以人为设定。
第一步,将像素坐标变换到相机坐标系:
z\begin{bmatrix}u \\ v\\ 1 \end{bmatrix} = \begin{bmatrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1 \end{bmatrix} \begin{bmatrix}x \\ y\\ 1 \end{bmatrix} = K\begin{bmatrix}x \\ y\\ 1 \end{bmatrix}
两边乘以K的逆后推导出:
\begin{bmatrix}x \\ y\\ z \end{bmatrix}=K^{-1} \begin{bmatrix}u \\ v\\ 1 \end{bmatrix}
第二步,从相机坐标系变换到世界坐标系:
\begin{bmatrix}X_c \\ Y_c\\ Z_c \end{bmatrix} = R \begin{bmatrix}X \\ Y\\ Z \end{bmatrix} + t
将方程乘以R^{-1},可以推导出:
\begin{bmatrix}X \\ Y\\ Z \end{bmatrix} = \begin{bmatrix}X_c \\ Y_c \\ Z_c \end{bmatrix}R^{-1} - t R^{-1}= z\begin{bmatrix}x\\ y\\ 1 \end{bmatrix}R^{-1} - t R^{-1}

代码

通过输入相机的内参,旋转向量,平移向量和像素坐标,可以通过以下函数求出对应的世界坐标点。
以下代码中需求注意要对平移向量取转置,将1x3矩阵变为3x1矩阵后,才能实现3x3矩阵和3x1矩阵的乘法运算。

void cameraToWorld(InputArray cameraMatrix, InputArray rV, InputArray tV, vector<Point2f> imgPoints, vector<Point3f> &worldPoints)
{
    Mat invK64, invK;
    invK64 = cameraMatrix.getMat().inv();
    invK64.convertTo(invK, CV_32F);
    Mat r, t, rMat;
    rV.getMat().convertTo(r, CV_32F);
    tV.getMat().convertTo(t, CV_32F);
    Rodrigues(r, rMat);

    //计算 invR * T
    Mat invR = rMat.inv();
    //cout << "invR\n" << invR << endl;
    //cout << "t\n" << t << t.t() << endl;
    Mat transPlaneToCam;
    if(t.size() == Size(1, 3)){
        transPlaneToCam = invR * t;//t.t();
    }
    else if(t.size() == Size(3, 1)){
        transPlaneToCam = invR * t.t();
    }
    else{
        return;
    }
    //cout << "transPlaneToCam\n" << transPlaneToCam << endl;

    int npoints = (int)imgPoints.size();
    //cout << "npoints\n" << npoints << endl;
    for (int j = 0; j < npoints; ++j){
        Mat coords(3, 1, CV_32F);
        Point3f pt;
        coords.at<float>(0, 0) = imgPoints[j].x;
        coords.at<float>(1, 0) = imgPoints[j].y;
        coords.at<float>(2, 0) = 1.0f;
        //[x,y,z] = invK * [u,v,1]
        Mat worldPtCam = invK * coords;
        //cout << "worldPtCam:" << worldPtCam << endl;
        //[x,y,1] * invR
        Mat worldPtPlane = invR * worldPtCam;
        //cout << "worldPtPlane:" << worldPtPlane << endl;
        //zc 
        float scale = transPlaneToCam.at<float>(2) / worldPtPlane.at<float>(2);
        //cout << "scale:" << scale << endl;
        Mat scale_worldPtPlane(3, 1, CV_32F);
        //scale_worldPtPlane.at<float>(0, 0) = worldPtPlane.at<float>(0, 0) * scale;
        //zc * [x,y,1] * invR
        scale_worldPtPlane = scale * worldPtPlane;
        //cout << "scale_worldPtPlane:" << scale_worldPtPlane << endl;
        //[X,Y,Z]=zc*[x,y,1]*invR - invR*T
        Mat worldPtPlaneReproject = scale_worldPtPlane - transPlaneToCam;
        //cout << "worldPtPlaneReproject:" << worldPtPlaneReproject << endl;
        pt.x = worldPtPlaneReproject.at<float>(0);
        pt.y = worldPtPlaneReproject.at<float>(1);
        //pt.z = worldPtPlaneReproject.at<float>(2);
        pt.z = 1.0f;
        worldPoints.push_back(pt);
    }
}

    def cameraToWorld(self, cameraMatrix, r, t, imgPoints):
        invK = np.asmatrix(cameraMatrix).I
        rMat = np.zeros((3, 3), dtype=np.float64)
        cv2.Rodrigues(r, rMat)
        #print('rMat=', rMat)
        #计算 invR * T
        invR =  np.asmatrix(rMat).I #3*3
        #print('invR=', invR)
        transPlaneToCam = np.dot(invR , np.asmatrix(t)) #3*3 dot 3*1 = 3*1
        #print('transPlaneToCam=', transPlaneToCam)
        worldpt = []   
        coords = np.zeros((3, 1), dtype=np.float64)

        for imgpt in imgPoints:
            coords[0][0] = imgpt[0][0]
            coords[1][0] = imgpt[0][1]
            coords[2][0] = 1.0
            worldPtCam = np.dot(invK , coords)  #3*3 dot 3*1 = 3*1
            #print('worldPtCam=', worldPtCam)
            #[x,y,1] * invR
            worldPtPlane = np.dot(invR , worldPtCam) #3*3 dot 3*1 = 3*1
            #print('worldPtPlane=', worldPtPlane)
            #zc 
            scale = transPlaneToCam[2][0] / worldPtPlane[2][0]
            #print("scale: ", scale)
            #zc * [x,y,1] * invR
            scale_worldPtPlane = np.multiply(scale , worldPtPlane)
            #print("scale_worldPtPlane: ", scale_worldPtPlane)
            #[X,Y,Z]=zc*[x,y,1]*invR - invR*T
            worldPtPlaneReproject = np.asmatrix(scale_worldPtPlane) - np.asmatrix(transPlaneToCam)  #3*1 dot 1*3 = 3*3
            #print("worldPtPlaneReproject: ", worldPtPlaneReproject)
            pt = np.zeros((3, 1), dtype=np.float64)
            pt[0][0] = worldPtPlaneReproject[0][0]
            pt[1][0] = worldPtPlaneReproject[1][0]
            pt[2][0] = 0
            worldpt.append(pt.T.tolist())
        #print('worldpt:',worldpt)
        return worldpt

验证

先使用projectPoints生成像素点:

Vec3f eulerAngles;//欧拉角
vector<Vec3d> translation_vectors;/* 每幅图像的平移向量 */
Mat rotationMatrix = eulerAnglesToRotationMatrix(eulerAngles);
*pR_matrix = rotationMatrix;
cvRodrigues2(pR_matrix, pnew_vec, 0);   //从旋转矩阵求旋转向量
Mat mat_tmp(pnew_vec->rows, pnew_vec->cols, pnew_vec->type, pnew_vec->data.fl);
cv::Mat distortion_coeffs1 = cv::Mat(1, 5, CV_32FC1, cv::Scalar::all(0)); /* 摄像机的5个畸变系数:k1,k2,p1,p2,k3 */
projectPoints(tempPointSet, mat_tmp, translation_vectors[i], intrinsic_matrix, distortion_coeffs1, image_points2);

使用以下欧拉角:

[0, 0, 0]//欧拉角度,表示平面和相机的角度
旋转向量:[0, 0, 0]

对应的平移向量,表示空间坐标原点相对在相平面原点偏移x=134mm,y=132mm,z=200mm。

原始:[134.0870803179094, 132.7580766544178, 200.3789038923399]

生成空间坐标点:

        Size board_size = Size(11,8);
        Size square_size = Size(30, 30);
        vector<Point3f> tempPointSet;
        for (int j = 0; j<board_size.height; j++)
        {
            for (int i = 0; i<board_size.width; i++)
            {
                /* 假设定标板放在世界坐标系中z=0的平面上 */
                Point3f tempPoint;
                tempPoint.x = i*square_size.height;
                tempPoint.y = j*square_size.width;
                tempPoint.z = 0;
                tempPointSet.push_back(tempPoint);
            }
        }

经projectPoints计算后对应的像素空间点是:

projectPoints(tempPointSet, mat_tmp, translation_vectors[i], intrinsic_matrix, distortion_coeffs1, image_points2);
cout << "原始空间点:\n" << image_points2 << endl;

[1194.8174, 1074.1355;
 1285.1735, 1074.1355;
 1375.5295, 1074.1355;
 1465.8856, 1074.1355;
 1556.2417, 1074.1355;
 1646.5978, 1074.1355;
 1736.9539, 1074.1355;
 1827.3099, 1074.1355;
 1917.666, 1074.1355;
 2008.0221, 1074.1355;
 2098.3782, 1074.1355;
 1194.8174, 1164.5713;
 1285.1735, 1164.5713;
 1375.5295, 1164.5713;
 1465.8856, 1164.5713;
 1556.2417, 1164.5713;
 1646.5978, 1164.5713;
 1736.9539, 1164.5713;
 1827.3099, 1164.5713;
 1917.666, 1164.5713;
 2008.0221, 1164.5713;
 2098.3782, 1164.5713;
 1194.8174, 1255.0072;
 1285.1735, 1255.0072;
 1375.5295, 1255.0072;
 1465.8856, 1255.0072;
 1556.2417, 1255.0072;
 1646.5978, 1255.0072;
 1736.9539, 1255.0072;
 1827.3099, 1255.0072;
 1917.666, 1255.0072;
 2008.0221, 1255.0072;
 2098.3782, 1255.0072;
 1194.8174, 1345.443;
 1285.1735, 1345.443;
 1375.5295, 1345.443;
 1465.8856, 1345.443;
 1556.2417, 1345.443;
 1646.5978, 1345.443;
 1736.9539, 1345.443;
 1827.3099, 1345.443;
 1917.666, 1345.443;
 2008.0221, 1345.443;
 2098.3782, 1345.443;
 1194.8174, 1435.8789;
 1285.1735, 1435.8789;
 1375.5295, 1435.8789;
 1465.8856, 1435.8789;
 1556.2417, 1435.8789;
 1646.5978, 1435.8789;
 1736.9539, 1435.8789;
 1827.3099, 1435.8789;
 1917.666, 1435.8789;
 2008.0221, 1435.8789;
 2098.3782, 1435.8789;
 1194.8174, 1526.3147;
 1285.1735, 1526.3147;
 1375.5295, 1526.3147;
 1465.8856, 1526.3147;
 1556.2417, 1526.3147;
 1646.5978, 1526.3147;
 1736.9539, 1526.3147;
 1827.3099, 1526.3147;
 1917.666, 1526.3147;
 2008.0221, 1526.3147;
 2098.3782, 1526.3147;
 1194.8174, 1616.7506;
 1285.1735, 1616.7506;
 1375.5295, 1616.7506;
 1465.8856, 1616.7506;
 1556.2417, 1616.7506;
 1646.5978, 1616.7506;
 1736.9539, 1616.7506;
 1827.3099, 1616.7506;
 1917.666, 1616.7506;
 2008.0221, 1616.7506;
 2098.3782, 1616.7506;
 1194.8174, 1707.1864;
 1285.1735, 1707.1864;
 1375.5295, 1707.1864;
 1465.8856, 1707.1864;
 1556.2417, 1707.1864;
 1646.5978, 1707.1864;
 1736.9539, 1707.1864;
 1827.3099, 1707.1864;
 1917.666, 1707.1864;
 2008.0221, 1707.1864;
 2098.3782, 1707.1864]

经函数求出的空间坐标点是:

vector<Point3f> worldPoint;
cameraToWorld(intrinsic_matrix, mat_tmp, translation_vec_tmp, image_points2, worldPoint);
cout << "计算空间点:\n" << worldPoint << endl;

[0, 0, 1;
 30, 0, 1;
 60, 0, 1;
 90.000015, 0, 1;
 120.00002, 0, 1;
 149.99995, 0, 1;
 179.99998, 0, 1;
 209.99998, 0, 1;
 239.99998, 0, 1;
 270, 0, 1;
 300, 0, 1;
 0, 29.999985, 1;
 30, 29.999985, 1;
 60, 29.999985, 1;
 90.000015, 29.999985, 1;
 120.00002, 29.999985, 1;
 149.99995, 29.999985, 1;
 179.99998, 29.999985, 1;
 209.99998, 29.999985, 1;
 239.99998, 29.999985, 1;
 270, 29.999985, 1;
 300, 29.999985, 1;
 0, 60.000015, 1;
 30, 60.000015, 1;
 60, 60.000015, 1;
 90.000015, 60.000015, 1;
 120.00002, 60.000015, 1;
 149.99995, 60.000015, 1;
 179.99998, 60.000015, 1;
 209.99998, 60.000015, 1;
 239.99998, 60.000015, 1;
 270, 60.000015, 1;
 300, 60.000015, 1;
 0, 89.999969, 1;
 30, 89.999969, 1;
 60, 89.999969, 1;
 90.000015, 89.999969, 1;
 120.00002, 89.999969, 1;
 149.99995, 89.999969, 1;
 179.99998, 89.999969, 1;
 209.99998, 89.999969, 1;
 239.99998, 89.999969, 1;
 270, 89.999969, 1;
 300, 89.999969, 1;
 0, 120.00002, 1;
 30, 120.00002, 1;
 60, 120.00002, 1;
 90.000015, 120.00002, 1;
 120.00002, 120.00002, 1;
 149.99995, 120.00002, 1;
 179.99998, 120.00002, 1;
 209.99998, 120.00002, 1;
 239.99998, 120.00002, 1;
 270, 120.00002, 1;
 300, 120.00002, 1;
 0, 149.99998, 1;
 30, 149.99998, 1;
 60, 149.99998, 1;
 90.000015, 149.99998, 1;
 120.00002, 149.99998, 1;
 149.99995, 149.99998, 1;
 179.99998, 149.99998, 1;
 209.99998, 149.99998, 1;
 239.99998, 149.99998, 1;
 270, 149.99998, 1;
 300, 149.99998, 1;
 0, 179.99998, 1;
 30, 179.99998, 1;
 60, 179.99998, 1;
 90.000015, 179.99998, 1;
 120.00002, 179.99998, 1;
 149.99995, 179.99998, 1;
 179.99998, 179.99998, 1;
 209.99998, 179.99998, 1;
 239.99998, 179.99998, 1;
 270, 179.99998, 1;
 300, 179.99998, 1;
 0, 209.99998, 1;
 30, 209.99998, 1;
 60, 209.99998, 1;
 90.000015, 209.99998, 1;
 120.00002, 209.99998, 1;
 149.99995, 209.99998, 1;
 179.99998, 209.99998, 1;
 209.99998, 209.99998, 1;
 239.99998, 209.99998, 1;
 270, 209.99998, 1;
 300, 209.99998, 1]

可以对比按11*8格和30mm/格所生成空间坐标点结果,基本一致。

最后编辑于
©著作权归作者所有,转载或内容合作请联系作者
  • 序言:七十年代末,一起剥皮案震惊了整个滨河市,随后出现的几起案子,更是在滨河造成了极大的恐慌,老刑警刘岩,带你破解...
    沈念sama阅读 141,621评论 1 298
  • 序言:滨河连续发生了三起死亡事件,死亡现场离奇诡异,居然都是意外死亡,警方通过查阅死者的电脑和手机,发现死者居然都...
    沈念sama阅读 60,780评论 1 254
  • 文/潘晓璐 我一进店门,熙熙楼的掌柜王于贵愁眉苦脸地迎上来,“玉大人,你说我怎么就摊上这事。” “怎么了?”我有些...
    开封第一讲书人阅读 93,376评论 0 211
  • 文/不坏的土叔 我叫张陵,是天一观的道长。 经常有香客问我,道长,这世上最难降的妖魔是什么? 我笑而不...
    开封第一讲书人阅读 40,770评论 0 174
  • 正文 为了忘掉前任,我火速办了婚礼,结果婚礼上,老公的妹妹穿的比我还像新娘。我一直安慰自己,他们只是感情好,可当我...
    茶点故事阅读 48,482评论 1 252
  • 文/花漫 我一把揭开白布。 她就那样静静地躺着,像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上,一...
    开封第一讲书人阅读 38,637评论 1 172
  • 那天,我揣着相机与录音,去河边找鬼。 笑死,一个胖子当着我的面吹牛,可吹牛的内容都是我干的。 我是一名探鬼主播,决...
    沈念sama阅读 30,297评论 2 267
  • 文/苍兰香墨 我猛地睁开眼,长吁一口气:“原来是场噩梦啊……” “哼!你这毒妇竟也来了?” 一声冷哼从身侧响起,我...
    开封第一讲书人阅读 29,094评论 0 165
  • 想象着我的养父在大火中拼命挣扎,窒息,最后皮肤化为焦炭。我心中就已经是抑制不住地欢快,这就叫做以其人之道,还治其人...
    爱写小说的胖达阅读 28,855评论 6 228
  • 序言:老挝万荣一对情侣失踪,失踪者是张志新(化名)和其女友刘颖,没想到半个月后,有当地人在树林里发现了一具尸体,经...
    沈念sama阅读 32,420评论 0 213
  • 正文 独居荒郊野岭守林人离奇死亡,尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 29,200评论 2 213
  • 正文 我和宋清朗相恋三年,在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 30,521评论 1 224
  • 白月光回国,霸总把我这个替身辞退。还一脸阴沉的警告我。[不要出现在思思面前, 不然我有一百种方法让你生不如死。]我...
    爱写小说的胖达阅读 24,181评论 0 31
  • 序言:一个原本活蹦乱跳的男人离奇死亡,死状恐怖,灵堂内的尸体忽然破棺而出,到底是诈尸还是另有隐情,我是刑警宁泽,带...
    沈念sama阅读 27,065评论 2 213
  • 正文 年R本政府宣布,位于F岛的核电站,受9级特大地震影响,放射性物质发生泄漏。R本人自食恶果不足惜,却给世界环境...
    茶点故事阅读 31,424评论 3 202
  • 文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹,春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 25,590评论 0 8
  • 文/苍兰香墨 我抬头看了看天上的太阳。三九已至,却和暖如春,着一层夹袄步出监牢的瞬间,已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 25,947评论 0 163
  • 我被黑心中介骗来泰国打工, 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留,地道东北人。 一个月前我还...
    沈念sama阅读 33,406评论 2 230
  • 正文 我出身青楼,却偏偏与公主长得像,于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子,可洞房花烛夜当晚...
    茶点故事阅读 33,509评论 2 229

推荐阅读更多精彩内容

  • 如果不熟悉线性代数的概念,要去学习自然科学,现在看来就和文盲差不多。”,然而“按照现行的国际标准,线性代数是通过公...
    Drafei阅读 1,452评论 0 3
  • 前言 最近翻阅关于从2D视频或者图片中重构3D姿态的文章及其源码,发现都有关于摄像机参数的求解,查找了相关资料,做...
    予汐阅读 5,776评论 0 3
  • 最原始出处:http://blog.csdn.net/myan/article/details/647511 (C...
    IIGEOywq阅读 3,793评论 2 62
  • 诗在生活里 在不经意间流露 今天 你是我的诗人 你说 我想 我们可以过平凡的日子 工作不用太累 钱不用太多 够花就...
    小城蜉蝣阅读 246评论 1 3
  • 对于职场人士而言,每天上班已经累成狗了,下班只想好好休息。虽然知道阅读的重要性,却根本没有时间来看书。有时候...
    a1a88c3a5c84阅读 287评论 0 3