功能:求解3D点云的刚体变换

格式:
[R,t,s] = SLAM_RigidTransformation(p1,p2,w,s0)
[R,t,s] = SLAM_RigidTransformation(p1,p2,w)
[R,t,s] = SLAM_RigidTransformation(p1,p2)

p1:为一个m*3的矩阵变量,存储参照对象的空间位置,每一行代表一个点的坐标
p2:为一个m*3的矩阵变量,存储变换对象的空间位置,每一行代表一个点的坐标,其中每一行与p1的每一行一一对应
w :为一个m*1的矩阵变量,存储每个匹配对的权重,每一行的值对应p1,p1匹配的权重值,默认所有权重均为1。w对应的值越大,表示此匹配越重要。
s0:为指定的缩放系数,当此参数为一个数值时,表示缩放系数为当前值;当本参数为">"时,表示求解的缩放系数必须大于0;当本参数为空时,表示本参数可正可负

R:返回的3*3刚体变换旋转矩阵
t:返回的3*1刚体变换平移矩阵
s:返回的缩放系数

原理:{3D点云配准之刚体变换<http://www.mathsword.com/aligmentcloudpoint/>}

说明:
1、本函数主要求解 p2 = s * R * p1 + t 这个问题,其中R为旋转矩阵,s为缩放系数,t是求解的偏移,p1,p2已知
2、注意本函数是求解刚度变换矩阵,而不是对数据进行刚度变换

例子:
p1 = rand(10,3,100);
v  = rand(1,1,3);
r0 = {SLAM_VToR<矩阵运算\SLAM_VToR>}(v);
r0 = {Build<矩阵运算\Build>}(r0,3,3);
t0 = [1,2,3];
s0 = 0.5 ;
d  = {Duplicate<矩阵运算\Duplicate>}(t0,100);
p2 = {mul<矩阵运算\mul>}(r0,p1);
p2 = s0 *p2;
p2 = p2';
p2 = p2 + d;
p1 = p1';
//上面的代码随机产生一组p1,p2

[r,t,s] = SLAM_RigidTransformation(p1,p2)//回车得到的r与r0一致,t与t0一致,s 与 s0一致