第二講 三維空間的剛體運(yùn)動課后題

[toc]

第二講 三維空間的剛體運(yùn)動

1、熟悉 Eigen 矩陣運(yùn)算

設(shè)線性?程 Ax = b,在 A 為?陣的前提下,請回答以下問題:

1.1 在什么條件下,x 有解且唯??

答案:非齊次線性方程組有唯一解的充要條件是 rank(A)=n。

1.2 ?斯消元法的原理是什么?

答案:最基本的那種求方程解的方法,就是對矩陣進(jìn)行行變換。

1.3 QR 分解的原理是什么?

答案:在了解QR分解之前,先了解一下Gram-Schmidt正交化:

存在可逆矩陣A的列向量組(\alpha_1,\alpha_2,\alpha_3,...,\alpha_n)經(jīng)過正交化之后可以得到:
\varepsilon_1 =\frac{\beta_1} {\begin{Vmatrix} \beta_1 \end{Vmatrix}} = t_{11}\alpha_1 \\ \varepsilon_2 =\frac{\beta_2} {\begin{Vmatrix} \beta_2\end{Vmatrix}} = t_{11}\alpha_1 + t_{22}\alpha_2 \\ \vdots \\ \varepsilon_1 =\frac{\beta_{j}} {\begin{Vmatrix} \beta_j\end{Vmatrix}} = t_{1j}\alpha_1 + t_{2j}\alpha_2 + \dots + t_{jj}\alpha_j
把上述式子使用矩陣表示就可以得到下面的定義

對于物理意義,有時(shí)候不是那么重要,知道是這種數(shù)學(xué)表達(dá)就可以了。

定義:對于n階方陣A,若存在正交矩陣Q和上三角矩陣R,使得A = QR,則該式稱為矩陣A的完全QR分解或正交三角分解。(對于可逆矩陣A存在完全QR分解)。
(\varepsilon_1,\varepsilon_2,\varepsilon_3,...,\varepsilon_j) = (\alpha_1,\alpha_2,\alpha_3,...,\alpha_n) \begin{pmatrix} t_{11}&\dots&t_{1n}\\ &\ddots&\vdots\\ 0&&t_{nn}\\ \end{pmatrix}
QR基于我們熟悉的Gram-Schmidt正交化,令:

A=(\alpha_1,\alpha_2,\alpha_3,...,\alpha_n)\\ T = (t_{ij})\\ Q = (\varepsilon_1,\varepsilon_2,\varepsilon_3,...,\varepsilon_j)

由此有Q=AT若記R=T^{-1},則此時(shí)A=QR。其中Q是由Gram-Schmidt正交化得到的標(biāo)準(zhǔn)的正交基。

1.4 Cholesky 分解的原理是什么?

可以閱讀Cholesky分解維基百科獲取更加詳細(xì)的解釋,下面只是為了便于理解而簡單的進(jìn)行說明。

直接先說Cholesky 分解是用于求解Ax=b這個(gè)方程,然后具有等式:

A = LL^T

從式子可以看到就和代數(shù)的平方一樣的效果,常用在優(yōu)化里面作為誤差;另外這個(gè)分解方法的優(yōu)點(diǎn)是提高代數(shù)運(yùn)算效率(矩陣求逆)、蒙特卡羅方法等場合中十分有用。

其中A是一個(gè)n階厄米特正定矩陣(Hermitian positive-definite matrix),L是下三角矩陣。下面介紹推導(dǎo)過程

所以我們的目的是為了求L矩陣,算法由i:=1開始,令:

A^{(1)}:=A

在步驟i中,矩陣A^{(i)}的形式如下:

A^{(i)} = \begin{pmatrix} I_{i-1}&0&0\\ 0&\alpha_{i,j}&b_{i}^*\\ 0&b_i&B^{i}\\ \end{pmatrix}

其中b_i^{*}表示b_i的共軛轉(zhuǎn)置,若b是實(shí)數(shù)矩陣, b_i^{*}就是轉(zhuǎn)置;I_{i-1}代表i-1維的單位矩陣。此時(shí)L_i定義為:
L_i := \begin{pmatrix} I_{i-1}&0&0\\ 0&\sqrt{\alpha_{i,j}}&0\\ 0&\frac{1}{\sqrt{a_{i,j}}}b_i&I_{n-1}\\ \end{pmatrix}
只有可以將矩陣A^{(i)}改寫為:

A^{(i)} = L_{i}A^{(i+1)}L_i^{*}

