calculate_transformation.m

% pts1 = [-97.7589, 107.423, 424;
%     92.029, 110.208, 432;
%     73.7443, 20.5885, 427;
%     -75.4565, 17.4702, 424;
%     13.7856, 64.4569, 425];
% 
% pts2 = [-37.0552, -124.662, -411.111;
%     -28.2589, 87.5111, -421.536;
%     -123.679, 63.8717, -417.216;
%     -132.088, -101.241, -408.636;
%     -73.1458, -2.91322, -414.225];

pts1 = [-7.25706, -31.4278, 643.5;
    36.8112, -30.0153, 646;
    53.801, 8.4949, 646;
    94.5564, -12.6477, 687;
    28.8827, 26.6174, 646];

pts2 = [29.076, -15.785, -147.635;
    -18.8262, -20.2719, -148.67;
    -38.2911, -16.4062, -104.167;
    -82.3088, 16.3664, -129.354;
    -9.21941, -21.092, -82.9692];

% figure(1);
% plot3(pts1(:,1), pts1(:,2), pts1(:,3), '-*');
% xlabel('x'); ylabel('y'); zlabel('z');
% text(pts1(1,1), pts1(1,2), pts1(1,3), '1');
% axis equal;
% figure(2);
% plot3(pts2(:,1), pts2(:,2), pts2(:,3), '-*');
% xlabel('x'); ylabel('y'); zlabel('z');
% text(pts2(1,1), pts2(1,2), pts2(1,3), '1');
% axis equal;

pts1t = pts1';
pts2t = pts2';

% http://nghiaho.com/?page_id=671
centroid1 = mean(pts1t, 2);
centroid2 = mean(pts2t, 2);

pts1tzero = pts1t - repmat(centroid1, 1, size(pts1t, 2));
pts2tzero = pts2t - repmat(centroid2, 1, size(pts2t, 2));


% figure(3);
% plot3(pts1tzero(1,:), pts1tzero(2,:), pts1tzero(3,:), '-*');
% xlabel('x'); ylabel('y'); zlabel('z');
% text(pts1tzero(1,1), pts1tzero(2,1), pts1tzero(3,1), '1');
% hold on
% plot3(pts2tzero(1,:), pts2tzero(2,:), pts2tzero(3,:), '-*');
% xlabel('x'); ylabel('y'); zlabel('z');
% text(pts2tzero(1,1), pts2tzero(2,1), pts2tzero(3,1), '1');
% axis equal;
% hold off;

H = zeros(3, 3);
for i = 1:size(pts1t, 2)
    H = H + pts1tzero(:,i) * pts2tzero(:,i)';
end

[U, S, V] = svd(H);
R = V * U
t = - R * centroid1 + centroid2

T = [R, t; 0, 0, 0, 1]
T_inv = inv(T)

mapped1 = R * pts1t + repmat(t, 1, size(pts1t,2))
% mapped1 = T * [pts1t; ones(1, size(pts1t,2))]

figure(3);
plot3(mapped1(1,:), mapped1(2,:), mapped1(3,:), '-*');
xlabel('x'); ylabel('y'); zlabel('z');
text(mapped1(1,1), mapped1(2,1), mapped1(3,1), '1');
hold on
plot3(pts2t(1,:), pts2t(2,:), pts2t(3,:), '-*');
xlabel('x'); ylabel('y'); zlabel('z');
text(pts2t(1,1), pts2t(2,1), pts2t(3,1), '1');
axis equal;
hold off;
legend('1', '2');