七参数坐标转换 matlab,利用三点法求解空间坐标系转换所需的七参数
同一個物體的點云在不同坐標系中的坐標表示不同,有時候需要將其進行轉換到同一坐標系中進行處理,經典的方法為七參數法(7-parameters),用matlab實現如下:
%PS.好久不用matlab了,倍感親切啊啊啊啊啊啊啊啊!
%7參數——三點法求解三維坐標系轉換;
%
13.12.08 By Hupc
% xt=lambda*delta_xyz+lambda*r*xs;
%
clear
close all
clc
format long
% xs為原坐標系,xt為目標坐標系;
xs=[0.1833 -0.0478 -0.7653
0.1003 -0.0093
-0.8043
0.1049 0.0381
-0.8207
];
xt=[465.8400 -180.5000 -76.5700
476.6800 -166.9200
-47.2300
492.2800 -161.0300
-47.8300
];
A=[0.1833 -0.0478 -0.7653 465.8400 -180.5000 -76.5700
0.1003 -0.0093 -0.8043
476.6800 -166.9200 -47.2300
0.1049 0.0381 -0.82070
492.2800 -161.0300 -47.8300
];
A=[xs xt];% better;
for i=1:2
B(i,1)=sqrt((A(i,1)-A(i+1,1))^2+(A(i,2)-A(i+1,2))^2+(A(i,3)-A(i+1,3))^2);
B(i,2)=sqrt((A(i,4)-A(i+1,4))^2+(A(i,5)-A(i+1,5))^2+(A(i,6)-A(i+1,6))^2);
if
i+2<4
B(3,1)=sqrt((A(i,1)-A(i+2,1))^2+(A(i,2)-A(i+2,2))^2+(A(i,3)-A(i+2,3))^2);
B(3,2)=sqrt((A(i,4)-A(i+2,4))^2+(A(i,5)-A(i+2,5))^2+(A(i,6)-A(i+2,6))^2);
end
end
for i=1:3
B(i,4)=B(i,2)/B(i,1);
end
B(1,3)=(sum(B(1:3,4)))/3;
% 求解縮放系數;
p1=0.5*(B(1,1)+B(2,1)+B(3,1));
p2=0.5*(B(1,2)+B(2,2)+B(3,2));
Ap=sqrt(p1*(p1-B(1,1))*(p1-B(2,1))*(p1-B(3,1)));
Bp=sqrt(p2*(p2-B(1,2))*(p2-B(2,2))*(p2-B(3,2)));
B(2,3)=Bp/Ap;
lambda=B(1,3);% λ縮放系數;
for i=1:1
u2=-1*(lambda*(A(i,1)-A(i+1,1))+(A(i,4)-A(i+1,4)));
v2=-1*(lambda*(A(i,2)-A(i+1,2))+(A(i,5)-A(i+1,5)));
w2=-1*(lambda*(A(i,3)-A(i+1,3))+(A(i,6)-A(i+1,6)));
x2=(-1*lambda*(A(i,1)-A(i+1,1))+(A(i,4)-A(i+1,4)));
y2=(-1*lambda*(A(i,2)-A(i+1,2))+(A(i,5)-A(i+1,5)));
z2=(-1*lambda*(A(i,3)-A(i+2,3))+(A(i,6)-A(i+2,6)));
if
i+2<4
u3=-1*(lambda*(A(i,1)-A(i+2,1))+(A(i,4)-A(i+2,4)));
v3=-1*(lambda*(A(i,2)-A(i+2,2))+(A(i,5)-A(i+2,5)));
w3=-1*(lambda*(A(i,3)-A(i+2,3))+(A(i,6)-A(i+2,6)));
end
end
xabc1=inv([0 -1*w2 -1*v2;-1*w2 0 u2;v3 u3 0]);
delta_h=u3*v2*w2-u2*v3*w2;
xabc=delta_h*xabc1;
delta_x=[x2,y2,z2];
abc=(1/delta_h).*xabc*delta_x';
a=abc(1,1);
b=abc(2,1);
c=abc(3,1);
delta=1+a^2+b^2+c^2;
%r為旋轉矩陣;
r=(1/delta).*[1+a^2-b^2-c^2 -2*c-2*a*b -2*b+2*a*c
2*c-2*a*b 1-a^2+b^2-c^2
-2*a-2*b*c
2*b+2*a*c 2*a-2*b*c
1-a^2-b^2+c^2];
% delta_xyz(Δx、Δy、Δz)為平移矩陣;
delta_xyz=(1/lambda)*([A(1,4);A(1,5);A(1,6)])-r'*([A(1,1);A(1,2);A(1,3)]);
%if needed;
%xtt=([delta_xyz delta_xyz delta_xyz]+lambda*(r'*xs'))';
%err=xtt-xt;
%if needed;
% %transformation
%
cloud=textread('C:\Users\Administrator\Desktop\1.txt');
% n=length(cloud);
% for j=1:n
% ?cloudj=cloud(j,:)';
% ?result(j,:)=(lambda*delta_xyz+lambda*r*cloudj)';%result為cloud變換之后的結果;
% end
總結
以上是生活随笔為你收集整理的七参数坐标转换 matlab,利用三点法求解空间坐标系转换所需的七参数的全部內容,希望文章能夠幫你解決所遇到的問題。
- 上一篇: Go test 命令行参数
- 下一篇: matlab 画思维图像,「4」图像思维