文章目錄

• 座標變換詳解
• • 座標轉換
• 程式碼實現

座標變換詳解

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[

\right] =\left[

\right] \left[

\right] \left[

\right]s⎣⎡​uv1​⎦⎤​=⎣⎡​fx​00​0fy​0​cx​cy​1​⎦⎤​⎣⎡​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​t1​t2​t3​​⎦

比例係數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