正交矩阵与齐次矩阵

正交矩阵

正交矩阵不用计算逆矩阵，计算转置矩阵就是它的逆矩阵

MM^T = I

因为MM^-1 = I

所以M^T = M^-1

检查正交矩阵

MM^T = I

M = [
  m11 m22 m33
  m21 m22 m23
  m31 m32 m33
]

// 将每一行用向量表示
r1 = [m11,m12,m13]
r2 = [m21,m22,m23]
r3 = [m31,m32,m33]

M = [
 r1,
 r2,
 r3
]   

M^T = [r1,r2,r3]

// 检查正交矩阵
r1 * r1 = 1 
r1 * r2 = 0
r1 * r3 = 0

r2 * r1 = 0
r2 * r2 = 1
r2 & r3 = 0

r3 * r1 = 0
r3 * r2 = 0
r3 * r3 = 1

矩阵正交化

修正矩阵计算误差，正交化修正

// 规范化
r1' = r1
r2' = r2 - (r1'r2)
r3' = r3 - (r1'r3)r1' - (r2'r3)r2'


r1' = r1-k(r1*r2)r2-k(r1*r3)r3  
r2' = r2-k(r1*r2)r1-k(r2*r3)r3  
r3' = r3-k(r1*r3)r1-k(r2*r3)r2    

齐次矩阵

利用齐次矩阵将线性变换矩阵与平移矩阵结合到一个矩阵中

[
 m11 m12 m13
 m21 m22 m23
 m31 m32 m33
]

[
 m11 m12 m13 0
 m21 m22 m23 0
 m31 m32 m33 0
 0   0  0   1
]

[x y z 1] * [
 m11 m12 m13 0
 m21 m22 m23 0
 m31 m32 m33 0
 0   0  0   1
]

= [
 xm11+y+m21+zm31
 xm12+y+m22+zm32
 xm13+y+m23+zm33
 1
]^T

平移矩阵

[x y z 1] * [
  1 0 0 0
  0 1 0 0
  0 0 1 0
  deltax deltay deltaz 1
]

[x+deltax y+deltay z+deltaz 1]

// 线性变换 + 平移

R = [
  r11 r12 r13 0
  r21 r22 r23 0
  r31 r32 r33 0
  0   0.  0.  1
]

T =  [
  1 0 0 0
  0 1 0 0
  0 0 1 0
  deltax deltay deltaz 1
]

M = RT =  [
  r11 r12 r13 0
  r21 r22 r23 0
  r31 r32 r33 0
  deltax deltay deltaz 1
]