http://nghiaho.com/?page_id=671
Key idea(main processing) is using SVD(singular Value Decomposition)
I made first group consist of 3d points by random selection.
Second groups of 3d points is made by random rotation and translation.
Blue color is points of second group.
How to find R,T between 2 groups of 3d points ?
for detail, see below matlab source code.
After get R,T, second group can transform to original position.
matlab source code.
printf
%Get R,T from 2 groups of 3d points
%The first group is created by random selection
A3pt = rand(3, 10);
figure(10);
plot3(A3pt(1,:),A3pt(2,:),A3pt(3,:),'r.');%, axis equal
%The second group is made by random R,T from first group
v1=0.6*(2*rand-1);
v2=0.6*(2*rand-1);
v3=0.6*(2*rand-1);
R1=[1 0 0;0 cos(v1) -sin(v1);0 sin(v1) cos(v1)];
R2=[cos(v2) 0 sin(v2);0 1 0;-sin(v2) 0 cos(v2)];
R3=[cos(v3) -sin(v3) 0;sin(v3) cos(v3) 0;0 0 1];
R=R3*R2*R1;
T = rand(3,1);
B3pt = R*A3pt; %Rotation
for i=1:3 %dimension
B3pt(i,:)=B3pt(i,:)+T(i); % translation
end
%show 2 group
figure(1);
plot3(A3pt(1,:),A3pt(2,:),A3pt(3,:),'r.',B3pt(1,:),B3pt(2,:),B3pt(3,:),'bo');%, axis equal
%% get R,T
MeanA = mean(A3pt, 2);
MeanB = mean(B3pt, 2);
HH=zeros(3,3);
n = length(A3pt);
for i=1:n
tA = A3pt(:,i) - MeanA;
tB = B3pt(:,i) - MeanB;
hh = tB * tA';
HH = HH + hh;
end
[U,~,V]=svd(HH);
Ri=V*U'; %get R
Ti=MeanA-Ri*MeanB; %Get T
%% confirm
B3pt_=Ri*B3pt; % Rotation μν€κΈ° Apply transformation
for i=1:3 %dimension
B3pt_(i,:)=B3pt_(i,:)+Ti(i); % translation μν€κΈ°
end
%show 2 group
figure(2);
plot3(A3pt(1,:),A3pt(2,:),A3pt(3,:),'r.',B3pt_(1,:),B3pt_(2,:),B3pt_(3,:),'bo');%, axis equal
...
No comments:
Post a Comment