单目固定平面已知像素坐标求世界坐标方法

单目固定平面已知像素坐标求世界坐标方法

环境:固定单目与固定工作平面(所有world定位点在一个平面内)

把所有坐标转到相机坐标下计算，求出对应的相机坐标，然后转为自己需要的空间坐标，这里没考虑畸变，所有像素坐标都利用undistortpoints转换为无畸变坐标。求出的像素坐标也是无畸变结果

#---------------------------------

相机坐标转为像素坐标：

       （公式1）

 = 设为P

#--------------------------

世界坐标转为相机坐标

   （公式2）

#-------------------------

其中：

M= 内参矩阵R是旋转矩阵 T是平移向量

uv1像素坐标 xcyczc相机坐标 xwywzw世界坐标

#---------------------------------------------------------------

利用camera坐标（u，v）求world坐标方法：

先求对应的相机坐标Pc，再求P

所有点在世界坐标系的z=0 平面内

红色部分是当时已知的量

     求出相机坐标P0

    求出相机坐标p1

    （像素点坐标对应的平面内坐标

   (*) 

  #法向量

    垂直向量内积为0

     (*)等式代入Pc

  求出Zc，代入(*) ,求出Pc

 Pc求出对应的Pw（公式2的逆运算）