nal4

TineSubic_64170402_vaja5.py

def addHomCoord3D(iPts):
    if iPts.shape[-1] == 4:
        return iPts
    else:
        return np.hstack((iPts, np.ones((iPts.shape[0], 1))))


def transAffine3D(iScale=(1, 1, 1), iTrans=(0, 0, 0), iRot=(0, 0, 0), iShear=(0, 0, 0, 0, 0, 0)):
    alfa = iRot[0] * np.pi / 180
    beta = iRot[1] * np.pi / 180
    gama = iRot[2] * np.pi / 180
    oMatScale = np.matrix(((iScale[0], 0, 0, 0),
                           (0, iScale[1], 0, 0),
                           (0, 0, iScale[2], 0),
                           (0, 0, 0, 1)))
    oMatTrans = np.matrix(((1, 0, 0, iTrans[0]),
                           (0, 1, 0, iTrans[1]),
                           (0, 0, 1, iTrans[2]),
                           (0, 0, 0, 1)))
    rotAlfa = np.matrix(
        ((1, 0, 0, 0),
         (0, np.cos(alfa), -np.sin(alfa), 0),
         (0, np.sin(alfa), np.cos(alfa), 0),
         (0, 0, 0, 1)))
    rotBeta = np.matrix(
        ((np.cos(beta), 0, np.sin(beta), 0),
         (0, 1, 0, 0),
         (-np.sin(beta), 0, np.cos(beta), 0),
         (0, 0, 0, 1)))
    rotGama = np.matrix(
        ((np.cos(gama), -np.sin(gama), 0, 0),
         (np.sin(gama), np.cos(gama), 0, 0),
         (0, 0, 1, 0),
         (0, 0, 0, 1)))
    oMatRot = rotAlfa * rotBeta * rotGama
    oMatShear = np.matrix(
        ((1, iShear[0], iShear[1], 0),
         (iShear[2], 1, iShear[3], 0),
         (iShear[4], iShear[5], 1, 0),
         (0, 0, 0, 1)))
    return oMatTrans * oMatShear * oMatRot * oMatScale


def renderHead(glava, matrix, title=''):
    rotglava = np.dot(addHomCoord3D(glava), matrix.transpose())
    xs = rotglava[:, 0]
    xs = np.squeeze(np.asarray(xs))
    ys = rotglava[:, 1]

    ys = np.squeeze(np.asarray(ys))
    zs = rotglava[:, 2]

    zs = np.squeeze(np.asarray(zs))

    render.renderSurface(xs, ys, zs, iTitle=title)


# %%Naloga3
def naloga3():
    heads = np.load('glave.npy')

    renderHead(heads[0], transAffine3D(iRot=(180, 0, 0)))
    renderHead(heads[0], transAffine3D(iRot=(0, 0, 180)))
    renderHead(heads[1], transAffine3D(iTrans=(10, 0, 0), iRot=(0, 0, 180)))
    renderHead(heads[1], transAffine3D(iShear=(1, 2, 3, 0, 0, 0), iRot=(0, 0, 180)))
    rv.plt.show()


from tqdm import tqdm


# %%Naloga4
def naloga4():
    def mapAffineApprox3D(iPtsRef, iPtsMov):
        iPtsRef = addHomCoord3D(np.matrix(iPtsRef)).transpose()
        iPtsMov = addHomCoord3D(np.matrix(iPtsMov)).transpose()
        return iPtsRef * iPtsMov.transpose() * np.linalg.inv(iPtsMov * iPtsMov.transpose())

    def findCorrespondingPoints3D(iPtsRef, iPtsMov):
        iPtsRef = (np.array(iPtsRef))
        iPtsMov = (np.array(iPtsMov))

        idxPair = -np.ones((iPtsRef.shape[0], 1), dtype='int32')
        idxDist = np.ones((iPtsRef.shape[0], iPtsMov.shape[0]), dtype='float')

        for i in range(iPtsRef.shape[0]):
            for j in range(iPtsMov.shape[0]):
                idxDist[i, j] = np.sum((iPtsRef[i, :3] - iPtsMov[j, :3]) ** 2)
        while not np.all(idxDist == np.inf):
            i, j = np.where(idxDist == np.min(idxDist))
            idxPair[i[0]] = j[0]
            idxDist[i[0], :] = np.inf
            idxDist[:, j[0]] = np.inf
        idxValid, idxNotValid = np.where(idxPair > 0)
        idxValid = np.array(idxValid)
        iPtsRef_t = iPtsRef[idxValid, :]
        iPtsMov_t = iPtsMov[idxPair[idxValid].flatten(), :]
        return iPtsRef_t, iPtsMov_t

    def alignICP3D(iPtsRef, iPtsMov, iEps=1e-6, iMaxIter=50):
        currentMat = []
        oErr = []
        iCurrIter = 0

        for i in range(iMaxIter):
            iPtsRef_t, iPtsMov_t = findCorrespondingPoints3D(iPtsRef, iPtsMov)
            if i == 0:
                oErr.append(np.sqrt(np.sum((iPtsRef_t[:, :3] - iPtsMov_t[:, :3]) ** 2)))
            oMat3D = mapAffineApprox3D(iPtsRef_t, iPtsMov_t)
            iPtsMov = np.dot(addHomCoord3D(iPtsMov), oMat3D.transpose())
            currentMat.append(oMat3D)
            oErr.append(np.sqrt(np.sum((iPtsRef_t[:, :3] - iPtsMov_t[:, :3]) ** 2)))
            dMat = np.abs(oMat3D - transAffine3D())
            if np.all(dMat < iEps):
                break

        oMat3D = transAffine3D()
        for mat in currentMat:
            oMat3D = np.dot(mat, oMat3D)
        return oMat3D, oErr

    heads = np.load('glave.npy')

    step = 1000

    errs = []
    for i, head in enumerate(heads[1:5]):
        transMatrix, err = alignICP3D(heads[0][::step, :], head[::step, :])
        errs.append([np.min(err[0]), np.min(err)])
        print('Rendering {}'.format(i))
        # renderHead(head[::step, :], transMatrix, title='Aligned head {}'.format(i + 1))

    # renderHead(heads[0][::step, :], transAffine3D(), title='Original Head (0)')
    before = [item[0] for item in errs]
    after = [item[1] for item in errs]
    print('Max error before alignment: {:3.3} for picture {}'.format(np.max(before), (np.argmax(before) + 1)))
    print('Min error before alignment: {:3.3} for picture {}'.format(np.min(before), (np.argmin(before) + 1)))
    print('Max error after alignment: {:3.3} for picture {}'.format(np.max(after), (np.argmax(after) + 1)))
    print('Min error after alignment: {:3.3} for picture {}'.format(np.min(after), (np.argmin(after) + 1)))

    rv.plt.show()

    myTrans = transAffine3D(iScale=(1, 2, 1), iTrans=(1, 0, 10))
    testHead = heads[2][::step, :]
    newHead = np.dot(addHomCoord3D(testHead), myTrans.transpose())

    transMatrix, err = alignICP3D(testHead, newHead)

    print("Average error {}".format(np.average(np.abs(myTrans - transMatrix))))
    print("Min err: {}, max err: {}".format(np.min(err), np.max(err)))