三维空间刚体变换
抱歉，由于markdown兼容问题有部分公式无法转换出来，可复制到typora下。
旋转矩阵和变换矩阵
在三位空间当中，研究相机📷的运动我们不仅要考虑它的位置（x,y,z）也要考虑它的姿态(朝向)，对此我们定义位姿来描述相机📷在三维空间的位置姿态。
那么问题来了我们怎么来描述位姿呢？向量──在相机座标系下，相机的朝向可以用一组相互正交基底
(
e
1
⃗
,
e
2
⃗
,
e
3
⃗
)
(\vec{e_1},\vec{e_2},\vec{e_3} )
(e1,e2,e3)来描述：
[
x
y
z
]
=
a
⃗
=
(
e
1
⃗
,
e
2
⃗
,
e
3
⃗
)
⋅
[
a
1
a
2
a
3
]
=
a
1
e
1
⃗
+
a
2
e
2
⃗
+
a
3
e
3
⃗
(1.0)
\begin{bmatrix} x \\ y \\ z \end{bmatrix}=\vec{a} = (\vec{e_1},\vec{e_2},\vec{e_3} )\cdot \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix} = a_1\vec{e_1}+a_2\vec{e_2}+a_3\vec{e_3}\tag{1.0}
xyz
=a=(e1,e2,e3)⋅
a1a2a3
=a1e1+a2e2+a3e3(1.0)
相机在空间中的位姿可以用==
a
⃗
\vec{a}
a来表示，那么相机旋转==变换怎么描述呢？
我们知道旋转后的
[
x
y
z
]
=
a
⃗
′
=
(
e
1
⃗
′
,
e
2
⃗
′
,
e
3
⃗
′
)
⋅
[
a
1
′
a
2
′
a
3
′
]
=
a
1
′
e
1
⃗
′
+
a
2
′
e
2
⃗
′
+
a
3
′
e
3
⃗
′
(1.1)
\begin{bmatrix} x\\ y \\ z\end{bmatrix}=\vec{a}^\prime = (\vec{e_1}^\prime,\vec{e_2}^\prime,\vec{e_3}^\prime )\cdot\begin{bmatrix} a_1 ^\prime\\ a_2 ^\prime\\ a_3^\prime\end{bmatrix} = a_1^\prime\vec{e_1}^\prime+a_2^\prime\vec{e_2}^\prime+a_3^\prime\vec{e_3}^\prime\tag{1.1}
xyz
=a′=(e1′,e2′,e3′)⋅
a1′a2′a3′
=a1′e1′+a2′e2′+a3′e3′(1.1)
座标系之间的转换
(
e
1
⃗
,
e
2
⃗
,
e
3
⃗
)
⋅
[
a
1
a
2
a
3
]
=
(
e
1
⃗
′
,
e
2
⃗
′
,
e
3
⃗
′
)
⋅
[
a
1
′
a
2
′
a
3
′
]
(1.2)
(\vec{e_1},\vec{e_2},\vec{e_3} )\cdot \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix}\\= (\vec{e_1}^\prime,\vec{e_2}^\prime,\vec{e_3}^\prime )\cdot\begin{bmatrix} a_1 ^\prime\\ a_2 ^\prime\\ a_3^\prime\end{bmatrix}\tag{1.2}
(e1,e2,e3)⋅
a1a2a3
=(e1′,e2′,e3′)⋅
a1′a2′a3′
(1.2)
(
e
1
⃗
,
e
2
⃗
,
e
3
⃗
)
(\vec{e_1},\vec{e_2},\vec{e_3} )
(e1,e2,e3)为标准正交矩阵则有：
[
a
1
a
2
a
3
]
=
(
e
1
⃗
,
e
2
⃗
,
e
3
⃗
)
T
⋅
(
e
1
⃗
′
,
e
2
⃗
′
,
e
3
⃗
′
)
⋅
[
a
1
′
a
2
′
a
3
′
]
(1.3)
\begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix}\\= (\vec{e_1},\vec{e_2},\vec{e_3} )^T\cdot (\vec{e_1}^\prime,\vec{e_2}^\prime,\vec{e_3}^\prime )\cdot\begin{bmatrix} a_1 ^\prime\\ a_2 ^\prime\\ a_3^\prime\end{bmatrix}\tag{1.3}
a1a2a3
=(e1,e2,e3)T⋅(e1′,e2′,e3′)⋅
a1′a2′a3′
(1.3)
令
R
=
(
e
1
⃗
,
e
2
⃗
,
e
3
⃗
)
T
⋅
(
e
1
⃗
′
,
e
2
⃗
′
,
e
3
⃗
′
)
R = (\vec{e_1},\vec{e_2},\vec{e_3} )^T\cdot (\vec{e_1}^\prime,\vec{e_2}^\prime,\vec{e_3}^\prime )
R=(e1,e2,e3)T⋅(e1′,e2′,e3′)则：
[
a
1
a
2
a
3
]
=
R
⋅
[
a
1
′
a
2
′
a
3
′
]
(1.4)
\begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix}\\= R\cdot\begin{bmatrix} a_1 ^\prime\\ a_2 ^\prime\\ a_3^\prime\end{bmatrix}\tag{1.4}
a1a2a3
=R⋅
a1′a2′a3′
(1.4)
R
R
R为旋转矩阵
R
R
R所属的群为
S
O
（
3
）
SO（3）
SO（3） 特殊正交群。
变换表示
P
′
=
R
⋅
p
+
t
(1.5)
P^\prime = R\cdot p + t \tag{1.5}
P′=R⋅p+t(1.5)
齐次变换：
[
P
′
1
]
=
[
R
t
0
1
]
⋅
[
P
1
]
(1.6)
\begin{bmatrix} P^\prime\\ 1 \end{bmatrix} = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} P\\ 1 \end{bmatrix}\tag{1.6}
[P′1]=[R0t1]⋅[P1](1.6)
T
T
T =
[
R
t
0
1
]
\begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}
[R0t1] 即为变换矩阵
S
E
（
3
）
SE（3）
SE（3）特殊欧式群，变换可表示为：
T − 1 T^{-1} T−1 = [ R T − R − 1 t 0 1 ] \begin{bmatrix} R ^{T}& -R^{-1}t \\ 0 & 1 \end{bmatrix} [RT0−R−1t1]
P ′ = T . t o p R o w s ( 3 ) ⋅ p (1.7) P^\prime = T.topRows(3)\cdot p \tag{1.7} P′=T.topRows(3)⋅p(1.7)
旋转角与旋转轴AngleAxis
我们知道向量外积:
a
⃗
=
[
a
1
a
2
a
3
]
\vec{a} = \begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}
a=
a1a2a3
,
b
⃗
=
[
b
1
b
2
b
3
]
\vec{b} = \begin{bmatrix}b_1\\b_2\\b_3\end{bmatrix}
b=
b1b2b3
c
r
o
s
s
=
a
⃗
∗
b
⃗
=
∣
a
⃗
∣
⋅
∣
b
⃗
∣
⋅
s
i
n
(
<
a
⃗
,
b
⃗
>
)
=
a
⃗
ˆ
⋅
b
⃗
=
[
0
−
a
3
a
2
a
3
0
a
1
−
a
2
−
a
1
0
]
⋅
b
⃗
(1.8)
\begin{aligned} cross &= \vec{a}*\vec{b} =|\vec{a}| \cdot|\vec{b}|\cdot sin(<\vec{a},\vec{b}>)\\ &=\vec{a}\^{} \cdot \vec{b} \\ &=\begin{bmatrix} 0 &-a_3&a_2\\ a_3 &0 &a_1\\ -a_2 &-a_1&0 \end{bmatrix}\cdot\vec{b} \end{aligned}\tag{1.8}
cross=a∗b=∣a∣⋅∣b∣⋅sin(<a,b>)=aˆ⋅b=
0a3−a2−a30−a1a2a10
⋅b(1.8)
若令$|\vec{a}|=1 $
b
⃗
\vec{b}
b绕转轴
a
⃗
\vec{a}
a的旋转
<
a
⃗
,
b
⃗
>
<\vec{a},\vec{b}>
<a,b>可以表示为
a
⃗
∗
b
⃗
\vec{a}*\vec{b}
a∗b的形式。
b
⃗
\vec{b}
b 绕
a
⃗
\vec{a}
a 旋转
θ
\theta
θ
$$
\begin{aligned}
\rarr \vec{b}^\prime = exp(\vec{a}^{}) \cdot \vec{b}
\vec{b}^\prime= R\cdot\vec{b}\
\end{aligned}
$$
R = c o s θ ⋅ I + ( 1 − c o s θ ) ⋅ a ⃗ ⋅ a ⃗ T + s i n θ ⋅ a ⃗ ˆ R = cos\theta \cdot I + (1-cos \theta)\cdot \vec{a}\cdot\vec{a}^T+sin\theta \cdot \vec{a}\^{} R=cosθ⋅I+(1−cosθ)⋅a⋅aT+sinθ⋅aˆ
t r a c e ( R ) = t r a c e ( c o s θ ⋅ I ) + t r a c e ( ( 1 − c o s θ ) ⋅ a ⃗ ⋅ a ⃗ T ) + t r a c e ( s i n θ ⋅ a ⃗ ˆ ) t r a c e ( R ) = c o s θ ⋅ t r a c e ( I ) + ( 1 − c o s θ ) ⋅ t r a c e ( a ⃗ ⋅ a ⃗ T ) + s i n ( θ ) t r a c e ( a ⃗ ˆ ) t r a c e ( R ) = 3 c o s θ + ( 1 − c o s θ ) ⋅ a ⃗ T ⋅ a ⃗ + 0 \begin{aligned} trace(R) &= trace(cos\theta \cdot I) + trace((1-cos \theta)\cdot \vec{a}\cdot\vec{a}^T)+trace(sin\theta \cdot \vec{a}\^{})\\ trace(R) &= cos\theta\cdot trace(I)+(1-cos\theta)\cdot trace(\vec{a}\cdot \vec{a}^T)+sin(\theta)trace(\vec{a}\^{})\\ trace(R)&=3cos\theta+(1-cos\theta)\cdot \vec{a}^T\cdot\vec{a}+0 \end{aligned} trace(R)trace(R)trace(R)=trace(cosθ⋅I)+trace((1−cosθ)⋅a⋅aT)+trace(sinθ⋅aˆ)=cosθ⋅trace(I)+(1−cosθ)⋅trace(a⋅aT)+sin(θ)trace(aˆ)=3cosθ+(1−cosθ)⋅aT⋅a+0
t r a c e ( R ) = 1 + 2 c o s θ trace(R) = 1+2cos\theta trace(R)=1+2cosθ ==$\theta = = = ∗ ∗ == ** ==∗∗\frac{arccos(trace®-1)}{2}$**
四元数Quaternion
用四位复数表示
q = ( s ⃗ , v ⃗ ) q = (\vec{s},\vec{v}) q=(s,v) s ⃗ = ( s ) T \vec{s} = (s)^T s=(s)T v ⃗ = ( x , y , z ) T \vec{v} = (x,y,z)^T v=(x,y,z)T
四元数的基本运算：
加减法
q
±
p
=
(
s
q
±
s
p
,
v
q
±
v
p
)
q\pm p = (s_q\pm s_p,v_q \pm v_p)
q±p=(sq±sp,vq±vp)
乘法
$$
\begin{aligned}
pq = (s_p,v_p)(s_q,v_q) \
\end{aligned}
$$
向量
p
=
(
x
,
y
,
z
)
T
p = (x,y,z)^T
p=(x,y,z)T绕轴$q = (n_x,n_y,n_z)旋转\theta $可以表示为
p
=
(
x
,
y
,
z
)
→
P
(
0
,
x
,
y
,
z
)
p = (x,y,z)\rarr P (0,x,y,z)
p=(x,y,z)→P(0,x,y,z)
q = θ ⋅ q ( n x , n y , n z ) → Q ( c o s θ 2 , n x s i n θ 2 , n y s i n θ 2 , n z θ 2 ) q = \theta \cdot q(n_x,n_y,n_z)\rarr Q(cos\frac{\theta}{2},n_xsin\frac{\theta}{2},n_ysin\frac{\theta}{2},n_z\frac{\theta}{2})\\ q=θ⋅q(nx,ny,nz)→Q(cos2θ,nxsin2θ,nysin2θ,nz2θ)
P ′ = Q ⋅ P ⋅ Q − 1 P^\prime = Q\cdot P \cdot Q^{-1} P′=Q⋅P⋅Q−1
Q
=
(
s
,
v
)
Q = (s,v)
Q=(s,v)
Q
−
1
=
(
s
,
−
v
)
s
2
+
Q
T
Q
Q^{-1} = \frac{(s,-v)}{\sqrt{s^2+Q^TQ}}
Q−1=s2+QTQ(s,−v)
P
′
=
Q
⋅
P
⋅
Q
−
1
P^\prime = Q\cdot P \cdot Q^{-1}
P′=Q⋅P⋅Q−1
四元数转旋转
Quaternion.matrix();//转旋转矩阵
李群和李代数
李群 S O ( 3 ) SO(3) SO(3) 特殊正交群 R ∈ R 3 ∗ 3 ∣ d e t ( R ∗ R T ) = 1 { R\in R^{3*3}|det(R*R^T) = 1} R∈R3∗3∣det(R∗RT)=1
李代数
s
o
(
3
)
so(3)
so(3)=
ϕ
∈
R
3
,
ϕ
ˆ
∈
R
3
∗
3
{\phi \in R^{3},\phi \^{}\in R^{3*3}}
ϕ∈R3,ϕˆ∈R3∗3
e
x
p
(
ϕ
ˆ
)
=
R
exp(\phi \^{}) = R
exp(ϕˆ)=R
ϕ = θ ⋅ a ϕ ˆ = ( θ ϕ ) ˆ = θ ϕ ˆ α ˆ α ˆ = α α T − I α ˆ α ˆ α ˆ = − α ˆ \begin{aligned} &\phi = \theta\cdot a \\ &\phi \^{} = (\theta\phi)\^{} = {\theta\phi \^{}}\\ &\alpha\^{}\alpha\^{} = \alpha\alpha^T-I\\ &\alpha\^{}\alpha\^{}\alpha\^{} = -\alpha\^{} \end{aligned} ϕ=θ⋅aϕˆ=(θϕ)ˆ=θϕˆαˆαˆ=ααT−Iαˆαˆαˆ=−αˆ
e x p ( x ) = ∑ n ∞ x i n ! = 1 + x + x 2 2 ! + x 3 3 ! + x 4 4 ! + . . . . . . . + x n n ! = I + ϕ ˆ + ϕ ˆ 2 2 ! + ϕ ˆ 3 3 ! + ϕ ˆ 4 4 ! + . . . . . . . + ϕ ˆ n n ! = I + θ α ˆ + θ 2 α ˆ 2 2 ! + θ 3 α ˆ 3 3 ! + θ 4 α ˆ 4 4 ! + . . . . . . . + θ n α ˆ n n ! = α α T − ( α ˆ ) 2 + θ α ˆ + θ 2 ( α ˆ ) 2 2 ! − θ 3 α ˆ 3 ! − θ 4 ( α ˆ ) 2 4 ! + θ 5 α ˆ 5 ! + . . . . + = α α T + α ˆ ⋅ ( θ − θ 3 3 ! + . . . . ) − ( α ˆ ) 2 ⋅ ( 1 − θ 2 2 ! + θ 4 4 ! + . . . . . ) = α α T + α ˆ s i n θ − ( α α T − I ) c o s θ = c o s θ I + ( 1 − c o s θ ) α α T + s i n θ α ˆ (a-a) \begin{aligned} exp(x) &= \sum_n^{\infty}{\frac{x^i}{n!}}\\ &=1+x+\frac{x^2}{2!}+\frac{x^3}{3!}+\frac{x^4}{4!}+.......+\frac{x^n}{n!}\\ &=I+\phi \^{}+\frac{{\phi \^{}}^2}{2!}+\frac{{\phi \^{}}^3}{3!}+\frac{{\phi \^{}}^4}{4!}+.......+\frac{{\phi \^{}}^n}{n!}\\ &= I+\theta\alpha \^{}+\frac{{\theta ^2\alpha \^{}}^2}{2!}+\frac{{\theta^3\alpha \^{}}^3}{3!}+\frac{{\theta^4\alpha \^{}}^4}{4!}+.......+\frac{{\theta^n\alpha \^{}}^n}{n!}\\ &=\alpha\alpha^T -(\alpha\^{})^2 +\theta\alpha\^{}+\frac{\theta ^2(\alpha\^{})^2}{2!}-\frac{\theta^3\alpha\^{}}{3!}-\frac{\theta^4(\alpha\^{})^2}{4!}+\frac{\theta^5\alpha\^{}}{5!}+....+\\ &=\alpha\alpha^T+\alpha\^{}\cdot(\theta-\frac{\theta^3}{3!}+....)-(\alpha\^{})^2\cdot(1-\frac{\theta^2}{2!}+\frac{\theta^4}{4!}+.....)\\ &=\alpha\alpha^T+\alpha\^{}sin\theta-(\alpha\alpha^T-I)cos\theta\\ &=cos\theta I+(1-cos\theta)\alpha\alpha^T+sin\theta\alpha\^{} \end{aligned}\tag{a-a} exp(x)=n∑∞n!xi=1+x+2!x2+3!x3+4!x4+.......+n!xn=I+ϕˆ+2!ϕˆ2+3!ϕˆ3+4!ϕˆ4+.......+n!ϕˆn=I+θαˆ+2!θ2αˆ2+3!θ3αˆ3+4!θ4αˆ4+.......+n!θnαˆn=ααT−(αˆ)2+θαˆ+2!θ2(αˆ)2−3!θ3αˆ−4!θ4(αˆ)2+5!θ5αˆ+....+=ααT+αˆ⋅(θ−3!θ3+....)−(αˆ)2⋅(1−2!θ2+4!θ4+.....)=ααT+αˆsinθ−(ααT−I)cosθ=cosθI+(1−cosθ)ααT+sinθαˆ(a-a)
李群
S
E
(
3
)
SE(3)
SE(3) 特殊欧式群
$$
SE(3) = \begin{aligned}\left{
{T = \begin{bmatrix}R &t\0&1\end{bmatrix}, T\in R^{4*4}|R \in SO(3),t\in R^3
}\right}
\end{aligned}
KaTeX parse error: Can't use function '$' in math mode at position 5: 李代数$̲se(3)$=$\{\xi =…
\begin{aligned}\xi^{}
&= \begin{bmatrix}\phi^{}&[I]\rho\0&0\end{bmatrix}\
\xi ^{}
\xi ^{}
&= \begin{bmatrix}\phi^{}
&\rho\0&0\end{bmatrix}\begin{bmatrix}\phi^{}
&\rho\0&0\end{bmatrix} = \begin{bmatrix}(\phi^{})^2&(\phi^{})\rho\0&0\end{bmatrix}\
\xi ^{}
\xi ^{}
\xi^{}
&= \begin{bmatrix}(\phi^{})^2
&(\phi^{})
\rho\0&0\end{bmatrix}\begin{bmatrix}\phi^{}
&\rho\0&0\end{bmatrix} = \begin{bmatrix}(\phi^{})
^3&[(\phi ^{})^2] \rho\0&0\end{bmatrix}\&…………\(\xi^{}&)^n =\begin{bmatrix}(\phi^{})^n
&[(\phi^{})^{n-1}]\cdot\rho\0&0\end{bmatrix} \
\\end{aligned}
$$
e x p ( ξ ˆ ) = T = ∑ n = 0 ∞ x n n ! = I + ∑ n = 1 ∞ ( ξ ˆ ) n n ! = I + ξ ˆ + ( ξ ˆ ) 2 2 ! + ( ξ ˆ ) 3 3 ! + ( ξ ˆ ) 4 4 ! + . . . . . + ( ξ ˆ ) n n ! = I + ∑ n = 1 ∞ ( ξ ˆ ) n n ! = I + ∑ n = 1 ∞ [ ( ϕ ˆ ) n [ ( ϕ ˆ ) n − 1 ] ⋅ ρ 0 0 ] n ! = [ I + ∑ n = 1 ∞ ( ϕ ˆ ) n n ! ∑ n = 0 ∞ ( ϕ ˆ ) n − 1 ⋅ ρ n ! 0 1 ] = [ ∑ n = 1 ∞ ( ϕ ˆ ) n n ! ∑ n = 0 ∞ ( ϕ ˆ ) n ( n + 1 ) ! 0 1 ] = [ R J ρ 0 1 ] (bb) \begin{aligned} exp(\xi \^{})&= T\\ &= \sum_{n=0}^{\infty}\frac{x^n}{n!}\\ &=I+\sum_{n=1}^{\infty}\frac{({\xi \^{}})^n}{n!}\\ &=I+\xi\^{}+\frac{(\xi\^{})^2}{2!}+\frac{(\xi\^{})^3}{3!}+\frac{(\xi\^{})^4}{4!}+.....+\frac{(\xi\^{})^n}{n!}\\ &=I+\sum_{n=1}^{\infty}\frac{({\xi \^{}})^n}{n!} =I+ \sum_{n=1}^{\infty}\frac{\begin{bmatrix}(\phi\^{})^n&[(\phi\^{})^{n-1}]\cdot\rho\\0&0\end{bmatrix}}{n!} \\ &=\begin{bmatrix}I+\sum_{n=1}^{\infty}\frac{(\phi\^{})^n}{n!}&\sum_{n=0}^{\infty}\frac{(\phi\^{})^{n-1}\cdot\rho}{n!}\\ 0&1\end{bmatrix}\\ &=\begin{bmatrix}\sum_{n=1}^{\infty}\frac{(\phi\^{})^n}{n!}&\sum_{n=0}^{\infty}\frac{(\phi\^{})^n}{(n+1)!}\\0&1\end{bmatrix}\\ &=\begin{bmatrix}R&J\rho\\0&1\end{bmatrix} \end{aligned}\tag{bb} exp(ξˆ)=T=n=0∑∞n!xn=I+n=1∑∞n!(ξˆ)n=I+ξˆ+2!(ξˆ)2+3!(ξˆ)3+4!(ξˆ)4+.....+n!(ξˆ)n=I+n=1∑∞n!(ξˆ)n=I+n=1∑∞n![(ϕˆ)n0[(ϕˆ)n−1]⋅ρ0]=[I+∑n=1∞n!(ϕˆ)n0∑n=0∞n!(ϕˆ)n−1⋅ρ1]=[∑n=1∞n!(ϕˆ)n0∑n=0∞(n+1)!(ϕˆ)n1]=[R0Jρ1](bb)
ϕ = θ α ( θ α ) ˆ = θ α ˆ J = ∑ n = 0 ∞ ( ϕ ˆ ) n ( n + 1 ) ! = I + ( ϕ ˆ ) 1 2 ! + ( ϕ ˆ ) 2 3 ! + ( ϕ ˆ ) 3 4 ! + ( ϕ ˆ ) 4 5 ! + . . . . . + ( ϕ ˆ ) n − 1 ( n ) ! + ( ϕ ˆ ) n ( n + 1 ) ! = I + θ α ˆ 2 ! + θ 2 ( α ˆ ) 2 3 ! + θ 3 ( α ˆ ) 3 4 ! + θ 4 ( α ˆ ) 4 5 ! + . . . . + θ n − 1 ( α ˆ ) n − 1 n ! + θ n ( α ˆ ) n ( n + 1 ) ! = I + 1 θ ⋅ ( θ 2 α ˆ 2 ! + θ 3 ( α ˆ ) 2 3 ! + θ 4 ( α ˆ ) 3 4 ! + θ 5 ( α ˆ ) 4 5 ! + . . . . + θ n ( α ˆ ) n − 1 n ! + θ n + 1 ( α ˆ ) n ( n + 1 ) ! ) = I + 1 θ ⋅ ( θ 2 α ˆ 2 ! + θ 3 ( α ˆ ) 2 3 ! − θ 4 α ˆ 4 ! + θ 5 ( α ˆ ) 4 5 ! + . . . . + θ n ( α ˆ ) n − 1 n ! + θ n + 1 ( α ˆ ) n ( n + 1 ) ! ) = I + 1 θ ⋅ [ α ˆ ( θ 2 2 ! − θ 4 4 ! + θ 6 6 ! + . . . . . ) + ( α ˆ ) 2 ( θ 3 3 ! − θ 5 5 ! + θ 7 7 ! + . . . . ) ] = I + 1 θ ⋅ [ α ˆ ( 1 − c o s θ ) + ( α ˆ ) 2 ( θ − s i n θ ) ] = I + α ˆ θ ( 1 − c o s θ ) + ( α α T − I ) ( 1 − s i n θ θ ) = s i n θ θ I + ( 1 − s i n θ θ ) α α T + 1 − c o s θ θ α ˆ \begin{aligned} \phi = \theta\alpha& \ \ \ (\theta\alpha)\^{} = \theta\alpha\^{} \\ J &=\sum_{n=0}^{\infty}\frac{(\phi\^{})^n}{(n+1)!}\\ &=I+\frac{(\phi\^{})^1}{2!}+\frac{(\phi\^{})^2}{3!}+\frac{(\phi\^{})^3}{4!}+\frac{(\phi\^{})^4}{5!}+.....+\frac{(\phi\^{})^{n-1}}{(n)!}+\frac{(\phi\^{})^n}{(n+1)!}\\ & = I+\frac{\theta\alpha\^{}}{2!}+\frac{\theta^2(\alpha\^{} )^2}{3!}+\frac{\theta^3(\alpha\^{} )^3}{4!}+\frac{\theta^4(\alpha\^{} )^4}{5!}+....+\frac{\theta^{n-1}(\alpha\^{} )^{n-1}}{n!}+\frac{\theta^{n}(\alpha\^{} )^{n}}{(n+1)!}\\ & = I+\frac{1}{\theta}\cdot\left({\frac{\theta^2\alpha\^{}}{2!}+\frac{\theta^3(\alpha\^{} )^2}{3!}+\frac{\theta^4(\alpha\^{} )^3}{4!}+\frac{\theta^5(\alpha\^{} )^4}{5!}+....+\frac{\theta^{n}(\alpha\^{} )^{n-1}}{n!}+\frac{\theta^{n+1}(\alpha\^{} )^{n}}{(n+1)!}}\right)\\ &=I+\frac{1}{\theta}\cdot\left({\frac{\theta^2\alpha\^{}}{2!}+\frac{\theta^3(\alpha\^{} )^2}{3!}-\frac{\theta^4\alpha\^{} }{4!}+\frac{\theta^5(\alpha\^{} )^4}{5!}+....+\frac{\theta^{n}(\alpha\^{} )^{n-1}}{n!}+\frac{\theta^{n+1}(\alpha\^{} )^{n}}{(n+1)!}}\right)\\ &=I+\frac{1}{\theta} \cdot\left[{\alpha\^{}\left({\frac{\theta^2}{2!}-\frac{\theta^4}{4!}+\frac{\theta^6}{6!}+.....}\right)+(\alpha\^{})^2\left({\frac{\theta^3}{3!}-\frac{\theta^5}{5!}+\frac{\theta^7}{7!}+....}\right) }\right]\\ &=I+\frac{1}{\theta}\cdot\left[\alpha\^{}{\left({1-cos\theta}\right)+(\alpha\^{})^2\left({\theta-sin\theta}\right)}\right]\\ &=I+\frac{\alpha\^{}}{\theta}(1-cos\theta)+(\alpha\alpha^T-I)(1-\frac{sin\theta}{\theta})\\ &=\frac{sin\theta}{\theta}I+(1-\frac{sin\theta}{\theta})\alpha\alpha^T+\frac{1-cos\theta}{\theta}\alpha\^{} \end{aligned} ϕ=θαJ (θα)ˆ=θαˆ=n=0∑∞(n+1)!(ϕˆ)n=I+2!(ϕˆ)1+3!(ϕˆ)2+4!(ϕˆ)3+5!(ϕˆ)4+.....+(n)!(ϕˆ)n−1+(n+1)!(ϕˆ)n=I+2!θαˆ+3!θ2(αˆ)2+4!θ3(αˆ)3+5!θ4(αˆ)4+....+n!θn−1(αˆ)n−1+(n+1)!θn(αˆ)n=I+θ1⋅(2!θ2αˆ+3!θ3(αˆ)2+4!θ4(αˆ)3+5!θ5(αˆ)4+....+n!θn(αˆ)n−1+(n+1)!θn+1(αˆ)n)=I+θ1⋅(2!θ2αˆ+3!θ3(αˆ)2−4!θ4αˆ+5!θ5(αˆ)4+....+n!θn(αˆ)n−1+(n+1)!θn+1(αˆ)n)=I+θ1⋅[αˆ(2!θ2−4!θ4+6!θ6+.....)+(αˆ)2(3!θ3−5!θ5+7!θ7+....)]=I+θ1⋅[αˆ(1−cosθ)+(αˆ)2(θ−sinθ)]=I+θαˆ(1−cosθ)+(ααT−I)(1−θsinθ)=θsinθI+(1−θsinθ)ααT+θ1−cosθαˆ
在李代数上求导
首先我们要知道为什么要在李代数上求导，我们本来的目的是为了找到最优的李群，但是由于李群对加法不封闭，因此对李代数进行求导。
B
C
H
BCH
BCH公式：
exp
(
ϕ
ˆ
)
exp
(
Δ
ϕ
ˆ
)
≈
exp
(
(
J
r
−
1
(
ϕ
)
Δ
ϕ
+
ϕ
)
ˆ
)
exp
(
Δ
ϕ
ˆ
)
exp
(
ϕ
ˆ
)
≈
exp
(
(
J
l
−
1
(
ϕ
)
Δ
ϕ
+
ϕ
)
ˆ
)
\exp(\phi\^{})\exp(\Delta\phi\^{})\approx \exp((J_r^{-1}(\phi)\Delta\phi+\phi)\^{}) \\ \exp(\Delta\phi\^{})\exp(\phi\^{})\approx \exp((J_l^{-1}(\phi)\Delta\phi+\phi)\^{})
exp(ϕˆ)exp(Δϕˆ)≈exp((Jr−1(ϕ)Δϕ+ϕ)ˆ)exp(Δϕˆ)exp(ϕˆ)≈exp((Jl−1(ϕ)Δϕ+ϕ)ˆ)
这里的$J_l = \frac{sin\theta I}{\theta}+(1-\frac{sin\theta}{ \theta})\alpha\cdot\alpha^T+\frac{1-cos\theta}{\theta}\alpha^{} \ \ \ \ \ J_r = J_l(-\phi) $
KaTeX parse error: Expected '}', got '&' at position 143: …gin{bmatrix}{0 &̲0&0\\0&\frac{si…
$$
$$
$$
$$
对李代数so3求导 P ′ = R P P_\prime = RP P′=RP
KaTeX parse error: Undefined control sequence: \part at position 24: …aligned} \frac{\̲p̲a̲r̲t̲ ̲Rp}{\part\delta…
对李代数so3扰动模型求导 P ′ = R 扰动 R P P_\prime = R_{扰动}RP P′=R扰动RP
扰动模型，给予
R
扰动
=
exp
(
Φ
ˆ
)
R_{扰动} = \exp(\Phi\^{})
R扰动=exp(Φˆ)
$$
\begin{aligned}
\frac{\part RP}{\part\Phi}& = lim_{\Phi \rarr0}\frac{\exp(\Phi^{})\exp(\phi^{}) - \exp(\phi^{})}{\Phi }P \
& = lim_{\Phi \rarr0}\frac{(I+\Phi^{})\exp(\phi^{}) - \exp(\phi^{})}{\Phi }P \
& = lim_{\Phi \rarr0}\frac{\Phi^{}\exp(\phi^{}) P}{\Phi } \
& = lim_{\Phi \rarr0}\frac{-(\exp(\phi^{})P)^{}\Phi }{\Phi } \
& = -(RP)^{}\ \
\end{aligned}
$$
李代数se3扰动模型求导 P ′ = T 扰动 T P P_\prime = T_{扰动}TP P′=T扰动TP
$$
\begin{aligned}
\frac{\part TP}{\part\delta\xi} &= lim_{\delta\xi \rarr0}\frac{\exp(\delta\xi^{})\exp(\xi^{}) - \exp(\xi^{})}{\delta\xi }P \
\delta\xi &= \begin{bmatrix}\delta\rho\\delta\phi\end{bmatrix} \ \ \delta\xi^ = \begin{bmatrix}\delta\phi^{}&\delta\rho\0&0\end{bmatrix}\ \ \ &lim_{\delta\xi^{}\rarr0} \exp(\begin{bmatrix}\delta\phi^{}&\delta\rho\0&0\end{bmatrix})\& = \begin{bmatrix}\exp(\delta\phi^{})&J(\phi)\delta\rho\0&1\end{bmatrix} \&=
\begin{bmatrix}I+\delta\phi^{}&0\0&1\end{bmatrix} =
I+\begin{bmatrix}\delta\phi^{}&\delta\rho\0&0\end{bmatrix} = I+\delta\xi^{}
\
& = lim_{\delta\xi \rarr0}\frac{\exp(\delta\xi^{})\exp(\xi^{}) - \exp(\xi^{})}{\delta\xi }P \
& = lim_{\delta\xi \rarr0}\frac{(I+\delta\xi^{})\exp(\xi^{}) - \exp(\xi^{})}{\delta\xi }P \
& = lim_{\delta\xi \rarr0}\frac{\delta\xi^{}\exp(\xi^{}) }{\delta\xi }P \
& = lim_{\delta\xi \rarr0} \ \ \ \frac{\begin{bmatrix}\delta\phi^{}&\delta\rho\0 &0\end{bmatrix}\begin{bmatrix}R&t\0&1\end{bmatrix}\begin{bmatrix}p\1\end{bmatrix}}{\begin{bmatrix}\delta\rho\\delta\phi\end{bmatrix}} =\frac{\begin{bmatrix}\delta\phi^{}Rp+\delta\phi^{}t+\delta\rho\0\end{bmatrix}}{\begin{bmatrix}\delta\rho\\delta\phi\end{bmatrix}}\
& = lim_{\delta\xi \rarr0} \ \ \ \frac{\begin{bmatrix}(Rp+t)^{}\delta\phi^+\delta\rho\0\end{bmatrix}}{\begin{bmatrix}\delta\rho\\delta\phi\end{bmatrix}} =\begin{bmatrix}I&-(Rp+t)^{}\0&0\end{bmatrix} \
\end{aligned}
$$
Sophus库（索福斯）
相机与图像
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-0OepIYG9-1692510864675)(/home/lvx/桌面/笔记/座标系变换.jpeg)]
图中存在四个座标系，分别是：
- O w O_w Ow世界座标系 ( x , y , z ) (x,y,z) (x,y,z)
- O c 相机座标系 ( x , y , z ) O_c相机座标系(x,y,z) Oc相机座标系(x,y,z)
- O i 成像座标系 ( x , y ) O_i成像座标系(x,y) Oi成像座标系(x,y)
- O p 像素座标系 ( u , v ) O_p像素座标系(u,v) Op像素座标系(u,v)
世界座标系转相机：
P c = R P w + t P_c = RP_w+t Pc=RPw+t
相机转成像平面：小孔成像原理
f
z
=
x
′
x
=
y
′
y
\frac{f}{z} = \frac{x_\prime}{x} = \frac{y_\prime}{y}
zf=xx′=yy′
x
′
=
x
f
z
y
′
=
y
f
z
z
[
x
′
y
′
1
]
=
[
f
0
0
0
f
0
0
0
1
]
[
x
y
z
]
x_\prime = x\frac{f}{z} \ \ \ \ \ \ \ \ y_\prime = y\frac{f}{z}\\ z\begin{bmatrix}x_\prime\\y_\prime\\1\end{bmatrix}=\begin{bmatrix}f&0&0\\0&f&0\\0&0&1\end{bmatrix}\begin{bmatrix}x\\y\\z\end{bmatrix}
x′=xzf y′=yzfz
x′y′1
=
f000f0001
xyz
成像转像素：
先介绍一下相机的各个参数：$分辨率R = R_x\cdot R_y,相机的物理大小P_x,P_y; dx =\frac{P_x}{R_x} ,dy =\frac{P_y}{R_y} ;U_x = {R_x\over 2}，V_y = {R_y\over 2} $
u
=
x
d
x
+
U
x
v
=
y
d
y
+
V
y
[
u
v
1
]
=
[
1
d
x
0
U
x
0
1
d
y
V
y
0
0
1
]
[
x
y
1
]
\begin{aligned} &u = \frac{x}{dx}+U_x \ \ \ \ \ \ \ \ \ \ \ v = \frac{y}{dy}+V_y \\ &\begin{bmatrix}u\\v\\1\end{bmatrix}=\begin{bmatrix}{1\over dx}&0&U_x\\0&{1\over dy}&V_y\\0&0&1\end{bmatrix}\begin{bmatrix}x\\y\\1\end{bmatrix} \end{aligned}
u=dxx+Ux v=dyy+Vy
uv1
=
dx1000dy10UxVy1
xy1
相机转像素:
[
u
v
1
]
=
[
1
d
x
0
U
x
0
1
d
y
V
y
0
0
1
]
[
x
y
1
]
=
[
1
d
x
0
U
x
0
1
d
y
V
y
0
0
1
]
⋅
1
z
[
f
0
0
0
f
0
0
0
1
]
[
x
y
z
]
=
1
z
[
f
d
x
0
U
x
0
f
d
y
V
y
0
0
1
]
[
x
y
z
]
\begin{aligned} \begin{bmatrix}u\\v\\1\end{bmatrix}&=\begin{bmatrix}{1\over dx}&0&U_x\\0&{1\over dy}&V_y\\0&0&1\end{bmatrix}\begin{bmatrix}x\\y\\1\end{bmatrix} \\&=\begin{bmatrix}{1\over dx}&0&U_x\\0&{1\over dy}&V_y\\0&0&1\end{bmatrix}\cdot{1\over z} \begin{bmatrix}f&0&0\\0&f&0\\0&0&1\end{bmatrix}\begin{bmatrix}x\\y\\z\end{bmatrix} \\ & ={1\over z} \begin{bmatrix}{f\over dx}&0&U_x\\0&{f\over dy}&V_y\\0&0&1\end{bmatrix}\begin{bmatrix}x\\y\\z\end{bmatrix} \end{aligned}
uv1
=
dx1000dy10UxVy1
xy1
=
dx1000dy10UxVy1
⋅z1
f000f0001
xyz
=z1
dxf000dyf0UxVy1
xyz
**令$ \begin{bmatrix}{ f\over dx}&0&U_x\0&{f\over dy}&V_y\0&0&1\end{bmatrix} = K，K为内参矩阵
∗
∗
=
=
** ==
∗∗==P_p = KP_C(齐次) = K(RP_w+t)(齐次)$==
视觉里程计：相邻帧图像估计相机运动R，t
2D-2D估计相机运动
对极约束
I 1 像素平面和 I 2 像素平面上的 P 1 , P 2 是一组特征匹配点，是世界座标系下 P 点在两个成像平面上的投影 I_1像素平面和I_2像素平面上的P_1,P_2是一组特征匹配点，是世界座标系下P点在两个成像平面上的投影 I1像素平面和I2像素平面上的P1,P2是一组特征匹配点，是世界座标系下P点在两个成像平面上的投影
$p_1 = {1\over s_1}KP_1 \ \ \ \ \ \ \ \ p_2 = {1\over s_2}kP_2；\ \ \ \ \ \ \ x_1 ={1\over s_1}P_1\rarr x_1\simeq P_1 \ \ \ \ \ \ \ \ \ x_2 ={1\over s_2}P_2\rarr x_2\simeq P_2 \ \ \ \ \ \ \ \ p_1 = Kx_1 \ \ \ \ \ \ \ \ p_2 = kx_2； $
x
1
=
K
−
1
p
1
x
2
=
K
−
1
p
2
P
2
=
R
P
1
+
t
→
x
2
≃
R
x
1
+
t
x
2
≃
R
x
1
+
t
消去
t
左乘
t
ˆ
t
ˆ
x
2
≃
t
ˆ
R
x
1
t
ˆ
x
2
与
t
和
x
2
所在的平面都垂直等式两边左乘
x
2
T
x
2
T
t
ˆ
x
2
≃
x
2
T
t
ˆ
R
x
1
0
=
x
2
T
t
ˆ
R
x
1
0
=
(
K
−
1
P
2
)
T
t
ˆ
R
(
K
−
1
P
1
)
0
=
P
2
T
K
−
T
t
ˆ
R
K
−
1
P
1
\begin{aligned} &x_1 = K^{-1}p_1 \ \ \ \ \ \ x_2 = K^{-1}p_2 \\ &P_2 = RP_1+t\rarr x_2\simeq Rx_1+t\\ &x_2\simeq Rx_1+t \ \ \ 消去t\ \ \ 左乘t\^{} \\ &t\^{}x_2\simeq t\^{}Rx_1\ \ \ \ \ t\^{}x_2与t和x_2所在的平面都垂直 等式两边左乘x_2^T\\ &x_2^Tt\^{}x_2\simeq x_2^Tt\^{}Rx_1 \\ &0 = x_2^Tt\^{}Rx_1\\ &0 = (K^{-1}P_2)^Tt\^{}R(K^{-1}P_1)\\ &0 = P_2^TK^{-T}t\^{}RK^{-1}P_1\\ \end{aligned}
x1=K−1p1 x2=K−1p2P2=RP1+t→x2≃Rx1+tx2≃Rx1+t 消去t 左乘tˆtˆx2≃tˆRx1 tˆx2与t和x2所在的平面都垂直等式两边左乘x2Tx2Ttˆx2≃x2TtˆRx10=x2TtˆRx10=(K−1P2)TtˆR(K−1P1)0=P2TK−TtˆRK−1P1
令
t
ˆ
R
=
E
t\^{}R = E
tˆR=E
E
E
E 为本质矩阵；
F
=
K
−
T
E
K
−
1
矩阵
F
为基础
F =K^{-T}EK^{-1} \ \ \ \ \ 矩阵\ F为基础
F=K−TEK−1 矩阵 F为基础
P
2
T
F
p
1
=
0
P_2^TFp_1 = 0
P2TFp1=0
对此求R，t的步骤可分为：
-
求基础矩阵 F F F
p 2 T F p 1 = 0 0 = [ u 2 v 2 1 ] [ e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ] [ u 1 v 2 1 ] = [ u 2 e 1 + v 2 e 4 + e 7 u 2 e 2 + v 2 e 5 + e 8 u 2 e 3 + v 2 e 6 + e 9 ] [ u 1 v 1 1 ] = u 1 u 2 e 1 + u 1 v 2 e 4 + u 1 e 7 + v 1 u 2 e 2 + v 2 v 1 e 5 + v 1 e 8 + u 2 e 3 + v 2 e 6 + e 9 = [ u 1 u 2 u 2 v 1 u 2 u 1 v 2 v 1 v 2 v 2 u 1 v 1 1 ] [ e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ] \begin{aligned} &p_2^TFp_1 = 0 \\ 0 &= \begin{bmatrix}u_2&v_2&1\end{bmatrix}\begin{bmatrix}e_1&e_2&e_3\\ e_4&e_5&e_6\\e_7&e_8&e_9\end{bmatrix}\begin{bmatrix}u_1\\v_2\\1\end{bmatrix} \\ & = \begin{bmatrix}u_2e_1+v_2e_4+e_7&u_2e_2+v_2e_5+e_8&u_2e_3+v_2e_6+e_9\end{bmatrix}\begin{bmatrix}u_1\\v_1\\1\end{bmatrix}\\ & = u_1u_2e_1+u_1v_2e_4+u_1e_7 + v_1u_2e_2+v_2v_1e_5+v_1e_8+ u_2e_3+v_2e_6+e_9\\ & = \begin{bmatrix}u_1u_2&u_2v_1&u_2&u_1v_2&v_1v_2&v_2&u_1&v_1&1\end{bmatrix}\begin{bmatrix}e_1\\e_2\\e_3\\e_4\\e_5\\e_6\\e_7\\e_8\\e_9\end{bmatrix} \end{aligned} 0p2TFp1=0=[u2v21] e1e4e7e2e5e8e3e6e9 u1v21 =[u2e1+v2e4+e7u2e2+v2e5+e8u2e3+v2e6+e9] u1v11 =u1u2e1+u1v2e4+u1e7+v1u2e2+v2v1e5+v1e8+u2e3+v2e6+e9=[u1u2u2v1u2u1v2v1v2v2u1v11] e1e2e3e4e5e6e7e8e9
==八点法：==八个点对求e -
求本质矩阵$E \ \ \ \ \ \ \ \ \ \ F = K{-T}EK{-1}\rarr E = K^TFK $
-
求R，t
E = t ˆ R E = U ∑ V T \begin{aligned} &E = t\^{}R\\ & E=U\sum V^T \\ \end{aligned} E=tˆRE=U∑VT简单介绍一下SVD分解
$$$$
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