我們發(fā)現(xiàn)這個(gè)和我們熟悉的的特征值分解結(jié)構(gòu)相似Q \Lambda Q,接下來可以得到A^{(i+1)}的形式:
A^{(i+1)} = \begin{pmatrix} I_{i-1}&0&0\\ 0&1&0\\ 0&0&B^{(i)}-\frac{1}{a_{i,j}}b_ib_i^{*} \end{pmatrix}

需要注意的是這里的b_{i}b_{i}^{*}是一個(gè)外積。

重復(fù)此步驟,直到i從1到nn步之后,我們可以得到A^{(n+1)} = I。因此下三角矩陣L為:

L := L_1L_2\dots L_n

1.5 編程實(shí)現(xiàn) A 為 100×100 隨機(jī)矩陣時(shí),? QR 和 Cholesky 分解求 x 的程序。你可以參考本次課 ?到的 useEigen 例程。
#include <iostream>

using namespace std;

#include <ctime>

// Eigen 部分
#include <Eigen/Core>
// 稠密矩陣的代數(shù)運(yùn)算(逆,特征值等)
#include <Eigen/Dense>

#define MATRIX_SIZE 100


int main( int argc, char** argv )

{

    // 解方程
    // 我們求解 matrix_NN * x = v_Nd 這個(gè)方程
    // N的大小在前邊的宏里定義,它由隨機(jī)數(shù)生成
    // 直接求逆自然是最直接的,但是求逆運(yùn)算量大
    
    cout <<"---解方程---"<<endl;
    clock_t time_stt = clock(); // 計(jì)時(shí)
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_Dy;
    Eigen::Matrix< double, Eigen::Dynamic,  1> v_Nd;
    Eigen::Matrix< double, Eigen::Dynamic,  1> x;
    matrix_Dy = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
    matrix_Dy=matrix_Dy.transpose()*matrix_Dy;       //喬利斯基分解需要正定矩陣
    
    // 求逆
    
    v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE, 1 );
    x = matrix_Dy.inverse()*v_Nd;
    cout<<x[0]<<endl;
    cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;


    // Cholesky 

    time_stt = clock();
    x = matrix_Dy.ldlt().solve(v_Nd);
    cout << x[0] << endl;
    cout <<"time use in Cholesky decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    // QR/Lu
    
    time_stt = clock();
    x = matrix_Dy.colPivHouseholderQr().solve(v_Nd);
    cout << x[0] << endl;
    x = matrix_Dy.fullPivLu().solve(v_Nd);
    cout << x[0] << endl;
    cout <<"time use in Qr decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    return 0;
}

2、幾何運(yùn)算練習(xí)

設(shè)有?蘿?1?號和?蘿??號位于世界坐標(biāo)系中。?蘿??號的位姿為: q1 = [0.55,0.3,0.2,0.2],t1 = [0.7,1.1,0.2]^Tq 的第?項(xiàng)為實(shí)部)。這?的 qt表達(dá)的是 T_{cw},也就是世界到相機(jī)的變換關(guān)系。?蘿? ?號的位姿為 q_2 = [?0.1,0.3,?0.7,0.2],t_2 = [?0.1,0.4,0.8]^T。現(xiàn)在,?蘿??號看到某個(gè)點(diǎn)在??的坐 標(biāo)系下,坐標(biāo)為p_1 = [0.5,?0.1,0.2]^T,求該向量在?蘿??號坐標(biāo)系下的坐標(biāo)。請編程實(shí)現(xiàn)此事,并提交 你的程序。

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

int main ( int argc, char** argv )

{
    Eigen::Isometry3d Tcw1= Eigen::Isometry3d::Identity();                
    Eigen::Isometry3d Tcw2= Eigen::Isometry3d::Identity();              

    Eigen::Quaterniond q1(0.55,0.3,0.2,0.2);//定義四元數(shù)Q1
    Eigen::Quaterniond q2(-0.1,0.3,-0.7,0.2);

    cout<<"quaternion q1 = \n"<<q1.coeffs() <<endl;  // 請注意coeffs的順序是(x,y,z,w),w為實(shí)部,前三者為虛部

    cout<<"quaternion q2 = \n"<<q2.coeffs() <<endl;   

    Eigen::Matrix<double, 3, 1> t1(0.7, 1.1, 0.2 );
    Eigen::Matrix<double, 3, 1> t2(-0.1, 0.4, 0.8 );

    Eigen::Matrix< double, 3, 1 > p1c(0.5, -0.1, 0.2);
    Eigen::Matrix< double, 3, 1 > p1w;
    Eigen::Matrix< double, 3, 1 > p2c;


    q1 = q1.normalized();
    q2 = q2.normalized();

    Tcw1.rotate ( q1.toRotationMatrix() );       // 按照rotation_vector進(jìn)行旋轉(zhuǎn)
    Tcw1.pretranslate (t1);                     // 平移向量
    cout << " Tcw1 Transform matrix = \n" << Tcw1.matrix() <<endl;

    Tcw2.rotate ( q2.toRotationMatrix() );     // 按照rotation_vector進(jìn)行旋轉(zhuǎn)
    Tcw2.pretranslate (t2);                     // 平移向量
    cout << "Tcw2 Transform matrix = \n" << Tcw2.matrix() <<endl;

    p1w = Tcw1.inverse()*p1c;
    p2c = Tcw2*p1w;
    cout << "p2c = \n" << p2c.matrix() <<endl;

    return 0;
}

