Get Rotation and Translation from 2 groups of 3d points (calculate R, T between 2 points.)

I made this example source code referencing from this site.
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


    
    
    



...

Comments

Popular posts from this blog

OpenCV Stitching example (Stitcher class, Panorama)

(OpenCV Study) Background subtractor MOG, MOG2, GMG example source code (BackgroundSubtractorMOG, BackgroundSubtractorMOG2, BackgroundSubtractorGMG)

Example source code of extract HOG feature from images, save descriptor values to xml file, using opencv (using HOGDescriptor )

Real-time N camera stitching Class.

8 point algorithm (Matlab source code) / The method to get the Fundamental Matrix and the Essential matrix

Optical Flow sample source code using OpenCV

Video Stabilization example source code, (using cvFindHomography, cvWarpPerspective functions in openCV)

(OpenCV Study) calcOpticalFlowFarneback example source code ( dense optical flow )

yuv422(YUYV) to RGB and RGB to yuv422(YUYV), (Using OpenCV and TBB)

(C, C++) TinyXML , xml read & write