python - 点云坐标 投射至 像素坐标

问题: 已知某点 点云坐标 (x,y,z) 求解 投射到相面 像素坐标(v , u) -- v为像素纵坐标, u 为像素横坐标

源代码为 c++  出处: https://github.com/IntelRealSense/librealsense/blob/master/include/librealsense2/rsutil.h#L67 中的 'rs2_project_point_to_pixel' 函数

python 实现如下:

注: rs2_project_point_to_pixel 中实现了多种畸变模型下的 '点云坐标 投射至 像素坐标' 之功能,  本人自用相机为 realsense D415 畸变模型为MODIFIED_BROWN_CONRADY, 故仅实现了该模型

import numpy as np
def xyz2vu(xyz,depth_cam_matrix,depth_dist_coeff=np.zeros(5)):
        # 畸变类型带添加
        fx,fy=depth_cam_matrix[0,0],depth_cam_matrix[1,1]
        cx,cy=depth_cam_matrix[0,2],depth_cam_matrix[1,2]
        xyz=np.array(xyz).flatten()
        x,y=xyz[0]/xyz[2],xyz[1]/xyz[2]
        r2=x*x+y*y
        f=1+depth_dist_coeff[0]*r2+depth_dist_coeff[1]*r2*r2+depth_dist_coeff[1]*r2*r2*r2
        x*=f
        y*=f        
        dx=x+2*depth_dist_coeff[2]*x*y+depth_dist_coeff[3]*(r2+2*x*x)
        dy=y+2*depth_dist_coeff[3]*x*y+depth_dist_coeff[2]*(r2+2*y*y)
        x,y=dx,dy
        u,v=x*fx+cx,y*fy+cy
        vu=np.int0([v,u])
        return vu

 

input:

xyz: 点云坐标

depth_cam_matrix: 深度相机内参矩阵(3x3)

depth_dist_coeff : 相机畸变系数(D415 的畸变系数为[0,0,0,0,0], 故本人将该值填为np.zeros(5))

output:

像素坐标 vu

最后: 内参矩阵, 畸变系数, 以及相机的畸变模型是什么, 去查相机的文档吧, 一般相机厂商都会提供这些参数

 

--------------------------------更新-------------------------------

另外 opencv 中存在相似功能函数 cv2.projectPoints, 最开始我以为该函数必须输入 姿态相关参数(旋转矢量 rvec, tvec, 红笔标出), 因为本人并不想求任何姿态(只想求3d点映射2d点), 但是阅读文档后中的note(蓝线标出)后发现: 可以设置 rvec = tvec = (0,0,0) 即可......, 如此便同我所实现的 xyz2vu 功能相同了.