像素坐標(biāo)轉(zhuǎn)世界坐標(biāo)的計算

原理

下圖表示了小孔成像模型（圖片及公式參考opencv官方資料）

這個圖里涉及4個坐標(biāo)系：

世界坐標(biāo)系：其坐標(biāo)原點可視情況而定谚咬，可以表示空間的物體萝快，單位為長度單位川蒙，比如mm，用矩陣 $\begin{bmatrix}X_W \\ Y_W \\Z_W \end{bmatrix}$ 表示蹈矮；
相機坐標(biāo)系：以攝像機光心為原點（在針孔模型中也就是針孔為中心）砰逻，z軸與光軸重合，也就是z軸指向相機的前方（與成像平面垂直）泛鸟，x軸與y軸的正方向與世界坐標(biāo)系平行蝠咆，單位為長度單位，比如mm北滥，用矩陣 $\begin{bmatrix}X_c \\ Y_c \\ Z_c\end{bmatrix}$ 表示刚操；
圖像物理坐標(biāo)系（也叫成像平面坐標(biāo)系）：用物理長度單位表示像素的位置，坐標(biāo)原點為攝像機光軸與圖像物理坐標(biāo)系的交點位置再芋。坐標(biāo)系為圖上o-xy菊霜，單位為長度單位，比如mm济赎，用矩陣 $\begin{bmatrix}x \\ y \end{bmatrix}$ 表示鉴逞。
像素坐標(biāo)系：坐標(biāo)原點在左上角，以像素為單位司训，有明顯的范圍限制构捡，即用于表示全畫面的像素長和像素長寬，矩陣 $\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$ 之間的轉(zhuǎn)換關(guān)系蓖谢。
$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_x$ 和 $d_y$ 表示1個像素有多少長度，即用傳感器的尺寸除以像素數(shù)量闪幽，比如2928.384umx2205.216um的傳感的分辨率為2592x1944啥辨，每個像素的大小即約1.12um。
$f$ 表示焦距盯腌，在上圖中根據(jù)相似三角形溉知，P點和p點具有以下關(guān)系：
$\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$ 越大腕够， $x$ 和 $y$ 越大级乍， $Z_c$ 越大， $x$ 和 $y$ 越小帚湘。
$c_x$ 和 $c_y$ 表示中心點在像素坐標(biāo)系中的位置玫荣。
要求像素坐標(biāo)系中某像素點對應(yīng)在世界坐標(biāo)系中的位置，需要知道相機的內(nèi)參大诸、外參捅厂，相機的內(nèi)參可以通過標(biāo)定獲得贯卦，外參可以人為設(shè)定。
第一步焙贷，將像素坐標(biāo)變換到相機坐標(biāo)系：
$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的逆后推導(dǎo)出：
$\begin{bmatrix}x \\ y\\ z \end{bmatrix}=K^{-1} \begin{bmatrix}u \\ v\\ 1 \end{bmatrix}$
第二步撵割，從相機坐標(biāo)系變換到世界坐標(biāo)系：
$\begin{bmatrix}X_c \\ Y_c\\ Z_c \end{bmatrix} = R \begin{bmatrix}X \\ Y\\ Z \end{bmatrix} + t$
將方程乘以 $R^{-1}$ ，可以推導(dǎo)出：
$\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}$

代碼

通過輸入相機的內(nèi)參辙芍，旋轉(zhuǎn)向量啡彬，平移向量和像素坐標(biāo)，可以通過以下函數(shù)求出對應(yīng)的世界坐標(biāo)點故硅。
以下代碼中需求注意要對平移向量取轉(zhuǎn)置庶灿，將1x3矩陣變?yōu)?x1矩陣后，才能實現(xiàn)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);   //從旋轉(zhuǎn)矩陣求旋轉(zhuǎn)向量
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個畸變系數(shù)：k1,k2,p1,p2,k3 */
projectPoints(tempPointSet, mat_tmp, translation_vectors[i], intrinsic_matrix, distortion_coeffs1, image_points2);

使用以下歐拉角：

[0, 0, 0]//歐拉角度跳仿，表示平面和相機的角度
旋轉(zhuǎn)向量：[0, 0, 0]

對應(yīng)的平移向量，表示空間坐標(biāo)原點相對在相平面原點偏移x=134mm捐晶，y=132mm菲语，z=200mm。

原始:[134.0870803179094, 132.7580766544178, 200.3789038923399]

生成空間坐標(biāo)點：

        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++)
            {
                /* 假設(shè)定標(biāo)板放在世界坐標(biāo)系中z=0的平面上 */
                Point3f tempPoint;
                tempPoint.x = i*square_size.height;
                tempPoint.y = j*square_size.width;
                tempPoint.z = 0;
                tempPointSet.push_back(tempPoint);
            }
        }

經(jīng)projectPoints計算后對應(yīng)的像素空間點是：

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]

經(jīng)函數(shù)求出的空間坐標(biāo)點是：

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/格所生成空間坐標(biāo)點結(jié)果惑灵，基本一致山上。

最后編輯于：2019.08.21 19:46:12

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者

