opencv 從像素坐標得到世界坐標系坐標

文章目錄

• 坐標變換詳解
• • 坐標轉換
• 代碼實現

 

坐標變換詳解

1.1 坐標關系

相機中有四個坐標系，分別為world，camera，image，pixel

• world為世界坐標系，可以任意指定軸和軸，為上圖P點所在坐標系。
• camera為相機坐標系，原點位於小孔，z軸與光軸重合，軸和軸平行投影面，為上圖坐標系。
• image為圖像坐標系，原點位於光軸和投影面的交點，軸和軸平行投影面，為上圖坐標系。
• pixel為像素坐標系，從小孔向投影面方向看，投影面的左上角為原點，uv軸和投影面兩邊重合，該坐標系與圖像坐標系處在同一平面，但原點不同。

坐標轉換

下式為像素坐標pixel與世界坐標world的轉換公式，左側第一個矩陣為相機內參數矩陣，第二個矩陣為相機外參數矩陣。假設圖像坐標已知，同時相機內參數矩陣通過標定已獲取，還需計算比例系數s和外參數矩陣。
s [ u v 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ r 11 r 12 r 13 t 1 r 21 r 22 r 23 t 2 r 31 r 32 r 33 t 3 ] [ x y z 1 ] s \left[

uv1uv1

\right] =\left[

fx00amp;0amp;fyamp;0amp;cxamp;cyamp;1fxamp;0amp;cx0amp;fyamp;cy0amp;0amp;1

\right] \left[

r11r21r31amp;r12amp;r22amp;r32amp;r13amp;r23amp;r33amp;t1amp;t2amp;t3r11amp;r12amp;r13amp;t1r21amp;r22amp;r23amp;t2r31amp;r32amp;r33amp;t3

\right] \left[

xyz1xyz1

\right]s⎣⎡​uv1​⎦⎤​=⎣⎡​fx​00​0fy​0​cx​cy​1​⎦⎤​⎣⎡​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​t1​t2​t3​​⎦⎤​⎣⎢⎢⎡​xyz1​⎦⎥⎥⎤​\right] = M(R\left[\right]+t)

 

比例系數s轉換公式可簡化為：

 

s [ u v 1 ] = M ( R [ X Y Z c o n s t ] + t ) s \left[

 

uv1uv1

 

XYZconstXYZconsts⎣⎡​uv1​⎦⎤​=M(R⎣⎡​XYZconst​​⎦⎤​+t)\right] = \left[\right] +R^{-1}t

 

M為相機內參數矩陣，R為旋轉矩陣，t為平移矩陣，為世界坐標系高度，可設置為0。

通過矩陣變換可得下式：

R − 1 M − 1 s [ u v 1 ] = [ X Y Z c o n s t ] + R − 1 t R^{-1}M^{-1}s \left[

 

uv1uv1

 

XYZconstXYZconstR−1M−1s⎣⎡​uv1​⎦⎤​=⎣⎡​XYZconst​​⎦⎤​+R−1t

 

求解出旋轉矩陣和平移矩陣即可算得s。

代碼實現

• input：Point2f inPoints - 像素坐標系下的點Mat rvec - 標定得到的rvecs(下的rvecs[i])Mat tvec - 標定得到的tvecs(下的tvecs[i]) ------以第i個棋盤格頂點建立坐標系Mat cameraMatrix - 標定得到的內參矩陣

vector<Mat>
vector<Mat>

• output: (return value)Point3f worldPoint

 

Point3f getWorldPoints(Point2f &inPoints, Mat &rvec, Mat &tvec, Mat &cameraMatrix) { //initialize parameter Mat rotationMatrix;//3*3 Rodrigues(rvec,rotationMatrix); double zConst = 0;//實際坐標系的距離，若工作平面與相機距離固定可設置為0 double s; //獲取圖像坐標 cv::Mat imagePoint = (Mat_<double>(3,1)<<double(inPoints.x),double(inPoints.y),1); // cv::Mat::ones(3, 1, cv::DataType<double>::type); //u,v,1 // imagePoint.at<double>(0, 0) = inPoints.x; // imagePoint.at<double>(1, 0) = inPoints.y; //計算比例參數S cv::Mat tempMat, tempMat2; tempMat = rotationMatrix.inv() * cameraMatrix.inv() * imagePoint; tempMat2 = rotationMatrix.inv() * tvec; s = zConst + tempMat2.at<double>(2, 0); s /= tempMat.at<double>(2, 0); //計算世界坐標 Mat wcPoint = rotationMatrix.inv() * (s * cameraMatrix.inv() * imagePoint - tvec); Point3f worldPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0)); return worldPoint; }

 

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26

參考：

https://blog.csdn.net/Kalenee/article/details/80659489