Fixed a bug that off-diagonal are not being properly added during the normal equation construction when partial diagonalization is performed.