3、旋轉(zhuǎn)的表達(dá)

課程中提到了旋轉(zhuǎn)可以?旋轉(zhuǎn)矩陣、旋轉(zhuǎn)向量與四元數(shù)表達(dá),其中旋轉(zhuǎn)矩陣與四元數(shù)是?常應(yīng)?中常見的表達(dá)?式。請根據(jù)課件知識,完成下述內(nèi)容的證明。

3.1 設(shè)有旋轉(zhuǎn)矩陣 R,證明 R^TR = Idet R = +1^2
3.1.1 正交矩陣性質(zhì):
  • 正交矩陣的每一個(gè)列向量都是單位向量,且向量之間兩兩正交。
  • 正交矩陣的行列式為1或者-1.
  • A^-1 = A^T (充要條件)
3.1.2 證明旋轉(zhuǎn)矩陣是正交矩陣且行列式為1

首先我們令空間的一個(gè)位置x_i經(jīng)過旋轉(zhuǎn)R和一次平移t之后得到新的位置x_i^\prime:
x_i\prime = Rx_i+t
對于每一個(gè)i=1,\dots,n有:
x_i = \begin{pmatrix} x_i\\ y_i\\ z_i \end{pmatrix}
需要注意的是旋轉(zhuǎn)之后平移與平移之后旋轉(zhuǎn)是不同的例如:
x\prime = R(x+s) = Rx+Rs = Rx + t\\ t = Rs
由于旋轉(zhuǎn)矩陣可以對任意形式的(列)向量組和進(jìn)行變換,這里我們考慮沿著x,y和z軸變換的情況,也就是笛卡爾坐標(biāo)系上進(jìn)行變換:
R\begin{pmatrix} x&y&z \end{pmatrix} = \begin{pmatrix} R_{11}&R_{12}&R_{13}\\ R_{21}&R_{22}&R_{23}\\ R_{31}&R_{32}&R_{33} \end{pmatrix} \begin{pmatrix} 1&0&0\\ 0&1&0\\ 0&0&1 \end{pmatrix} = \begin{pmatrix} r_1&r_2&r_3 \end{pmatrix}
因?yàn)橐婚_始的(列)向量組\begin{pmatrix}x&y&z\end{pmatrix}是正交的,并且是單位向量;所以最后的結(jié)果\begin{pmatrix}r_1&r_2&r_3\end{pmatrix}一定也是正交的單位向量組,然后一起組成了旋轉(zhuǎn)向量R。

之后考慮R的轉(zhuǎn)置R^T:
R^TR = \begin{pmatrix} r_1^T\\ r_2^T\\ r_3^T \end{pmatrix} \begin{pmatrix} r_1 r_2 r_3 \end{pmatrix}
因?yàn)?img class="math-inline" src="https://math.jianshu.com/math?formula=r_1%2Cr_2%E5%92%8Cr_3" alt="r_1,r_2和r_3" mathimg="1">都是單位向量并且相互正交,也就是r_1\cdot r_1 = 1, r_1 \cdot r_2 = 0,因此:
R^TR = \begin{pmatrix} 1&0&0\\ 0&1&0\\ 0&0&1 \end{pmatrix}
由此可以得到 R^T = R^{-1}即證旋轉(zhuǎn)矩陣是正交的。

對于行列式,我們知道矩陣的行列式是向量的混合積: r_1 \cdot (r_2 \times r_3)。這也表示以這些向量為邊構(gòu)成的平行六面體的體積。

<img src="https://i.loli.net/2020/04/03/ufa6XKNnlh7cxDI.png" alt="image-20200403184551535" />

由前面的結(jié)果可以得到R是正交矩陣,因此行列式為 +1或者-1;

