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

ight] =left[

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

ight] left[

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

ight] left[

xyz1xyz1

ight]s⎣⎡​uv1​⎦⎤​=⎣⎡​fx​00​0fy​0​cx​cy​1​⎦⎤​⎣⎡​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​t1​t2​t3​​⎦⎤​⎣⎢⎢⎡​xyz1​⎦⎥⎥⎤​ ight] = M(Rleft[ ight]+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) ight] = left[ ight] +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