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

基于博主 https://blog.csdn.net/u012836279/article/details/80203170的文章实现的matlab版 test.m 利用给定R1T1矩阵，生成两个点云，在通过调用RTbySVD脚本计算R2T2，对比R1T1，R2T2得到正确性评估 RTbySVD.m基于上述博文实现的核心代码 代码诸多不足~敬请指正

%算法理论根据 https://blog.csdn.net/u012836279/article/details/80203170 的博主提供
%输入pointAs 和 pointBs  要求两个点云的长度是一致，不一致将较长的截取一下即可
%输出R旋转矩阵，T平移矩阵
function [RT] = RTbySVD（pointAspointBs）
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（[pointA2spointB2s]）;直接用这个生成的是16*16 不行，生成3*3
H=zeros（33）;
for i=1:4
    H=H+pointA2s（:i）*pointB2s（:i）‘;
end
[USV]=svd（H）;
U=U‘;
R=V*U;%A点集合不受Zc收缩影响
%Great!!! you got it
R=R*-1
T=R*centerPointA+centerPointB;




end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        1004  2019-03-13 19:06  TbySVD.m
     文件         539  2019-03-13 19:03  test.m