DLT.rar_close form 求旋转矩阵_closeform 求单位四元数_dlt ao

clc; load xy.mat; num=size(x,2); A=zeros(4,4); X=zeros(4,4); Y=zeros(4,4); for i=1:num xi=[0;x(1,i);x(2,i);x(3,i)]; yi=[0;y(1,i);y(2,i);y(3,i)]; X(:,i)=xi; Y(:,i)=yi; yi_L=[yi(1,1),-yi(2,1),-yi(3,1),-yi(4,1); yi(2,1),yi(1,1),-yi(4,1),yi(3,1); yi(3,1),yi(4,1),yi(1,1),-yi(2,1); yi(4,1),-yi(3,1),yi(2,1),yi(1,1)]; xi_R=[xi(1,1),-xi(2,1),-xi(3,1),-xi(4,1); xi(2,1),xi(1,1),xi(4,1),-xi(3,1); xi(3,1),-xi(4,1),xi(1,1),xi(2,1); xi(4,1),xi(3,1),-xi(2,1),xi(1,1)]; A=A+yi_L'*xi_R; end [Q,lamda]=eig(A); [m,n]=find(max(max(lamda))); %求得四元数 q=Q(:,m); %计算误差 qv=q(2:4,1); q_t=[q(1,1);-qv]; tmp1=zeros(4,4); tmp=zeros(3,3); for k=1:num tmp1(:,k)=new_multiply(new_multiply(q,X(:,k)),q_t); tmp(:,k)=tmp1(2:4,k); end tmp=y-tmp; error=sum(sum(tmp.*tmp,1))/m; disp('直接法求四元数的误差为：'); disp(error); %对原始数据加高斯噪声 sigma=1; %方差，可修改 x_add=randn(3,m)*sigma+x; y_add=randn(3,m)*sigma+y; X_add=zeros(4,4); Y_add=zeros(4,4); for i=1:num xi_add=[0;x(1,i);x(2,i);x(3,i)]; yi_add=[0;y(1,i);y(2,i);y(3,i)]; X_add(:,i)=xi_add; Y_add(:,i)=yi_add; end for k=1:num tmp1(:,k)=new_multiply(new_multiply(q,X_add(:,k)),q_t); tmp(:,k)=tmp1(2:4,k); end tmp=y_add-tmp; error_new=sum(sum(tmp.*tmp,1))/m; disp('对原始数据加均值为0的高斯噪声，方差设为：'); disp(sigma); disp('误差为：'); disp(error_new);