目前,在进实验室之前自己学习SLAM,用的是高翔博士所著的《视觉SLAM十四讲》,其中涉及到罗德里格斯公式,这里讲自己的推导过程记录如下。
罗德里格斯公式如下:
上述公式是在视觉SLAM中常用的公式,可以讲旋转角变换为旋转矩阵,其中R为旋转矩阵,θ为绕轴旋转的角度,n为旋转轴方向的单位向量。
下面给出推导过程:
这是一个证明的辅助示意图(比较粗糙,不要见怪),大致是利用了向量的分解以及一些叉乘的知识。
证明
因为我们的最终目的是要的到旋转矩阵R,而所用到R的地方就是vrot =Rv,我们就要尽可能的用v来表示vrot。
vrot = v⊥ + v//
vrot = a + b + v//
然后我们就把 a,b,v// 分别用 v 来表示就好了。
首先是 v//,从简单到难嘛,很容易看出来
v// = (v·n)·n
解释一下,v·n是一个内积,也就是向量v在n上的投影,是一个数,然后再数乘n,变成一个方向和n相同,大小为v·n的向量,也就是v//.
然后是b,也不难看出
b = cosθ·v⊥
这又是怎么得出来的呢,下面给出简单的推导。
b 应该是长度为 vrot 的膜乘-cosθ,方向与 v⊥ 相反,那么
b = ||vrot⊥||·(-cosθ)·(-v⊥n)
这里注意cosθ是负的,继续推导,由于||vrot⊥||与||v⊥||相等,
b = ||v⊥||·cosθ·v⊥n
b = cosθ·v⊥
最后是a,先给出a的结果
a = sinθ · n × v
与 b 的推导一样,a 的长度是 ||vrot⊥||·sinθ,方向是 n×v⊥n,那么
a = ||vrot⊥||·sinθ·n×v⊥n
又因为上面提到的||vrot⊥||与||v⊥||相等,所以
a = ||v⊥||·sinθ·n×v⊥n
a = sinθ·n×v⊥
但是,由于我们最终要表现得形式是 vrot 和 v 之间的,所以把上述a的表示变成由v表示,注意到,n× v⊥ 是膜为||n||·||v⊥||·sin90° = ||n||·||v⊥||,方向为n×v的向量,而 n × v 同样也是,所以直接替换
a = sinθ · n × v
到此为止,就把所有的向量用与 v 有关的向量来表示了,目前的推导公式已经变成了如下样子:
vrot = sinθ·n×v + cosθ·v⊥ + (v·n)·n
然而,此时还并没有转换为 vrot = Rv的形式,下面就要把所有的 v 的系数转化为矩阵。
首先第一项,可以直接利用叉乘进行转化,如果你看过《视觉SLAM十四讲》就对这个肯定不陌生,可以转化为 sinθ·n^·v,
其中,n^大概就是左边矩阵的这个样子,这是规定好的,可以算一下,这样算出来和直接叉乘的结果一致,图中的 k 即为我所说的 n。
然后第二项,v⊥ = v - v//
所以原式 = cosθ·(v - (v·n)·n),就转化为了对第三项进行变换
最后是第三项,由数乘的交换律,(v·n)·n = n·(v·n),然后由于v·n = nT·v,所以原式转化为n·nT·v,前半部分转化为矩阵。
综上所述,最终可得(剩下的自己顺着写就出来了)
如有问题,欢迎留言~~~