经过了一段时间的研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的意思是,还有空间之外的部分——没有点的部分。
三列里知道两个,就足以找到你想要的篮子了。
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了么?
嘿嘿嘿!
发出了政委的笑声。