照相机定位matlab代码

%data.mclearall;clc;globaldat;A=imread('picture.bmp','bmp');dim=size(A);dat=zeros(dim(1),dim(2));dat=dat+1;fori=1:dim(1)forj=1:dim(2)ifA(i,j)==255dat(i,j)=0;elsedat(i,j)=1;endendendimshow(dat)savedata1dat;%求轮廓bound;a=bound;dim=size(a);zb_bound={};k=1;h=0;whileh==0m=[];fori=1:dim(1)-1forj=1:dim(2)-1ifa(i,j)==1break;endendifa(i,j)==1break;endendifi==768&j==1024h=1;break;elsehh=0;whilehh==0a(i,j)=0;m=[m;i,j];ifa(i-1,j)==1i=i-1;j=j;elseifa(i+1,j)==1i=i+1;j=j;elseifa(i,j+1)==1i=i;j=j+1;elseifa(i,j-1)==1i=i;j=j-1;elseifa(i-1,j-1)==1i=i-1;j=j-1;elseifa(i-1,j+1)==1i=i-1;j=j+1;elseifa(i+1,j-1)==1i=i+1;j=j-1;elseifa(i+1,j+1)==1i=i+1;j=j+1;elsehh=1;endendendfori=1:dim(1)-1forj=1:dim(2)-1ifa(i,j)==1ifa(i-1,j)==0&a(i+1,j)==0&a(i,j-1)==0&a(i,j+1)==0&a(i-1,j+1)==0&a(i-1,j-1)==0&a(i+1,j+1)==0&a(i+1,j-1)==0a(i,j)=0;endendendendzb_bound=[zb_bound;{m}];end%画轮廓%plot.m%bound;%imshow(bound);%holdon;%a1a2a3a4a5分别是五个圆的边界坐标figure(1);plot(a1(:,1),a1(:,2));%画第一个椭圆的边界；holdon;plot(a2(:,1),a2(:,2));%画第2个椭圆的边界；plot(a3(:,1),a3(:,2));%画第3个椭圆的边界；plot(a4(:,1),a4(:,2));%画第4个椭圆的边界；plot(a5(:,1),a5(:,2));%画第5个椭圆的边界；ticclc;%求公切线及切点p1=xlsread('1.xls');p2=xlsread('2.xls');p3=xlsread('3.xls');p4=xlsread('4.xls');p5=xlsread('5.xls');%bound;%imshow(bound);bound=imread('picture.bmp','bmp');imshow(bound);holdon;%cicle1&cicle3111%oneofthecommontangent[min1,i]=min(p1(:,1));qd(1,1)=i;[min3,i]=min(p3(:,1));qd(1,2)=i;size1=size(p1);size3=size(p3);flag=1;whileflag==1flag=0;fori=1:size1(1)ifp1(i,1)(p1(qd(1,1),1)-p3(qd(1,2),1))/(p1(qd(1,1),2)-p3(qd(1,1),2))*(p1(i,2)-p1(qd(1,1),2))+p1(qd(1,1),1)qd(1,1)=i;flag=1;break;endendfori=1:size3(1)ifp3(i,1)(p1(qd(1,1),1)-p3(qd(1,2),1))/(p1(qd(1,1),2)-p3(qd(1,1),2))(p3(i,2)-p1(qd(1,1),2))+p1(qd(1,1),1)qd(1,2)=i;break;endendendx=1:0.5:1024;y=(p1(qd(1,1),1)-p3(qd(1,2),1))/(p1(qd(1,1),2)-p3(qd(1,2),2))*(x-(p1(qd(1,1),2)))+p1(qd(1,1),1);plot(x,y,':')holdon;%theothercommontangent另一条公切线[max1i]=max(p1(:,1));qd(1,3)=i;[max3i]=max(p3(:,1));qd(1,4)=i;size1=size(p1);size3=size(p3);flag=1;whileflag==1flag=0;fori=1:size1(1)ifp1(i,1)(p1(qd(1,3),1)-p3(qd(1,4),1))/(p1(qd(1,3),2)-p3(qd(1,4),2))*(p1(i,2)-p1(qd(1,3),2))+p1(qd(1,3),1)qd(1,3)=i;flag=1;break;endendfori=1:size3(1)ifp3(i,1)(p1(qd(1,3),1)-p3(qd(1,4),1))/(p1(qd(1,3),2)-p3(qd(1,4),2))*(p3(i,2)-p1(qd(1,3),2))+p1(qd(1,3),1)qd(1,4)=i;flag=1;break;endendendx=1:0.5:1024;y=(p1(qd(1,3),1)-p3(qd(1,4),1))/(p1(qd(1,3),2)-p3(qd(1,4),2))*(x-(p1(qd(1,3),2)))+p1(qd(1,3),1);plot(x,y,':')holdon;%circle1&circle42222[min1,i]=min(p1(:,1));qd(2,1)=i;[min4,i]=min(p4(:,1));qd(2,2)=i;size1=size(p1);size4=size(p4);flag=1;whileflag==1flag=0;fori=1:size1(1)ifp1(i,1)(p1(qd(2,1),1)-p4(qd(2,2),1))/(p1(qd(2,1),2)-p4(qd(2,2),2))*(p1(i,2)-p1(qd(2,1),2))+p1(qd(2,1),1)qd(2,1)=i;flag=1;break;endendfori=1:size4(1)ifp4(i,1)(p1(qd(2,1),1)-p4(qd(2,2),1))/(p1(qd(2,1),2)-p4(qd(2,2),2))*(p4(i,2)-p1(qd(2,1),2))+p1(qd(2,1),1)qd(2,2)=i;flag=1;break;endendendx=1:0.5:1024;y=(p1(qd(2,1),1)-p4(qd(2,2),1))/(p1(qd(2,1),2)-p4(qd(2,2),2))*(x-(p1(qd(2,1),2)))+p1(qd(2,1),1);plot(x,y,':')holdon;%theothercommontangent[min1,i]=max(p1(:,1));qd(2,3)=i;[min4,i]=max(p4(:,1));qd(2,4)=i;size1=size(p1);size4=size(p4);flag=1;whileflag==1flag=0;fori=1:size1(1)ifp1(i,1)(p1(qd(2,3),1)-p4(qd(2,4),1))/(p1(qd(2,3),2)-p4(qd(2,4),2))*(p1(i,2)-p1(qd(2,3),2))+p1(qd(2,3),1)qd(2,3)=i;flag=1;break;endendfori=1:size4(1)ifp4(i,1)(p1(qd(2,3),1)-p4(qd(2,4),1))/(p1(qd(2,3),2)-p4(qd(2,4),2))*(p4(i,2)-p1(qd(2,3),2))+p1(qd(2,3),1)qd(2,4)=i;flag=1;break;endendendx=1:0.5:1024;y=(p1(qd(2,3),1)-p4(qd(2,4),1))/(p1(qd(2,3),2)-p4(qd(2,4),2))*(x-(p1(qd(2,3),2)))+p1(qd(2,3),1);plot(x,y,':')holdon;%circle4&circle53333[min4,i]=min(p4(:,1));qd(3,1)=i;[min5,i]=min(p5(:,1));qd(3,2)=i;size4=size(p4);size5=size(p5);flag=1;whileflag==1flag=0;fori=1:size4(1)ifp4(i,1)(p4(qd(3,1),1)-p5(qd(3,2),1))/(p4(qd(3,1),2)-p5(qd(3,2),2))*(p4(i,2)-p4(qd(3,1),2))+p4(qd(3,1),1)qd(3,1)=i;flag=1;break;endendfori=1:size5(1)ifp4(i,1)(p4(qd(3,1),1)-p4(qd(3,2),1))/(p4(qd(3,1),2)-p5(qd(3,2),2))*(p5(i,2)-p4(qd(3,1),2))+p4(qd(3,1),1)qd(3,2)=i;flag=1;break;endendendx=1:0.5:1024;y=(p4(qd(3,1),1)-p5(qd(3,2),1))/(p4(qd(3,1),2)-p5(qd(3,2),2))*(x-(p4(qd(3,1),2)))+p4(qd(3,1),1);plot(x,y,':')holdon;%theothercommontangent[min4,i]=max(p4(:,1));qd(3,3)=i;[min5,i]=max(p5(:,1));qd(3,4)=i;size1=size(p4);size4=size(p5);flag=1;whileflag==1flag=0;fori=1:size4(1)ifp4(i,1)(p4(qd(3,3),1)-p5(qd(3,4),1))/(p4(qd(3,3),2)-p5(qd(3,4),2))*(p4(i,2)-p4(qd(3,3),2))+p4(qd(3,3),1)qd(3,3)=i;flag=1;break;endendfori=1:size5(1)ifp5(i,1)(p4(qd(3,3),1)-p5(qd(3,4),1))/(p4(qd(3,3),2)-p5(qd(3,4),2))*(p5(i,2)-p4(qd(3,3),2))+p4(qd(3,3),1)qd(3,4)=i;flag=1;break;endendendx=1:0.5:1024;y=(p4(qd(3,3),1)-p5(qd(3,4),1))/(p4(qd(3,3),2)-p5(qd(3,4),2))*(x-(p4(qd(3,3),2)))+p4(qd(3,3),1);plot(x,y,':')holdon;%circle3&circle54444[min3,i]=min(p3(:,1));qd(4,1)=i;[min5,i]=min(p5(:,1));qd(4,2)=i;size3=size(p3);size5=size(p5);flag=1;whileflag==1flag=0;fori=1:size3(1)ifp3(i,1)(p3(qd(4,1),1)-p5(qd(4,2),1))/(p3(qd(4,1),2)-p5(qd(4,2),2))*(p3(i,2)-p3(qd(4,1),2))+