rigid_registration.py 文件源码

def updateTransform(self):
    muX = np.divide(np.sum(np.dot(self.P, self.X), axis=0), self.Np)
    muY = np.divide(np.sum(np.dot(np.transpose(self.P), self.Y), axis=0), self.Np)

    self.XX = self.X - np.tile(muX, (self.N, 1))
    YY      = self.Y - np.tile(muY, (self.M, 1))

    self.A = np.dot(np.transpose(self.XX), np.transpose(self.P))
    self.A = np.dot(self.A, YY)

    U, _, V = np.linalg.svd(self.A, full_matrices=True)
    C = np.ones((self.D, ))
    C[self.D-1] = np.linalg.det(np.dot(U, V))

    self.R = np.dot(np.dot(U, np.diag(C)), V)

    self.YPY = np.dot(np.transpose(self.P1), np.sum(np.multiply(YY, YY), axis=1))

    self.s = np.trace(np.dot(np.transpose(self.A), self.R)) / self.YPY

    self.t = np.transpose(muX) - self.s * np.dot(self.R, np.transpose(muY))