经过了一段时间的研bai究gei。。。终于可以偷得几天闲了。
这里来补个档。
无论是ICP还是TPS，缺乏锚点的前提下。你总是要通过找另一个曲面的最近的点来实现你的work
beimat：点数*3，float：点的坐标
feimat：三角数*3，int：顶点序号
首先是一个非精确手动校准。
def handfix(thx,thy,thz,k,x,y,z):
mat1=np.zeros((3,3),dtype=float)
mat2=np.zeros((3,3),dtype=float)
mat3=np.zeros((3,3),dtype=float)
mat4=np.zeros((3,3),dtype=float)
mat4[0][0]=k
mat4[1][1]=k
mat4[2][2]=k
mat1[2][2]=1
mat2[1][1]=1
mat3[0][0]=1
mat1[0][0]=np.cos(thx)
mat1[1][0]=-np.sin(thx)
mat1[0][1]=np.sin(thx)
mat1[1][1]=np.cos(thx)
mat2[0][0]=np.cos(thy)
mat2[2][0]=-np.sin(thy)
mat2[0][2]=np.sin(thy)
mat2[2][2]=np.cos(thy)
mat3[1][1]=np.cos(thz)
mat3[2][1]=-np.sin(thz)
mat3[1][2]=np.sin(thz)
mat3[2][2]=np.cos(thz)
mat=np.dot(mat4,np.dot(mat3,np.dot(mat2,mat1)))
t=np.zeros((3),dtype=float)
t[0]=x
t[1]=y
t[2]=z
return mat,t
我们定义了四个矩阵和三类参数。
(thx,thy,thxz)&(k)&(dx,dy,dz)分别代表了绕轴旋转的量，放缩的量，平移的量。
mat=np.dot(mat4,np.dot(mat3,np.dot(mat2,mat1)))
这句话规定了，旋转，放缩的顺序。先绕x，接绕y，后绕z，整体放缩，再平移。
如果你愿意的话，你可以调整旋转的顺序。你也可以分开旋转，把旋转矩阵都整明白了，乘起来，再平移。
因为这种手动校准，我是打了“断点”。（我其实把后面全部注释掉。。。）
由于这种手动校准，通常只会出现一次，所以，我就不优化了。哈哈哈哈哈你打我啊！！！
def handfixbei(beimat,potnumber,mat,t):
beinew=copy.deepcopy(beimat)
for i in range(potnumber):
x=beimat[i]
y=np.dot(mat,x)+t
beinew[i]=y
return beinew
你要知道，这里的所谓平移只是一个整合，我一次列了七个量，有点混的说。
所以你们自己做的时候，可以把thxthythz整合一个向量输入，也可以不输入dxdydz，反正我只是将他整合成一个向量。总之这都是优化的方向。
因为这是个日抛函数，所以，嘿嘿嘿。。。反正你打不到我。
既然刚才是在整合矩阵，我们这个函数呢，就是执行Rx+t，换言之，就是给他旋转好。
好了，接下来才是重点。就是寻找最近点。
如果你愿意历遍所有点，那我们自然没有什么好讲的。
我的方法有点八叉树的意思。但是单纯的八叉树不好用。
我一次把空间分割成16*16*16块该有多好。
如果你的计算机支持的话，1000*1000*1000也不是不行。。。也行可以，但没必要。
我现在要做的事情是准备好了篮子，然后把每个点丢进去。
首先，我们要准备篮子，你得知道这个点云，占了空间中多大的位置？他是跟老和山一样大呢，还是跟我手边的快乐水一样大，这是个问题。
def maxmatdeal(beimat): maxmat=np.zeros((3,3),dtype=float) for i in range(3): maxmat[i][0]=int(max(beimat[:,i])+1) maxmat[i][1]=int(min(beimat[:,i])-1) return maxmat def maxmatdeta(maxmat,deta): for i in range(3): maxmat[i][2]=(maxmat[i][0]-maxmat[i][1])/deta return maxmat
三个坐标轴占三行，第一列最大值，第二列最小值，第三列细度。
除非你不想等分。。。
需要指出的是，我们将每个坐标轴都分为n+1个部分。（n就是deta）。+1的意思是，还有空间之外的部分——没有点的部分。
三列里知道两个，就足以找到你想要的篮子了。
find zone
1defdetadealnew(deta,potnumber,beimat,maxmat):2intmatnew=np.zeros((potnumber,3),dtype=int)3potmat=[[[[]foriinrange(deta)]forjinrange(deta)]forkinrange(deta)]4foriinrange(potnumber):5coor=beimat[i,:]6coorback=icp.findxyz(coor,maxmat)7intmatnew[i,0:3]=coorback8potmat[int(coorback[0])][int(coorback[1])][int(coorback[2])].append(i)9returnpotmat,intmatnew10deffindxyz(coor,maxmat):11coorback=np.zeros((3),dtype=int)12foriinrange(3):13coorback[i]=(coor[i]-maxmat[i][1])/maxmat[i][2]14returncoorback
findxyz能输出你要找的区的编号。
coorback 返回一个三维整型：i,j,k，意思就是，你应该在三维空间里的第i，j，k格。
我们创立了一个deta*deta*deta的list，存的是序号。
中间多数格子是空的，因为我们是一个曲面，，，，稠密才是八太行的，虽然在空间里，稠密的曲面也是存在的，你化用皮阿诺曲线可得。
现在，我们可以把点集A的空间划分找到了。
然后我们把点集B，按A的标准，找每个点should在A划分里的哪一格。
再去B的对应格里，历遍，你做了这样的划分，你还不想历遍的话，我只想说，兄台有想法。。。你可以按八叉树的思想，划第二次。
EZ4ENCE ENCE ENCE~~~
如果只是这样的话，不见得work。
首先，这里默认了两个网格/点云同尺寸。
万一B比A要大可就毁了。
for i in range(3): maxmat2[i][0]+=20 maxmat2[i][1]-=20
所以你放大一下A的上下界不就好了。
我一开始，上界上取整，下界下取整也是这个意思。
万一A,B他不在一起也毁了。A在三墩镇，B是老和山。就算A再小，b范围再大，也不可能work啊。
1 def beitran(maxmat1,maxmat2,way): 2 #way=0:auto,need to do###way=1,changing mat1###way==2,changing mat2 3 mat1=np.zeros((3,4),dtype=float) 4 mat2=np.zeros((3,4),dtype=float) 5 mat111=np.zeros((4,2),dtype=float) #length, midpoint 6 mat222=np.zeros((4,2),dtype=float) 7 for i in range(3): 8 mat1[i][i]=1 9 mat2[i][i]=1 10 mat111[i][0]=maxmat1[i][0]-maxmat1[i][1] 11 mat111[i][1]=(maxmat1[i][0]+maxmat1[i][1])/2 12 mat222[i][0]=maxmat2[i][0]-maxmat2[i][1] 13 mat222[i][1]=(maxmat2[i][0]+maxmat2[i][1])/2 14 mat111[3][0]=(mat111[0][0]*mat111[1][0]*mat111[2][0])**(1/3) 15 mat222[3][0]=(mat222[0][0]*mat222[1][0]*mat222[2][0])**(1/3) 16 if way==0: 17 pass#wait for define and will choose m1m2 auto 18 if way==1: 19 for i in range(3): 20 mat1[i][i]=mat222[3][0]/mat111[3][0] 21 mat1[i][3]=mat222[i][1]-mat1[i][i]*mat111[i][1] 22 if way==2: 23 for i in range(3): 24 mat2[i][i]=mat111[3][0]/mat222[3][0] 25 mat2[i][3]=mat111[i][1]-mat2[i][i]*mat222[i][1] 26 return mat1,mat2 27 28 def beitranbei(maxmat,beimat,potnumber): 29 mat=np.zeros((3,3),dtype=float) 30 tran=np.zeros((3),dtype=float) 31 mat[:,:]=maxmat[:,0:3] 32 tran[:]=maxmat[:,3] 33 beimatnew=copy.deepcopy(beimat) 34 tranv=np.zeros((3),dtype=float) 35 for i in range(potnumber): 36 tranv=beimat[i][:] 37 tranx=np.dot(mat,tranv) 38 for j in range(3): 39 beimatnew[i][j]=tranx[j]+tran[j] 40 return beimatnew
我写这些玩意，在几个大项目里用，在我用手动调整之前，我用的是这个自动调整。
他可以解决，放缩，平移，但不能解决旋转。
way=1和2代表着A往B方向靠，和B往A方向靠的区别。
那个way=0的全自动，其实我很想将两个点集都丢在原点上。可以，但没必要。
这个放缩呢，也很迷，是按照三个尺度的比例来取均值：均值有很多种，算术平均，、平方平均。。。
因为没有更好的方法——你不可能绕过旋转去解决尺寸。
我还有一个工具。
原来，我先取随机子集，在做，这样每个点就有了双重索引——原集序号，子集序号。
在痛定思痛之后，我把这套东西扔光了，变成了这样的东西。
def potmatrandom(potmat,choose,deta): chooseset=1/choose potmatnew=[[[[] for i in range(deta)] for j in range(deta)] for k in range(deta)] for i in range(deta): for j in range(deta): for k in range(deta): for ijk in range(len(potmat[i][j][k])): t=random.random() if t<chooseset: potmatnew[i][j][k].append(potmat[i][j][k][ijk]) return potmatnew
deta是这个序号集的尺寸：deta*deta*deta
choose是多少取1，choose=5的话就是我有0.2的概率保留这个点。
需要指出的是，原来1000个点 ，choose=5，最后可能是200个点，也可能198或者220个。。。随机数的事情，谁说得好呢。反正多一个少一个，问题不大。
你自然可以在存序号的时候就随机。但是如果你可以有两个集——你甚至可以按1--1/2--1/4--1/8这种梯度来做多个集来平衡这个阶段的运算量和精度。
1 def detadealnew(deta,potnumber,beimat,maxmat): 2 intmatnew=np.zeros((potnumber,3),dtype=int) 3 potmat=[[[[] for i in range(deta)] for j in range(deta)] for k in range(deta)] 4 for i in range(potnumber): 5 coor=beimat[i,:] 6 coorback=icp.findxyz(coor,maxmat) 7 intmatnew[i,0:3]=coorback 8 potmat[int(coorback[0])][int(coorback[1])][int(coorback[2])].append(i) 9 return potmat,intmatnew
这里告诉你一个点集按照某一个划分标准（刚才说过的maxmat），的序号。
1 def nearestpoint(beimat1,beimat2,potnumber1,potmat2,maxmat2,deta2): 2 intmatnew=np.zeros((potnumber1,1),dtype=int) 3 floatmatnew=np.zeros((potnumber1,1),dtype=float) 4 for i in range(potnumber1): 5 #for i in range(5): 6 coor=beimat1[i,:] 7 8 coorback=icp.findxyz(coor,maxmat2) 9 #print(coor,":",i,"!!!",coorback) 10 dis=1000000 11 index=-1 12 for j in range(len(potmat2[int(coorback[0])][int(coorback[1])][int(coorback[2])])): 13 coor2=beimat2[int(potmat2[int(coorback[0])][int(coorback[1])][int(coorback[2])][j]),:] 14 disnew=icp.distance(coor,coor2) 15 if disnew<dis: 16 dis=disnew 17 index=j 18 #if dis<100: 19 #print("ab",i) 20 21 22 for i1 in range(3): 23 k1=coorback[0]-1+i1 24 for i2 in range(3): 25 k2=coorback[1]-1+i2 26 for i3 in range(3): 27 k3=coorback[2]-1+i3 28 k4=max(k1,k2,k3) 29 k5=min(k1,k2,k3) 30 if k4<deta2 and k5>=0: 31 for j in range(len(potmat2[k1][k2][k3])): 32 coor2=beimat2[int(potmat2[k1][k2][k3][j]),:] 33 #print(coor2,k1,k2,k3) 34 disnew=icp.distance(coor,coor2) 35 if disnew<dis: 36 dis=disnew 37 index=j 38 if dis<10000: 39 intmatnew[i][0]=index 40 floatmatnew[i][0]=dis 41 else: 42 #print(i) 43 for i1 in range(7): 44 k1=coorback[0]-3+i1 45 for i2 in range(7): 46 k2=coorback[1]-3+i2 47 for i3 in range(7): 48 k3=coorback[2]-3+i3 49 k4=max(k1,k2,k3) 50 k5=min(k1,k2,k3) 51 if k4<deta2 and k5>=0: 52 for j in range(len(potmat2[k1][k2][k3])): 53 coor2=beimat2[int(potmat2[k1][k2][k3][j]),:] 54 disnew=icp.distance(coor,coor2) 55 if disnew<dis: 56 dis=disnew 57 index=j 58 if dis<10000: 59 intmatnew[i][0]=index 60 floatmatnew[i][0]=dis 61 else: 62 intmatnew[i][0]=-1 63 floatmatnew[i][0]=0 64 if i/2500==i//2500: 65 print(i) 66 67 return intmatnew,floatmatnew 68 69 70 def distance(coor1,coor2): 71 dis=((coor1[0]-coor2[0])**2+(coor1[1]-coor2[1])**2+(coor1[2]-coor2[2])**2)**0.5 72 return dis
这里就是最近点的算法
理论上，我们最好按这样的顺序，依次寻找最近点——万一推定的格子里没有点呢？
上面是最好的方法，但是费行数。
我 就这么找了，你打我啊？
需要指出的是，一定要确认，你搜索的范围，没有超过你定义的网格区域。
我一共8*8*8，我找（0，7，2）的边上的（0，8，2）这格——就没有这格好嘛！
所以，序号不能到deta，也不能小于0。。。
我尝试过很多种数据存储格式，但是都费内存。
我现在用一个整型的数组和一个浮点数组。
整型数组存的最近点的序号。
浮点数组存的距离——但接着我们就会按序号找到xyz三坐标。
这是一种整合信息的尝试——之前的数据存法很头疼
##########number and means of every one #n: the dim of the X-set #p: the num of the select point #samjsq:number of sample #samzonejsq:number of sample zone(>0) #beimat :x,y,z for every point potnumber*3 #feimat :A,B,C for every triangle trinumber*3 #maxmat(int),maxmatfirst(float) i0=imax 3*2 #detamat :length of a part in deta 3 #potlist :potnumber in bei of ijk deta*deta*deta #potmat :sum(potmat)=potnumber deta*deta*deta #potsample :sample in bei of ijk deta*deta*deta #potsamlist : samjsq*5 i,j,k,beimat,samzonejsq #potex :zone ijk we have [4]~[4]+[3] samzonejsq*5 i.j.k,sum,start(in samjsq) #xeimat :all inf for sample samjsq*8 x,y,z,beimat,i,j,k,samzonejsq #samplezone :zone ijk inf deta*deta*deta*3 sum(sam),samjsq,sum(pot) #nearmatlist: deta*deta*deta nearest ijk(s) #nearpointfirst the n in xeimat2 samjsq(*1) #yeimat :all inf for sample samjsq*16 x1,y1,z1,x2,y2,z2;bei1;bei2,i1,j1,k1,szj1,i2,j2,k2,szj2 #lsjsq into samjsq #potjsq into samzonejsq
hhhhhhh我自己都很自闭。要命的是，虽然自闭但是自洽。。。
就是，它work，但你根本不知道该从何改起。
icp，tps都是我自己的函数文件。
1 import numpy as np 2 import cv2 3 import tps 4 import sys 5 import random 6 import xlrd 7 import math 8 import copy 9 import icp 10 11 ''' 12 f=open(r"C:\\Users\\28395\\Desktop\\tpstry\\3.ply",'r') 13 g=open(r"C:\\Users\\28395\\Desktop\\tpstry\\1.ply",'r') 14 [potnumber1,trinumber1,beimat1,feimat1,colmat1]=tps.freadold(f,2) 15 [potnumber2,trinumber2,beimat2,feimat2]=tps.fread(g,2) 16 17 beimat1=tps.xspinall(beimat1,potnumber1,2087,14450,8150,8189) 18 beimat2=tps.xspinall(beimat2,potnumber2,214919,353923,229469,314308) 19 maxmat1=icp.maxmatdeal(beimat1) 20 maxmat2=icp.maxmatdeal(beimat2) 21 [mat1,mat2]=icp.beitran(maxmat1,maxmat2,1) 22 beimat1=icp.beitranbei(mat1,beimat1,potnumber1) 23 maxmat1=icp.maxmatdeal(beimat1) 24 [matfix,tfix]=icp.handfix(0.1,0,0.1,0.9,-5,50,-30) 25 #[matfix,tfix]=icp.handfix(0,0,0.1,0.9,0,50,-30) 26 beimat1=icp.handfixbei(beimat1,potnumber1,matfix,tfix) 27 maxmat1=icp.maxmatdeal(beimat1) 28 for i in range(3): 29 maxmat2[i][0]+=20 30 maxmat2[i][1]-=20 31 32 fname1="11.ply" 33 fname2="22.ply" 34 fname1=tps.plywrite(fname1,beimat1,feimat1,potnumber1,trinumber1) 35 fname2=tps.plywrite(fname2,beimat2,feimat2,potnumber2,trinumber2) 36 ''' 37 f=open(r"C:\\Users\\28395\\Desktop\\tpstry\\11.ply",'r') 38 g=open(r"C:\\Users\\28395\\Desktop\\tpstry\\22.ply",'r') 39 [potnumber1,trinumber1,beimat1,feimat1]=tps.fread(f,2) 40 [potnumber2,trinumber2,beimat2,feimat2]=tps.fread(g,2) 41 maxmat1=icp.maxmatdeal(beimat1) 42 maxmat2=icp.maxmatdeal(beimat2) 43 for i in range(3): 44 maxmat2[i][0]+=20 45 maxmat2[i][1]-=20 46 47 48 49 50 51 print("end for plywrite") 52 deta2=32 53 choose2=5.5 54 maxmat2=icp.maxmatdeta(maxmat2,deta2) 55 56 #beimat: xyz *potnumber 57 #intmat: i,j,k,n in bei2 *potnumber 58 intmat1=np.zeros((potnumber1,7),dtype=int) 59 floatmat1=np.zeros((potnumber1,7),dtype=float) 60 61 [potmat2,intmatnew2]=icp.detadealnew(deta2,potnumber2,beimat2,maxmat2) 62 potmatnew2=icp.potmatrandom(potmat2,choose2,deta2) 63 [intmatnew1,floatmatnew1]=icp.nearestpoint(beimat1,beimat2,potnumber1,potmatnew2,maxmat2,deta2) 64 65 for i in range(potnumber1): 66 intmat1[i][6]=intmatnew1[i][0] 67 floatmat1[i][3:6]=beimat2[int(intmatnew1[i][0]),:] 68 intmat1[i][3:6]=intmatnew2[int(intmatnew1[i][0]),:] 69 floatmat1[i,0:3]=beimat1[i,:] 70 floatmat1[i][6]=floatmatnew1[i][0]
前面有大段注释。
嗯？那才是重要的！
我手动调教后，就不需要每次再调了！！！！
我直接引用那个调好的不就vans了么？
嘿嘿嘿！
发出了政委的笑声。