基于仿射的非刚体配准方法(i) 法向

Posted 2020-11-24 wushibei

为啥闲呢，因为work干完了。

为啥补档呢，因为有新work了。

呃，因为新work让人自闭。

 

我现在干完了两部分。一是把最近邻的部分迁移过来。

二是求法向。

首先是给三个点，就能确定平面——因为是三角面片，也不太会有三点共线。

法向量垂直于平面，也就垂直于三个顶点之间构成的向量。

(x1-x2,y1-y2,z1-z2)垂直于法向量；

(x1-x3,y1-y3,z1-z3)垂直于法向量。

所以就是一个不定方程：

所以我们规定法向量z坐标为1——如果它不为0的话。

如果z坐标为零，我们规定y坐标为1——除非它也为0.

如果都为0，那除了(1,0,0)还能咋办。

def normaloftri(xmat):
    x=np.zeros((2,2),dtype=float)
    y=np.zeros((2),dtype=float)
    coor=np.zeros((3),dtype=float)
    x[0][0]=xmat[0][0]-xmat[2][0]
    x[0][1]=xmat[0][1]-xmat[2][1]
    x[1][0]=xmat[1][0]-xmat[2][0]
    x[1][1]=xmat[1][1]-xmat[2][1]
    y[0]=-xmat[0][2]+xmat[2][2]
    y[1]=-xmat[1][2]+xmat[2][2]
    if np.linalg.matrix_rank(x)==2:
        z=np.dot(np.linalg.inv(x),y)
        coorsum=(z[0]**2+z[1]**2+1)**0.5
        coor[0]=z[0]/coorsum
        coor[1]=z[1]/coorsum
        coor[2]=1/coorsum
    elif x[0][0]==0 and x[1][0]==0:
        coor[0]=1
    else:
        coor[1]=1
        if x[0][0]!=0:
            coor[0]=-x[0][1]/x[0][0]
        else:
            coor[0]=-x[1][1]/x[1][0]
        coorsum=(1+coor[0]**2)**0.5
        coor[0]=coor[0]/coorsum
        coor[1]=coor[1]/coorsum
    #affine.cherk(xmat,coor)
    return coor

　　

理论上我应该验证一下，这三种情况所规定的法向与面片的点的顺序是否一致。。。但是其实都在第一个分支里，你懂吧，可以，且没有必要。

法向量求好了，我现在是一个三角面片的法向量。

为了统一呢，我需要把它单位化。还需要检验。

def cherk(xmat,norm):
    for i in range(3):
        for j in range(2):
            xmat[j][i]=xmat[j][i]-xmat[2][i]
    y=np.dot(xmat,norm)
    t=abs(y[0])+abs(y[1])
    if t>0.01:
        print("error")

    return 0

　　

 

好，但我们其实要的点的坐标。

def normalfei(beimat,feimat,trinumber):
    weimat=np.zeros((trinumber,3),dtype=float)
    xmat=np.zeros((3,3),dtype=float)
    for i in range(trinumber):
        xmat[0,:]=beimat[int(feimat[i][0]),:]
        xmat[1,:]=beimat[int(feimat[i][1]),:]
        xmat[2,:]=beimat[int(feimat[i][2]),:]                 
        weimat[i,:]=affine.normaloftri(xmat)

    return weimat
def normalbei(beimat,feimat,weimat,potnumber,trinumber):
    yeimat=np.zeros((potnumber,3),dtype=float)
    for i in range(trinumber):
        for j in range(3):
            for k in range(3):
                yeimat[int(feimat[i][j])][k]=yeimat[int(feimat[i][j])][k]+weimat[i][k]
    for i in range(potnumber):
        coorsum=(yeimat[i][0]**2+yeimat[i][1]**2+yeimat[i][2]**2)**0.5
        for j in range(3):
            yeimat[i][j]=yeimat[i][j]/coorsum
    return yeimat

我们先按三角面片的信息，逐个找到每个面片的三顶点，算法向量。是第一部分。normalfei。

然后我们按每个面片，将每个三角面片的法向信息丢给每个顶点。

我们需要加权地丢：我想好了一个权重，就是按那个角的大小——啧啧啧，每个空间顶点都在不止一个空间三角形里，自然是在的顶角最大的那个should有最大的影响因子。

可以且有必要，但是我懒。[以后会有的。。。咕咕咕]

我就直接加了，权重都是1。

然后再单位化，就齐活了。

import numpy as np
import old
import cv2
import random
import old
import affine
import icp

f=open(r"11.ply",‘r‘)
g=open(r"22.ply",‘r‘)
[potnumber1,trinumber1,beimat1,feimat1]=old.fread(f,2)
[potnumber2,trinumber2,beimat2,feimat2]=old.fread(g,2)

weimat1=affine.normalfei(beimat1,feimat1,trinumber1)

yeimat1=affine.normalbei(beimat1,feimat1,weimat1,potnumber1,trinumber1)
maxmat1=icp.maxmatdeal(beimat1)
maxmat2=icp.maxmatdeal(beimat2)
for i in range(3):
    maxmat2[i][0]+=20
    maxmat2[i][1]-=20


deta2=32
choose2=5.5
maxmat2=icp.maxmatdeta(maxmat2,deta2)

#beimat: xyz *potnumber
#intmat: i,j,k,n in bei2 *potnumber
intmat1=np.zeros((potnumber1,7),dtype=int)
floatmat1=np.zeros((potnumber1,7),dtype=float)

[potmat2,intmatnew2]=icp.detadealnew(deta2,potnumber2,beimat2,maxmat2)
potmatnew2=icp.potmatrandom(potmat2,choose2,deta2)
[intmatnew1,floatmatnew1]=icp.nearestpoint(beimat1,beimat2,potnumber1,potmatnew2,maxmat2,deta2)
for i in range(potnumber1):
    intmat1[i][6]=intmatnew1[i][0]
    floatmat1[i][3:6]=beimat2[int(intmatnew1[i][0]),:]
    intmat1[i][3:6]=intmatnew2[int(intmatnew1[i][0]),:]
    floatmat1[i,0:3]=beimat1[i,:]
    floatmat1[i][6]=floatmatnew1[i][0]

　　走菜！

 

也许，你从这里看最近点模块还虚浮一点，毕竟整合过。

affine是刚才那些函数的所在文件。

choose2=5.5就是按5.5个点取1个来减少运算量。