【matlab】计算两个点云之间的R和T

%算法理论根据 https://blog.csdn.net/u012836279/article/details/80203170 的博主提供 %输入pointAs 和 pointBs 要求两个点云的长度是一致，不一致将较长的截取一下即可 %输出R旋转矩阵，T平移矩阵 function [R,T] = RTbySVD(pointAs,pointBs) centerPointA=[0 0 0]; centerPointB=centerPointA; for i=1:4 centerPointA=centerPointA+pointAs(:,i); centerPointB=centerPointB+pointBs(:,i); end centerPointA=centerPointA/4;%求得了A的中心点 centerPointB=centerPointB/4;%求得了B的中心点 %step 02 消除T的影响，将点集合中心，生成A2和B2， pointA2s=pointAs; for i=1:4 pointA2s(:,i)=pointAs(:,i)-centerPointA; end pointB2s=pointBs; for i=1:4 pointB2s(:,i)=pointBs(:,i)-centerPointB; end %step 03计算A2 B2的协方差矩阵H %H=cov([pointA2s,pointB2s]);直接用这个生成的是16*16 不行，生成3*3 H=zeros(3,3); for i=1:4 H=H+pointA2s(:,i)*pointB2s(:,i)'; end [U,S,V]=svd(H); U=U'; R=V*U;%A点集合不受Zc收缩影响 %Great!!! you got it R=R*-1 T=R*centerPointA+centerPointB; end