人面猴
序言：七十年代末，一起剝皮案震驚了整個濱河市英支，隨后出現(xiàn)的幾起案子佩憾，更是在濱河造成了極大的恐慌，老刑警劉巖干花，帶你破解...
沈念sama閱讀 206,214評論 6贊 481
死咒
序言：濱河連續(xù)發(fā)生了三起死亡事件妄帘，死亡現(xiàn)場離奇詭異，居然都是意外死亡池凄，警方通過查閱死者的電腦和手機抡驼，發(fā)現(xiàn)死者居然都...
沈念sama閱讀 88,307評論 2贊 382
救了他兩次的神仙讓他今天三更去死
文/潘曉璐我一進店門，熙熙樓的掌柜王于貴愁眉苦臉地迎上來肿仑，“玉大人致盟，你說我怎么就攤上這事∮任浚” “怎么了馏锡？”我有些...
開封第一講書人閱讀 152,543評論 0贊 341
道士緝兇錄：失蹤的賣姜人
文/不壞的土叔我叫張陵，是天一觀的道長伟端。經(jīng)常有香客問我杯道，道長，這世上最難降的妖魔是什么荔泳？我笑而不...
開封第一講書人閱讀 55,221評論 1贊 279
?港島之戀（遺憾婚禮）
正文為了忘掉前任蕉饼，我火速辦了婚禮虐杯，結(jié)果婚禮上玛歌，老公的妹妹穿的比我還像新娘昧港。我一直安慰自己，他們只是感情好支子，可當(dāng)我...
茶點故事閱讀 64,224評論 5贊 371
惡毒庶女頂嫁案：這布局不是一般人想出來的
文/花漫我一把揭開白布创肥。她就那樣靜靜地躺著，像睡著了一般值朋。火紅的嫁衣襯著肌膚如雪叹侄。梳的紋絲不亂的頭發(fā)上，一...
開封第一講書人閱讀 49,007評論 1贊 284
城市分裂傳說
那天昨登，我揣著相機與錄音趾代，去河邊找鬼。笑死丰辣，一個胖子當(dāng)著我的面吹牛撒强，可吹牛的內(nèi)容都是我干的。我是一名探鬼主播笙什，決...
沈念sama閱讀 38,313評論 3贊 399
雙鴛鴦連環(huán)套：你想象不到人心有多黑
文/蒼蘭香墨我猛地睜開眼飘哨，長吁一口氣：“原來是場噩夢啊……” “哼！你這毒婦竟也來了琐凭？” 一聲冷哼從身側(cè)響起芽隆，我...
開封第一講書人閱讀 36,956評論 0贊 259
萬榮殺人案實錄
序言：老撾萬榮一對情侶失蹤，失蹤者是張志新（化名）和其女友劉穎统屈，沒想到半個月后胚吁，有當(dāng)?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體，經(jīng)...
沈念sama閱讀 43,441評論 1贊 300
?護林員之死
正文獨居荒郊野嶺守林人離奇死亡愁憔，尸身上長有42處帶血的膿包…… 初始之章·張勛以下內(nèi)容為張勛視角年9月15日...
茶點故事閱讀 35,925評論 2贊 323
?白月光啟示錄
正文我和宋清朗相戀三年腕扶，在試婚紗的時候發(fā)現(xiàn)自己被綠了。大學(xué)時的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片惩淳。...
茶點故事閱讀 38,018評論 1贊 333
活死人
序言：一個原本活蹦亂跳的男人離奇死亡蕉毯，死狀恐怖，靈堂內(nèi)的尸體忽然破棺而出思犁，到底是詐尸還是另有隱情代虾，我是刑警寧澤，帶...
沈念sama閱讀 33,685評論 4贊 322
?日本核電站爆炸內(nèi)幕
正文年R本政府宣布激蹲，位于F島的核電站棉磨，受9級特大地震影響，放射性物質(zhì)發(fā)生泄漏学辱。R本人自食惡果不足惜乘瓤，卻給世界環(huán)境...
茶點故事閱讀 39,234評論 3贊 307
男人毒藥：我在死后第九天來索命
文/蒙蒙一环形、第九天我趴在偏房一處隱蔽的房頂上張望。院中可真熱鬧衙傀，春花似錦抬吟、人聲如沸。這莊子的主人今日做“春日...
開封第一講書人閱讀 30,240評論 0贊 19
一樁弒父案火本，背后竟有這般陰謀
文/蒼蘭香墨我抬頭看了看天上的太陽。三九已至聪建，卻和暖如春钙畔，著一層夾襖步出監(jiān)牢的瞬間，已是汗流浹背金麸。一陣腳步聲響...
開封第一講書人閱讀 31,464評論 1贊 261
情欲美人皮
我被黑心中介騙來泰國打工擎析，沒想到剛下飛機就差點兒被人妖公主榨干…… 1. 我叫王不留，地道東北人挥下。一個月前我還...
沈念sama閱讀 45,467評論 2贊 352
代替公主和親
正文我出身青樓揍魂，卻偏偏與公主長得像，于是被迫代替她去往敵國和親见秽。傳聞我的和親對象是個殘疾皇子愉烙，可洞房花燭夜當(dāng)晚...
茶點故事閱讀 42,762評論 2贊 345

像素坐標(biāo)轉(zhuǎn)世界坐標(biāo)的計算

原理

代碼

驗證

推薦閱讀更多精彩內(nèi)容