scipy/torch拟合变换矩阵
评论 0 热度 202
scipy.leastsq
最小化一组方程的平方和
$x=\underset{y}{\argmin}{(\sum{f^2(y), axis=0})}$
scipy.optimize.leastsq(func,x0,args = (),Dfun = None,full_output = 0,col_deriv = 0,ftol = 1.49012e-08,xtol = 1.49012e-08,gtol = 0.0,maxfev = 0,epsfcn = None,factor = 100,diag = None)
func: 误差函数
x0: 初始坐标点
args: 传入参数 配合func使用
网上例子多是用来求解函数最小值或者拟合一些简单函数。这里需要拟合一个矩阵(相机外参),使用scipy也可以拟合,与拟合简单函数没啥区别,但是需要注意scipy传入的func函数,该函数输出的为向量!具体可看下面的error函数。类似于深度学习里面的loss函数,为每个输入计算一个loss值。
公式:
$\begin{pmatrix}u \\ v \\ 1 \end{pmatrix} = M_{int} M_{ext}\begin{pmatrix}x_w \\ y_w \\ z_w \\ 1 \end{pmatrix}$
其中,$(u, v)^T$为图像坐标系,$(x_w, y_w, z_w)^T$为真实世界坐标系,$M_{int}$是已知的3x3大小矩阵(相机内参),$M_{ext}$是待拟合的3x4大小的矩阵(外参)。$M_{ext}$可以由6个参数计算得到。现已有一一对应的$(u, v)^T$和$(x_w, y_w, z_w)^T$数据,需要拟合的是$M_{ext}$。
定义部分代码:
def Fun(p, X, ext = False): # 计算矩阵(u, v, 1)^T
intrinsic = np.array([
[1970.086141564117, 0, 938.2454942738381],
[0, 1967.320777162276, 301.0933671111374],
[0, 0, 1]
])
if not type(ext) is np.ndarray: # 如果没有给定M_ext矩阵,则通过p参数计算
ext = getExtMatrix(p)
if ext.shape[0] != 3 or ext.shape[1] != 4:
ext = np.reshape(ext, (3, 4))
Y = []
for x in X: # 对每个样本计算 得到y值 存入Y Y内容即:((u1, v1), (u2, v2), ...)
x = np.array([x[0], x[1], x[2], 1])
y = np.dot(np.dot(intrinsic, ext), x)
y = y[:2] / y[2] # 注意 这里取了y的前两个元素 舍弃掉第三个元素
Y.append(y)
return np.array(Y)
def getExtMatrix(p): # 计算矩阵 M_ext (由p参数计算得出)
x, y, z = p[:3]
R = [[cos(y)*cos(z), cos(y)*sin(z), -sin(y)],
[cos(z)*sin(x)*sin(y) - cos(x)*sin(z), cos(x)*cos(z) + sin(x)*sin(y)*sin(z), cos(y)*sin(x)],
[sin(x)*sin(z) + cos(x)*cos(z)*sin(y), cos(x)*sin(y)*sin(z) - cos(z)*sin(x), cos(x)*cos(y)]]
T = p[3:]
ext = np.zeros((3, 4))
ext[:3, :3] = R
ext[0, 3] = T[0]
ext[1, 3] = T[1]
ext[2, 3] = T[2]
return ext
def error(p, x, y): # 计算残差
err = np.abs(Fun(p, x) - y) # 计算误差 此时形状为(Nx2)
err = np.sum(err, axis = 1) # 求和 最终得到的是一列向量 与输入x一一对应
return err
输入为fakeLidar,形状:(112x3),输出为fakePhoto,形状:(112x2)。
拟合:
initMatrix = np.array([0, 0, 0, 1, 1, 1])
para = leastsq(error, initMatrix, args=(fakeLidar, fakePhoto))
para[0], getExtMatrix(para[0]), np.mean(Fun(para[0], fakeLidar) - fakePhoto, axis = 0)
scipy.minimize
minimize: (fun: Any, x0: Any, args: Any = (), method: Any | None = None, jac: Any | None = None, hess: Any | None = None, hessp: Any | None = None, bounds: Any | None = None, constraints: Any = (), tol: Any | None = None, callback: Any | None = None, options: Any | None = None)
fun: 损失函数
x0: 初始值
args: 附加参数,默认从第二个开始
method: 采用优化方式
options: 其他设置 字典的形式 例: options={'maxiter': 400}
constraints: 约束条件
tol: 目标函数误差范围,控制迭代结束
沿用上面的代码,扩写如下:
def totalAvgError(p):
x = fakeLidar
y = fakePhoto
err = error(p, x, y)
return np.mean(np.square(err))
mymin = minimize(totalAvgError, initMatrix, method='CG')
mymin.x
torch
构建模型,继承nn.Module
类,使用nn.Parameter
包围tensor作为参数,使用优化器优化,学习率需要细调。
intrinsic = torch.tensor(intrinsic, dtype=torch.float32, requires_grad=False)
fakeLidar = torch.tensor(fakeLidar, dtype=torch.float32, requires_grad=False)
fakePhoto = torch.tensor(fakePhoto, dtype=torch.float32, requires_grad=False)
class mModel(nn.Module):
def __init__(self):
super(mModel, self).__init__()
# para: (x, y, z, T0, T1, T2)
self.params = nn.Parameter(torch.Tensor([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
def getExtMatrix(self):
x, y, z = self.params[:3]
R = torch.tensor([[torch.cos(y)*torch.cos(z), torch.cos(y)*torch.sin(z), -torch.sin(y)],
[torch.cos(z)*torch.sin(x)*torch.sin(y) - torch.cos(x)*torch.sin(z), torch.cos(x)*torch.cos(z) + torch.sin(x)*torch.sin(y)*torch.sin(z), torch.cos(y)*torch.sin(x)],
[torch.sin(x)*torch.sin(z) + torch.cos(x)*torch.cos(z)*torch.sin(y), torch.cos(x)*torch.sin(y)*torch.sin(z) - torch.cos(z)*torch.sin(x), torch.cos(x)*torch.cos(y)]], dtype=torch.float32)
T = self.params[3:]
ext = torch.zeros((3, 4))
ext[:3, :3] = R
ext[0, 3] = T[0]
ext[1, 3] = T[1]
ext[2, 3] = T[2]
return ext
def forward(self, x):
# x: (x, y, z, T0, T1, T2)
extMatrix = self.getExtMatrix()
tmp = torch.mm(intrinsic, extMatrix)
tmp = torch.unsqueeze(tmp, 0)
x = torch.unsqueeze(x, -1)
y = torch.matmul(tmp, x)
out = y.clone() # 注意这里 需要计算梯度的tensor不能有inplace操作,所以克隆新的tensor
out[:, 0] = y[:, 0] / y[:, -1]
out[:, 1] = y[:, 1] / y[:, -1]
out = torch.squeeze(out[:, :2], dim=-1)
return out
def calErr(y_pred, y_real):
return torch.mean(torch.abs(y_pred - y_real))
camModel = mModel()
optimizer = optim.SGD(camModel.parameters(), lr=1e-7, momentum=0.8)
优化过程如下。
BS = 4
minLoss = 10000
bestParam = []
for epoch in range(1000):
for i in range(len(fakeLidar)//BS):
X = fakeLidar[i*BS:(i+1)*BS]
X = torch.concat([X, torch.ones((X.shape[0], 1), dtype=X.dtype)], dim=1)
Y = fakePhoto[i*BS:(i+1)*BS]
optimizer.zero_grad()
outputs = camModel(X)
loss = calErr(outputs, Y)
loss.backward()
optimizer.step()
l = loss.item()
if l < minLoss:
minLoss = l
bestParam = (camModel.getExtMatrix().detach().cpu().numpy())
print('\r[%d, %5d] loss: %.3f' % (epoch + 1, i + 1, l), end='')
camModel.getExtMatrix(), minLoss, bestParam # 获取参数