scipy/torch拟合变换矩阵

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

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 # 获取参数

若无特别标注，本文由Luuljh创作
采用知识共享署名 3.0，可自由转载、引用，但需署名作者且注明文章出处。