: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ƥȨֵ,ĬȨؾΪ1wӦֵԽ,ʾƥԽҪ
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)//سõrr0һ,tt0һ,s  s0һ