代码收藏家 【Python教程】深度相机移动状态下像素坐标转世界坐标详解
作者：朱海生
一、前言
二、坐标的转化流程
三、计算公式及参数讲解
四、python代码
五、总结
关键词：python、深度相机、坐标转换
一、前言
深度相机依靠着可以获取距离信息和图像距离的优点被应用在各个领域，本文主要讲解在运动状态下的深度相机的像素中的深度点转化到世界坐标中，此过程使用python代码实现，本文不赘述公式的推理过程，只讲解使用过程。
二、坐标的转化流程
在本文中主要写的是运动中的坐标转换，所以在坐标的转化流程中需要加上刚体变换， 在相机的初始状态，我们只有像素和距离信息，在此的基础上我们还需要相机的内参和外参。至于如何获得相机的内参自行查看相机标定。
已知参数：像素坐标、距离信息、相机内参、相机外参
相机外参通常是在外部获得：
首先选择一点作为世界坐标的原点，根据IMU模块获得相机移动的距离和旋转角度，计算相机外参代码如下：
#获得旋转矩阵
def Pose_Matrix():
x_cos=0
y_cos=0
z_cos=0 # 需要在传感器上获得的数据，为偏移角度，此处为0，0，0
a=(x_cos,y_cos,z_cos)
x_y_z_Matrix = cv2.Rodrigues(a)
print(x_y_z_Matrix[0])
return x_y_z_Matrix[0]
#获得平移矩阵
def Translate_Matrix():
x=0
y=0
z=0 #从传感器获得平移参数，此处为0，0，0
t_Matrix=np.diag([1,1,1])
t_Matrix[0][0] = x
t_Matrix[1][0] = y
t_Matrix[2][0] = z
return t_Matrix
三、计算公式及参数讲解
相机的像素坐标转为世界坐标公式如下：
（X、Y、Z）为世界坐标，R为上述的旋转矩阵，T为上述的平移矩阵，M为相机的内参，通过相机标定获取，s为尺度因子。
尺度因子的计算如下：
，
上述的
为深度相机测出的距离。
四、python代码
Python的完全版代码如下：
import cv2
import numpy as np
import cv2 as cv
import math
#获得旋转矩阵
def Pose_Matrix():
x_cos=0
y_cos=0
z_cos=0 # 需要在传感器上获得的数据
a=(x_cos,y_cos,z_cos)
x_y_z_Matrix = cv2.Rodrigues(a)
print(x_y_z_Matrix[0])
return x_y_z_Matrix[0]
#获得平移矩阵
def Translate_Matrix():
x=0
y=0
z=0 #从传感器获得平移参数
t_Matrix=np.diag([1,1,1])
t_Matrix[0][0] = x
t_Matrix[1][0] = y
t_Matrix[2][0] = z
return t_Matrix
#内参矩阵
def internal_Reference():
internal_Reference_Matrix=np.array( [[557.34692705, 0 , 320.15921429],
[ 0 , 574.29121785 ,233.20683435],
[ 0 , 0 , 1 ]])
return internal_Reference_Matrix
# 像素坐标转为矩阵
def pixel_cordinate_Matrix(pixel_coordinate):
u=pixel_coordinate[0]
v=pixel_coordinate[1]
pixel_Matrix=np.ones((3,1))
pixel_Matrix[0][0]=u
pixel_Matrix[1][0]=v
return pixel_Matrix
# 将像素坐标转为世界坐标
def pixel2world(pixel_coordinate,s):
pose_Matrix=Pose_Matrix() # 旋转矩阵
pose_Matrix=pose_Matrix.T # 旋转矩阵的转置
translate_Matrix=Translate_Matrix() # 平移矩阵
pixel_Matrix=pixel_cordinate_Matrix(pixel_coordinate) # 像素坐标的矩阵
internal_reference_Matrix=internal_Reference() #相机内参
internal_reference_Matrix=internal_reference_Matrix.T #相机内参转置
print(pose_Matrix,internal_reference_Matrix,pixel_Matrix)
Mat1=np.dot(pose_Matrix,internal_reference_Matrix)
Mat1=np.dot(Mat1,pixel_Matrix)
Mat2=np.dot(pose_Matrix,translate_Matrix)
s=(s+Mat2[2][0])/Mat1[2][0]
# 下述为计算公式，过程如图
world_Matrix=pixel_Matrix
world_Matrix=s*world_Matrix
world_Matrix=np.dot(internal_reference_Matrix,world_Matrix)
print(world_Matrix)
world_Matrix=world_Matrix-translate_Matrix
world_Matrix=np.dot(pose_Matrix,world_Matrix)
print(world_Matrix)
return world_Matrix
#测试数据
world=pixel2world((200,500),300)
五、总结
上述代码可完成将深度相机中的像素坐标转为世界坐标，通过此方法可是实现深度相机的3维建模。
作者：众智创新团队
