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(map1,'\n',map2.T)
return
然后就可以将像素坐标转换到世界坐标了ÿ