以前、お答えした内容のデモで特異値分解(SVD)を利用して位置計算をしています。
findRtFromRGBD.m の内容をご確認ください。
%==========================================================================
% Solve the following minimization problem:
% min_{R, T} sum(|R*p+T-q|^2)
%
% p, q are all N-by-d matrix with N data points
%
% The problem is solved by SVD
%==========================================================================
function [R, T] = minimizePointToPointMetric(p, q)
n = size(p, 1);
m = size(q, 1);
% Find data centroid and deviations from centroid
pmean = sum(p,1)/n;
p2 = p - repmat(pmean, n, 1);
qmean = sum(q,1)/m;
q2 = q - repmat(qmean, m, 1);
% Covariance matrix
C = p2'*q2;
[U,~,V] = svd(C);
% Handle the reflection case
R = V*diag([ones(1,size(C,1)-1) sign(det(U*V'))])*U';
% Compute the translation
T = qmean' - R*pmean';
