前言:接上文,使用Python双目相机标定后,通过相机的内参数矩阵与外参数矩阵计算出以左相机为原点的三维空间坐标系的坐标,此时需要使用坐标系转换,将坐标转化为指定的坐标系下的坐标。
坐标转换我目前掌握了大概三种方法,好多人问坐标转换的方法,这里不再一一回复,使用几篇博客详细介绍。
1.七参数模型
首先我们介绍测绘中,最常用的坐标系转换方法——七参数模型。
这种方法把坐标系转换的未知数转化为1个缩放的尺度参数,3个平移的参数,3个旋转参数,具体原理不多解释了,大家上度娘吧。
注意:七参数模型的背景是测绘学中大地坐标系转换。但是,在转换过程中,由于大地坐标系之间的旋转角很小,因此提出的该模型。所以,重点是,该模型只能在旋转角很小时使用(多小呢,emm,就只有几秒吧,可以说非常小,因此,鸡肋,摄影测量中很少直接使用)。所以,在没有旋转时,可以使用七参数模型进行转换。
直接上代码,如下。
import numpy as np
import math
# 布尔莎七参数模型主函数
# 输入转换前、后坐标系下3个控制点的坐标oldXYZ,newXYZ,类型为list
def bursa7para(oldXYZ, newXYZ):
m = len(oldXYZ)
X1 = np.array(oldXYZ)[:, 0].reshape((m, 1))
Y1 = np.array(oldXYZ)[:, 1].reshape((m, 1))
Z1 = np.array(oldXYZ)[:, 2].reshape((m, 1))
X2 = np.array(newXYZ)[:, 0].reshape((m, 1))
Y2 = np.array(newXYZ)[:, 1].reshape((m, 1))
Z2 = np.array(newXYZ)[:, 2].reshape((m, 1))
if m < 3:
print('少于三个点')
# v=bx-l
# f分别构件矩阵B,L,通过最小二乘求解
P = np.eye(3 * m)
B1 = np.c_[np.ones((m, 1)), np.zeros((m, 1)), np.zeros((m, 1)), X1, np.zeros((m, 1)), np.dot(Z1, -1), Y1]
B2 = np.c_[np.zeros((m, 1)), np.ones((m, 1)), np.zeros((m, 1)), Y1, Z1, np.zeros((m, 1)), np.dot(X1, -1)]
B3 = np.c_[np.zeros((m, 1)), np.zeros((m, 1)), np.ones((m, 1)), Z1, np.dot(Y1, -1), X1, np.zeros((m, 1))]
B = np.r_[B1, B2, B3]
L = np.r_[X2, Y2, Z2]
N = npmulti(B.T, P, B)
dx = npmulti(np.linalg.inv(N), B.T, P, L)
print(dx)
X0 = dx[0][0]
Y0 = dx[1][0]
Z0 = dx[2][0]
u = dx[4][0] / dx[3][0]
v = dx[5][0] / dx[3][0]
w = dx[6][0] / dx[3][0]
k = dx[3][0] - 1
return X0, Y0, Z0, u, v, w, k
# 将原坐标转换为新坐标系坐标的函数
def trans(X0, Y0, Z0, u, v, w, k, XYZ):
T = np.array([X0, Y0, Z0]).reshape((3, 1))
Rx = np.eye(3)
Ry = np.eye(3)
Rz = np.eye(3)
Rx[1][1] = math.cos(u)
Rx[2][2] = math.cos(u)
Rx[1][2] = math.sin(u)
Rx[2][1] = -math.sin(u)
Ry[0][0] = math.cos(v)
Ry[2][2] = math.cos(v)
Ry[0][2] = -math.sin(v)
Ry[2][0] = math.sin(v)
Rz[0][0] = math.cos(w)
Rz[1][1] = math.cos(w)
Rz[0][1] = math.sin(w)
Rz[1][0] = -math.sin(w)
R = npmulti(Rz, Ry, Rx)
XB = T + npmulti(R, XYZ, (1 + k))
return XB
# 自己写的一个函数,可以实现任意个矩阵相乘,前提条件第一个参数a为矩阵(写判断很麻烦,不如制定规则)
def npmulti(a, *par):
# a为矩阵
multi = a
for p in par:
if isinstance(p, np.ndarray):
multi = np.matmul(multi, p)
elif isinstance(p, int) or isinstance(p, float):
multi = np.dot(multi, p)
return multi
以上为所有的函数,接下来使用一组点测试。
还是那句话,七参数不能有旋转,因此,测试数据只包括平移缩放(平移缩放顺序哪个先都可以),代码如下。
if __name__ == '__main__':
oldXYZ = []
newXYZ = []
# 原坐标系下的坐标
oldXYZ.append((50, 0, 0))
oldXYZ.append((0, 50, 0))
oldXYZ.append((0, 0, 50))
# 新坐标系的坐标
# 在原坐标基础上,先z+10,再每个轴缩小1/2
newXYZ.append((25, 0, 5))
newXYZ.append((0, 25, 5))
newXYZ.append((0, 0, 30))
X0, Y0, Z0, u, v, w, k = bursa7para(oldXYZ, newXYZ)
print(X0, Y0, Z0, u, v, w, k)
XA = np.array(oldXYZ[0]).reshape((3, 1))
XB = trans(X0, Y0, Z0, u, v, w, k, XA)
print(XB)
结果如下。
可以看出,平移量为5(emm,毕竟要先缩放的,前三项为平移),缩放为0.5(第四个值),后面三个为旋转量(该旋转是由矩阵正交保证的三个参数)。XB的值为原坐标转换后的坐标(该点取的是第一个控制点的坐标)。
如果想要验证旋转,就加旋转变量上去测试,结果肯定是不行的。
综上,七参数就到这里。由于不具有旋转,比较具有局限性,摄影测量中不怎么常用。下一章,13参数坐标转换?还是别的?看心情把。