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]suv1=fx000fy0cxcy1r11r21r31r12r22r32r13r23r33t1t2t3xyz1\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

 

XYZconstXYZconstsuv1=M(RXYZconst+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

 

XYZconstXYZconstR1M1suv1=XYZconst+R1t

 

求解出旋转矩阵和平移矩阵即可算得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

posted @ 2020-10-25 20:20  瘋耔  阅读(2882)  评论(0编辑  收藏  举报
跳至侧栏