基于MATLAB的点云配准程序

%funcio per determinar la transformacio Euclidea per Algorismes Genetics function [R,tf,P1,P2,er,comp_time]=chow(P1,P2,Ri,ti) c1=clock; n1=size(P1,2); n2=size(P2,2); min_n=min(n1,n2); %[Ri,ti]=regist_besl(P1,P2); [P1r,P2r]=acc_registration(P1,P2,Ri,ti); for i=1:min_n P1rt(:,i)=Ri*P1r(:,i)+ti; dis(i)=norm(P2r(1:3,i)-P1rt(1:3,i)); end error_inicial=median(dis); [rz,ry,rx]=R2rpy(Ri); cromosoma=[ti(1) ti(2) ti(3) rx ry rz]; error=error_inicial; fi=0; iteracio=0; iteracio_max=500; error_minim=10^17; iter_max=50; %Fem 50 iteracions sense trobar una soluci惱 m悶s bona iter=0; while fi==0 iteracio=iteracio+1; n_crom=size(cromosoma,1); if n_crom>1 %Podem fer cross-over, tenim mes dun cromosoma. Inicialment fem un sol cromosoma nou %%%%%%%% CROSS-OVER %%%%%%%%% n_cross_over=8; for i=1:n_cross_over %fprintf('realitzant cross-over\n') n_cros=3; for i=1:n_cros %escollim dos cromosomes n_c1=round(rand*n_crom); if n_c1==0 n_c1=1; elseif n_c1>n_crom; n_c1=n_crom; end n_c2=n_c1; while n_c2==n_c1 n_c2=round(rand*n_crom); if n_c2==0 n_c2=1; elseif n_c2>n_crom; n_c2=n_crom; end end %Escollim el nombre de gens a canviar n_gens=round(rand*5); if n_gens==0 n_gens=1; elseif n_gens>5; n_gens=5; end %Escollim els gens a canviar candidat=round(rand*6); if candidat==0 candidat=1; elseif candidat>5; candidat=5; end index_gens=candidat; while length(index_gens)<n_gens candidat=round(rand*6); if candidat==0 candidat=1; elseif candidat>5; candidat=5; end if min(abs(index_gens-candidat))>0 index_gens=[index_gens candidat]; end end %Substitucio dels diferents gens escollits nou_crom=cromosoma(n_c1,:); for i=1:n_gens nou_crom(index_gens(i))=cromosoma(n_c2,index_gens(i)); end R=rpy2R(nou_crom(6),nou_crom(5),nou_crom(4)); t=nou_crom(1:3)'; %[P1r,P2r]=registration(P1,P2,R,t); for i=1:min_n P1rt(:,i)=R*P1r(:,i)+t; dis(i)=norm(P2r(1:3,i)-P1rt(1:3,i)); end error_nou=median(dis); if norm(nou_crom-cromosoma(n_c1,:))>0.001 cromosoma=[cromosoma;nou_crom]; error=[error error_nou]; end end end end %%%%%%%% MUTATION %%%%%%%% %parametres MVt=.05; %si es tracta de translaccio MVr=.02; %si es tracta de rotacio % Decidim mutar tots els cromosomes n_crom=size(cromosoma,1); for i=1:n_crom %Decidim unicament fer una modificacio a un gen per cada mutacio n_gens=round(rand*6); if n_gens==0 n_gens=1; elseif n_gens>6; n_gens=6; end nou_crom=cromosoma(i,:); if n_gens<=3 inc=rand*2*MVt-MVt; %fprintf('Mutacio d`un parametre de translacio\n') else inc=rand*2*MVr-MVr; %fprintf('Mutacio d`un parametre de rotacio\n') end nou_crom(n_gens)=nou_crom(n_gens)+inc*error(i); R=rpy2R(nou_crom(6),nou_crom(5),nou_crom(4)); t=nou_crom(1:3)'; %[P1r,P2r]=registration(P1,P2,R,t); for i=1:min_n P1rt(:,i)=R*P1r(:,i)+t; dis(i)=norm(P2r(1:3,i)-P1rt(1:3,i)); end error_nou=median(dis); cromosoma=[cromosoma;nou_crom]; error=[error error_nou] ; end % Eliminem les files repetides index_eli=[]; for i=1:length(error) for j=1:length(error) if i~=j if length(find(cromosoma(i,:)==cromosoma(j,:)))==length(cromosoma(i,:)) %Guardem els indexs a eliminar; candidat=max(i,j); if length(index_eli)==0 index_eli=candidat; else if min(abs(index_eli-candidat))>0 index_eli=[index_eli candidat]; end end end end % cromosoma=reduc_row(cromosoma,j); % error=reduc_row(error',j)'; end end [val_e,index_e]=sort(index_eli); n_eli=length(index_eli); if n_eli>0 for i=n_eli:-1:1 cromosoma=reduc_row(cromosoma,val_e(i)); error=reduc_row(error',val_e(i))'; end end % A continuacio cal eliminar algunes mutacions per que no creixin massa. % [val,index]=sort(error); % n_escollits=50; % error_nou=[]; % %length(error) % cromosoma_nou=[]; % if length(error)>n_escollits+1 % for i=1:n_escollits % cromosoma_nou(i,:)=cromosoma(index(i),:); % error_nou(i)=val(i); % end % cromosoma_nou(n_escollits+1,:)=cromosoma(index(size(cromosoma,1)),:); % error_nou(n_escollits+1)=val(size(cromosoma,1)); % error=error_nou; % cromosoma=cromosoma_nou; % end % seleccio de la poblacio utilitzat per Brunnstrom, pero % cal canviar, ja que brunnstrolm maximitza la funcio, i % nosaltres la minimitzem %val_f n_escollits=50; n_crom=size(cromosoma,1); for i=1:n_crom if error(i)==0 error2(i)=10^25; else error2(i)=1/error(i); end end [val_f,index_f]=sort(error2); % Ordenem els valors de la funcio de fitness % size(cromosoma) % size(error2) % max(index_f) % pause if n_crom>n_escollits sum_F=sum(val_f); for i=1:n_escollits-1 %per cada iteracio escollim un element amb probabilitat f(i)/sum_F , tambe escollim obligatoriament el millor ind_popu=rand*sum_F; fi2=0; iter=1; while fi2==0 ind_popu=ind_popu-val_f(iter); if ind_popu<0 fi2=1; else iter=iter+1; end if iter>length(index_f) iter=length(index_f); end % index_f(iter) if index_f(iter)>size(cromosoma,2); index_f(iter)=size(cromosoma,2); end C_nou(i,:)=cromosoma(index_f(iter),:); f_nou(i)=error(index_f(iter)); end end cromosoma=[C_nou;cromosoma(index_f(n_crom),:)]; error=[f_nou error(index_f(n_crom))]; C_nou=[];f_nou=[]; error2=[]; n_crom=size(cromosoma,1); %numero de cromosomes end if min(error)<error_minim error_minim=min(error); iter2=0; else