python实现坐标求取_根据相机位姿求指定点的世界坐标及其python实现

Author

shaniadolphin

求解目的

本文将展示位姿估计的一种应用，即通过单目相机对环境进行测量。简单来说，本文的工作就是利用下面的两幅图，在已知P1、P2、P3、P4四点世界坐标的情况下，计算出其它点的世界坐标。

如图所示，一个标准的标定板，标定板每个格子的尺寸是30mm，通过标定四周的4个点P1、P2、P3、P4，旁边有茶罐，有待求点为P5、P6。

这种应用可以利用一个已经尺寸物体，通过两张不同视角的照片求未知物体的尺寸。比如上图中的通过已知的标定板求茶罐的尺寸。而在现实应用中可以用这种方式来求车的尺寸，建筑的高度，货物的体积等等。

求解原理

如下图，根据P1、P2、P3、P4四点的空间坐标，通过openCV的PNP函数，可以计算出两次拍照的相机位姿，从而进一步计算出相机的坐标

与

。那么将

与

，

与

连成直线，获得两条直线方程

和

，组成方程组求解得到它们的交点，即为待求目标点的坐标。

1. 求出

点的相机坐标系坐标

关于P点如何从二维映射到三维，参考上图，

的坐标通过解

已经求出，待求点P在图像中的像素坐标为

。

求出

在相机坐标系中的坐标

(也就是上图中的

点)。具体的转换公式如下，式中

为相机镜头的焦距

，在本次实验中使用的是中一光学的35mm手动镜头。

为点的像素坐标，其余为相机内参数。

输入拍到的图片的点，包括待求点的像素坐标，示例如下：

P11 = np.array([0, 0, 0])

P12 = np.array([0, 300, 0])

P13 = np.array([210, 0, 0])

P14 = np.array([210, 300, 0])

p11 = np.array([1765, 725])

p12 = np.array([3068, 1254])

p13 = np.array([1249, 1430])

p14 = np.array([2648, 2072])

p4psolver1.Points3D[0] = np.array([P11,P12,P13,P14])

p4psolver1.Points2D[0] = np.array([p11,p12,p13,p14])

#p4psolver1.point2find = np.array([4149, 671])

#p4psolver1.point2find = np.array([675, 835])

p4psolver1.point2find = np.array([691, 336])

读取标定文件中的相机内参，代码如下，在代码里预设了相机的传感器大小，笔者所用的D7000单反是DX画幅的，根据可查到的资料，传感器的规格为23.6mm*15.6mm。

笔者用在本机的镜头是中一的35mm手动定焦镜头。

def getudistmap(self, filename):

with open(filename, 'r',newline='') as csvfile:

spamreader = csv.reader(csvfile, delimiter=',', quotechar='"')

rows = [row for row in spamreader]

self.cameraMatrix = np.zeros((3, 3))

#Dt = np.zeros((4, 1))

size_w = 23.6

size_h = 15.6

imageWidth = int(rows[0][1])

imageHeight = int(rows[0][2])

self.cameraMatrix[0][0] = rows[1][1]

self.cameraMatrix[1][1] = rows[1][2]

self.cameraMatrix[0][2] = rows[1][3]

self.cameraMatrix[1][2] = rows[1][4]

self.cameraMatrix[2][2] = 1

print(len(rows[2]))

if len(rows[2]) == 5:

print('fisheye')

self.distCoefs = np.zeros((4, 1))

self.distCoefs[0][0] = rows[2][1]

self.distCoefs[1][0] = rows[2][2]

self.distCoefs[2][0] = rows[2][3]

self.distCoefs[3][0] = rows[2][4]

scaled_K = self.cameraMatrix * 0.8 # The values of K is to scale with image dimension.

scaled_K[2][2] = 1.0

#newcameramtx = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(self.cameraMatrix, self.distCoefs, (imageWidth, imageHeight), np.eye(3), balance=0)

#map1, map2 = cv2.fisheye.initUndistortRectifyMap(self.cameraMatrix, self.distCoefs, np.eye(3), newcameramtx, (imageWidth, imageHeight), cv2.CV_32FC1)

else:

print('normal')

self.distCoefs = np.zeros((1, 5))

self.distCoefs[0][0] = rows[2][1]

self.distCoefs[0][1] = rows[2][2]

self.distCoefs[0][2] = rows[2][3]

self.distCoefs[0][3] = rows[2][4]

self.distCoefs[0][4] = rows[2][5]

#newcameramtx, roi = cv2.getOptimalNewCameraMatrix(self.cameraMatrix, self.distCoefs, (imageWidth, imageHeight), 1, (imageWidth, imageHeight))

#map1, map2 = cv2.initUndistortRectifyMap(self.cameraMatrix, self.distCoefs, None, newcameramtx, (imageWidth, imageHeight), cv2.CV_32FC1)

print('dim = %d*%d'%(imageWidth, imageHeight))

print('Kt = \n', self.cameraMatrix)

#print('newcameramtx = \n', newcameramtx)

print('Dt = \n', self.distCoefs)

self.f = [self.cameraMatrix[0][0]*(size_w/imageWidth), self.cameraMatrix[1][1]*(size_h/imageHeight)]

#self.f = [350, 350]

print('f = \n', self.f)

#print