题目
已知:传感器坐标系(Body系)下有一平面P方程为Ax+By+Cz+D=0,简写为
[
n
D
]
∗
[
x
y
z
1
]
=
0
\begin{bmatrix} {n}&{D}\\\end{bmatrix}*\begin{bmatrix} {x}\\ {y}\\ {z}\\ {1}\\ \end{bmatrix} = 0
[nD]∗⎣⎢⎢⎡xyz1⎦⎥⎥⎤=0,其中n=[A B C];
另外传感器在世界坐标系(W系)下的位姿为
T
=
[
R
t
0
1
]
T=\begin{bmatrix} {R}&{t}\\ {0}&{1}\\ \end{bmatrix}
T=[R0t1]
求:平面P在W系下的方程
解法
[ n D ] ∗ [ T − 1 ] ∗ [ T ] [ x y z 1 ] = 0 \begin{bmatrix} {n}&{D}\\\end{bmatrix}* \begin{bmatrix} T^{-1}\\\end{bmatrix}* \begin{bmatrix} T\\\end{bmatrix} \begin{bmatrix} {x}\\ {y}\\ {z}\\ {1}\\ \end{bmatrix} = 0 [nD]∗[T−1]∗[T]⎣⎢⎢⎡xyz1⎦⎥⎥⎤=0
[ n D ] ∗ [ R T − R T t 0 1 ] ∗ [ T ] [ x y z 1 ] = 0 \begin{bmatrix} {n}&{D}\\\end{bmatrix}* \begin{bmatrix} R^{T}&-R^{T}t\\ 0&1\\\end{bmatrix}* \begin{bmatrix} T\\\end{bmatrix} \begin{bmatrix} {x}\\ {y}\\ {z}\\ {1}\\ \end{bmatrix} = 0 [nD]∗[RT0−RTt1]∗[T]⎣⎢⎢⎡xyz1⎦⎥⎥⎤=0
[ n R T − n R T t + D ] ∗ [ T ] [ x y z 1 ] = 0 \begin{bmatrix} {nR^{T}}&{-nR^{T}t+D}\\\end{bmatrix}* \begin{bmatrix} T\\\end{bmatrix} \begin{bmatrix} {x}\\ {y}\\ {z}\\ {1}\\ \end{bmatrix} = 0 [nRT−nRTt+D]∗[T]⎣⎢⎢⎡xyz1⎦⎥⎥⎤=0
[ n R T − n R T t + D ] ∗ [ x ′ y ′ z ′ 1 ] = 0 \begin{bmatrix} {nR^{T}}&{-nR^{T}t+D}\\\end{bmatrix}* \begin{bmatrix} {x^{'}}\\ {y^{'}}\\ {z^{'}}\\ {1}\\ \end{bmatrix} = 0 [nRT−nRTt+D]∗⎣⎢⎢⎡x′y′z′1⎦⎥⎥⎤=0
其中
[
x
′
y
′
z
′
1
]
\begin{bmatrix} {x^{'}}\\ {y^{'}}\\ {z^{'}}\\ {1}\\ \end{bmatrix}
⎣⎢⎢⎡x′y′z′1⎦⎥⎥⎤
为W系的基,因此W系下P的方程为
[
n
R
T
−
n
R
T
t
+
D
]
∗
[
x
′
y
′
z
′
1
]
=
0
\begin{bmatrix} {nR^{T}}&{-nR^{T}t+D}\\\end{bmatrix}* \begin{bmatrix} {x^{'}}\\ {y^{'}}\\ {z^{'}}\\ {1}\\ \end{bmatrix} = 0
[nRT−nRTt+D]∗⎣⎢⎢⎡x′y′z′1⎦⎥⎥⎤=0
推导完毕。