rigid_registration.py 文件源码

def updateVariance(self):
    qprev = self.q

    trAR     = np.trace(np.dot(self.A, np.transpose(self.R)))
    xPx      = np.dot(np.transpose(self.Pt1), np.sum(np.multiply(self.XX, self.XX), axis =1))
    self.q   = (xPx - 2 * self.s * trAR + self.s * self.s * self.YPY) / (2 * self.sigma2) + self.D * self.Np/2 * np.log(self.sigma2)
    self.err = np.abs(self.q - qprev)

    self.sigma2 = (xPx - self.s * trAR) / (self.Np * self.D)

    if self.sigma2 <= 0:
      self.sigma2 = self.tolerance / 10