One question about the translation vector (regParams.t):
I have the following 3 points before rotation:
A1 = (400, 400, 200,)
A2 = (600, 600, 200,)
A3 = (2000, 2000, 2000)
and after rotation:
B1 = (400, 400, 200,)
B2 = (600, 600, 200,)
B3 = (2000, 2000, 2001)
so all point are the same, just B3z is 2001 instead of 2000. Then the computed translation vector using your code is: (162, -76,4001)
What doesn't make sense to me? Because just one single point changed it's z-value from 2000 to 2001 what is just a difference of 1...?
Nevertheless, the result seems to be right, because:
b(:,1) = regParams.R * a(:,1) + regParams.t
is true...
Comment only