emmm 看了很多解釋,我覺得都不是很清除[Rotations and rotation matrices][ 2 ],所以還是來算一下吧,具體看下面三種形式的旋轉(zhuǎn)矩陣:

image-20200403185756164

簡單的計(jì)算一下,例如R_x(a)按照第一行展開:
det(R_x(a)) = 1 * \big((cos \alpha) ^2 + (sin \alpha)^2 \big) == 1

3.2 設(shè)有四元數(shù) q,我們把虛部記為\varepsilon,實(shí)部記為 \eta,那么 q = (\varepsilon,\eta)。請說明 \varepsilon\eta 的維度[gaoxiang-cnblogs][3]

q = [\varepsilon, \eta], \ \ \varepsilon = q_o \in R, \ \ \eta=\begin{pmatrix} q_1,q_2,q_3\end{pmatrix} \in R^3

3.3 四元數(shù)運(yùn)算總結(jié):

這里可以簡單的擴(kuò)展一下四元數(shù)相關(guān)的知識:

一個(gè)四元數(shù)可以寫成如下形式:
q = \begin{pmatrix} a+id&-b-ic\\ b-ic&a-id \end{pmatrix} = aI +\ bi + \ cj + \ dk
其中:
I = \begin{pmatrix} 1&0\\ 0&1 \end{pmatrix}, \ i = \begin{pmatrix} 0&-1\\ 1&0 \end{pmatrix}, \ j = \begin{pmatrix} 0&-i\\ -i&0 \end{pmatrix}, \ k = \begin{pmatrix} i&0\\ 0&i \end{pmatrix}

這樣子我們就可以得到四元數(shù)的一些性質(zhì):
\begin{cases} \begin{equation} \begin{aligned} &j^2 = k^2 = -1 \\ \\&ij = k, jk = i, ki = j\\ \\&ji = -k, kj = -i, ik = -j \end{aligned} \end{equation} \end{cases}
對于四元數(shù)的運(yùn)算(加法、乘法、共軛、模)有:
\begin{cases} \begin{equation} \begin{aligned} & q = [\varepsilon, \eta], \ \ \varepsilon = q_o \in R, \ \ \eta=\begin{pmatrix} q_1,q_2,q_3\end{pmatrix} \in R^3 \\ \\& p + q = [\varepsilon_p + \varepsilon_q, \eta_p + \eta_q] \\ \\& pq = [\varepsilon_p, \eta_p][\varepsilon_q, \eta_q] = [\varepsilon_p\varepsilon_q - \eta_p \cdot \eta_q , \ \varepsilon_p \eta_q + \varepsilon_q \eta_p + \eta_p \times \eta_q] \\ \\&p \cdot q = p_0q_0 + p_1q_1i + p_2q_2j + p_3q_3k \\ \\& \overline p =[\varepsilon_p, -\eta_p] \\ \\ & \mid q \mid = \mid [s, v] \ \mid = \sqrt{s^2 + \mid v \ \mid ^2} \end{aligned} \end{equation} \end{cases}

所以對于 p q是指四元數(shù)每個(gè)位置上的數(shù)值分別相乘經(jīng)過一頓操作可以寫成上面的式子的簡潔形式注意與點(diǎn)乘區(qū)分。

其中:
\eta_p \times \eta_q = \begin{vmatrix} i&j&k\\ p_1&p_2&p_3\\ q_1&q_2&q_3 \end{vmatrix}
\eta_p \cdot \eta_q = p_1q_1 + p_2q_2 + p_3q_3

4、羅德里格斯公式的證明

參考: https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula

羅德里格斯公式提供了一種算法,可以計(jì)算從so(3)SO(3)的李代數(shù))到SO(3)的指數(shù)映射,而無需實(shí)際計(jì)算完整矩陣指數(shù)。令v\in R^3然后k是一個(gè)單位向量,描述了根據(jù)右手定則圍繞其旋轉(zhuǎn)角度θ的旋轉(zhuǎn)軸,例如\begin{bmatrix}1 \ 0 \ 0\end{bmatrix}^T \ \begin{bmatrix}0 \ 1 \ 0\end{bmatrix}^T \ \begin{bmatrix}0\ 0\ 1 \end{bmatrix}^T,那么旋轉(zhuǎn)向量v_{rot}的羅德里格斯公式的形式如下:
v_{rot} = v cos\theta + (k \times v)sin\theta + k(k\cdot v)(1-cos\theta)
首先,使用點(diǎn)積和叉積,向量v可以分為與軸k平行和垂直的分量:

v = v_{\parallel } + v_{\perp}

