f_produce_cluster2.m

多目标跟踪 (三维)-算法matlab编制

function [cl_total,cl_element_total,cl_matrix]=f_produce_cluster2(exist_target,exist_target_total,u);%聚形成
xtag=zeros(exist_target_total,exist_target_total);
ytag=xtag;
ztag=xtag;
tag=linspace(0,0,exist_target_total);%目标是否已归入某聚标志
if (exist_target_total<=0)
    exit;%目标数为非正数，则结束该程序
end
cl_total=1;%有目标存在，则聚总数至少为1
kk=1; %kk为后续程序作准备，总是指尚未归入任何聚的目标中的第一个的编号
for i=1:exist_target_total
    if i~=1
        for j=2:exist_target_total
            if tag(j)==0
                cl_total=cl_total+1; %若全部遍历一遍以后还有未归入任何聚的目标，则新开一个聚
                kk=j; 
                break; %跳出j
            end
        end %end j
    end
    i=kk;
    if tag(i)==0
        cl_belong(i)=cl_total; %聚编码
        cl(cl_total).vector=i;%将第i个目标（未归入任何聚的第一个目标）归入该聚
        tag(i)=1;
        temp2=[]; %放遍历前的该聚在该层的元素个数
        temp2=size(cl(cl_total).vector);
        for j=kk:exist_target_total %遍历剩下所有的未归入任何聚的目标
                if tag(j)==0
                    if (u(1,j)<=u(2,i)&u(2,i)<=u(2,j))| (u(1,i)<=u(2,j)&u(2,j)<=u(2,i))
                        xtag(i,j)=1;
                    end
                    if (u(4,j)<=u(5,i)&u(5,i)<=u(5,j))| (u(4,i)<=u(5,j)&u(5,j)<=u(5,i))
                        ytag(i,j)=1;
                    end
                    if (u(7,j)<=u(9,i)&u(9,i)<=u(9,j))| (u(7,i)<=u(9,j)&u(9,j)<=u(9,i))
                        ztag(i,j)=1;
                    end
                    if (xtag(i,j)*ytag(i,j)*ztag(i,j))==1
                        tag(j)=1;
                        cl_belong(j)=cl_belong(i);%目标i属于目标j所在的聚
                        cl(cl_belong(i)).vector=[cl(cl_belong(i)).vector j]
                        temp4=[];
                        temp4=[temp4,j];
                    end
                end %end tag(j)==0
        end  %end j
        
        %如果有与该层的任何一个目标跟踪门相交的，则继续下一层的遍历 
        temp3=[];
        temp3=size(cl(cl_total).vector);
        while (temp3(2)~=temp2(2))
            temp2=temp3;
            temp5=size(temp4);
            temp6=[];
            for k=1;temp5(2)  %遍历该层的所有目标，找出与这些目标相交的所有其他目标
                for j=kk:exist_target_total %对该层的每个目标遍历剩下的未归入任何聚的目标
                    if tag(j)==0
                        if (u(1,j)<=u(2,temp4(k))&u(2,temp4(k))<=u(2,j))| (u(1,temp4(k))<=u(2,j)&u(2,j)<=u(2,temp4(k)))
                            xtag(temp4(k),j)=1;
                        end
                        if (u(4,j)<=u(5,temp4(k))&u(5,temp4(k))<=u(5,j))| (u(4,temp4(k))<=u(5,j)&u(5,j)<=u(5,temp4(k)))
                            ytag(temp4(k),j)=1;
                        end
                        if (u(7,j)<=u(9,temp4(k))&u(9,temp4(k))<=u(9,j))| (u(7,temp4(k))<=u(9,j)&u(9,j)<=u(9,temp4(k)))
                            ztag(temp4(k),j)=1;
                        end
                        if (xtag(temp4(k),j)*ytag(temp4(k),j)*ztag(temp4(k),j)==1)
                            tag(j)=1;
                            cl_belong(j)=cl_belong(i);%目标temp4(k)属于目标j所在的聚,而目标temp4(k)都是和目标i同聚
                            cl(cl_belong(i)).vector=[cl(cl_belong(i)).vector j];
                            temp6=[temp6,j];
                        end
                    end %end tag(j)==0
                end  %end j
            end %end k，结束对该层目标的查找
            temp3=size(cl(cl_total).vector);
            temp4=temp6;
        end %end while
        
    end %end tag(i)=0
end %end i

cl_element_total=[];
for i=1:cl_total
    temp1=size(cl(i).vector);
    cl_element_total(i)=temp1(2);%每个聚中的元素个数
end
for i=1:cl_total
    for j=1:cl_element_total(i)
        temp=cl(i).vector(j);
        cl(i).vector(j)=exist_target(temp);%映射到全局编号
    end
end
max_length=max(cl_element_total);
cl_matrix=[];
for i=1:cl_total
    temp=cl(i).vector;
    for j=cl_element_total(i)+1:max_length
        temp=[temp 0];
    end
    cl_matrix=[cl_matrix;temp];%按行存放所有的聚向量，每行代表一个聚中的所有目标编号，不足长度添零补齐
end