其中與k平行的分量為:v_{\parallel} = (v \cdot k)k

垂直于k的分量是vk上的向量投影:v_{\perp} = v - v_{\parallel} = v - (k \cdot v)k = -k \times (k \times v)

向量k \times v可以看作是v_{\perp}的副本,逆時(shí)針繞k旋轉(zhuǎn)了90°,因此它們的大小相等,但方向垂直。

同理可得,向量k \times(k \times v) 是將v的副本繞k逆時(shí)針旋轉(zhuǎn)到180°,因此k \times(k \times v)v_⊥的大小相等,但方向相反,所以是負(fù)號。

展開向量三乘積( vector triple product )可建立平行分量和垂直分量之間的連接,給定任何三個(gè)向量a,b,c公式的公式為a\times(b \times c)=(a \cdot c)b ?(a \cdot b)c 。

平行于軸的分量在旋轉(zhuǎn)下不會改變大小或方向:v_{\parallel rot} = v_{\parallel}

只有垂直分量會改變方向,但保持其大小:
\begin{cases} \mid v_{\perp rot} \mid = \mid v_{\perp} \mid, \\ \\ v_{\perp rot} = cos \theta v_{\perp} + sin \theta k \ \times \ v_{\perp} \end{cases}
由于 k \ \text{and} \ v_{\parallel}是平行的,因此他們的叉積為0:
\begin{cases} k \ \times \ v_{\perp} = k \ \times \ (v - v_{\parallel}) = k \ \times \ v - k \ \times v_{\parallel} = k \times v \\ \\ v_{\perp rot} = cos \theta v_{\perp} + sin \theta k \times v \end{cases}
旋轉(zhuǎn)分量的形式類似于笛卡爾基礎(chǔ)上二維平面極坐標(biāo)(r,θ)中的徑向矢量:
r = rcos\theta e_x + rsin\theta e_y
其中ex,ey是其指示方向上的單位向量。

現(xiàn)在完整的旋轉(zhuǎn)向量為:
v_{rot} = v_{\parallel rot} + v_{\perp rot}
通過將v_{∥rot}v_{⊥rot}的定義代入等式,得出:
\begin{equation} \begin{aligned} v_rot &= v_{\parallel} + cos \theta v_{\perp} + sin\theta \ (k \times v) \\ &= v_{\parallel} + cos\theta ({v - v_{\parallel}}) + sin\theta \ (k \times v ) \\ &= cos \theta v + (1-cos \theta)v_{\parallel} + sin \theta \ (k \times v) \\ &= cos \theta v + (1-cos \theta)(k \cdot v)k + sin \theta \ (k \times v) \end{aligned} \end{equation}

參考鏈接:

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者
【社區(qū)內(nèi)容提示】社區(qū)部分內(nèi)容疑似由AI輔助生成,瀏覽時(shí)請結(jié)合常識與多方信息審慎甄別。
平臺聲明:文章內(nèi)容(如有圖片或視頻亦包括在內(nèi))由作者上傳并發(fā)布,文章內(nèi)容僅代表作者本人觀點(diǎn),簡書系信息發(fā)布平臺,僅提供信息存儲服務(wù)。

相關(guān)閱讀更多精彩內(nèi)容

  • 旋轉(zhuǎn)矩陣 點(diǎn),向量,坐標(biāo)系 剛體不光有位置,還有自身的姿態(tài).位置是指剛體在空間中的哪個(gè)地方,姿態(tài)是指剛體的朝向. ...
    南衍兒閱讀 4,366評論 0 2
  • 作為備用知識,memo 學(xué)過矩陣?yán)碚摶蛘呔€性代數(shù)的肯定知道正交矩陣(orthogonal matrix)是一個(gè)非常...
    HappyPieBinLiu閱讀 6,195評論 0 5
  • 一.判別分析降維 LDA降維和PCA的不同是LDA是有監(jiān)督的降維,其原理是將特征映射到低維上,原始數(shù)據(jù)的類別也...
    wlj1107閱讀 12,378評論 0 4
  • 變換(Transformations) 我們可以嘗試著在每一幀改變物體的頂點(diǎn)并且重設(shè)緩沖區(qū)從而使他們移動,但這太繁...
    IceMJ閱讀 4,487評論 0 1
  • 想用一些外在的動力來激勵自己,現(xiàn)在基本上每周末都去書城看書,原來看書也是需要氛圍的,這些人像是一個(gè)個(gè)的善男信女,捧...
    清榮俊茂閱讀 176評論 0 1

友情鏈接更多精彩內(nèi)容