[toc]
第二講 三維空間的剛體運(yùn)動
1价说、熟悉 Eigen 矩陣運(yùn)算
設(shè)線性?程 棵磷,在 A 為?陣的前提下歹啼,請回答以下問題:
1.1 在什么條件下玄渗,x 有解且唯??
答案:非齊次線性方程組有唯一解的充要條件是 rank(A)=n狸眼。
1.2 ?斯消元法的原理是什么藤树?
答案:最基本的那種求方程解的方法,就是對矩陣進(jìn)行行變換拓萌。
1.3 QR 分解的原理是什么岁钓?
答案:在了解分解之前,先了解一下Gram-Schmidt正交化:
存在可逆矩陣的列向量組經(jīng)過正交化之后可以得到:
把上述式子使用矩陣表示就可以得到下面的定義
對于物理意義,有時(shí)候不是那么重要屡限,知道是這種數(shù)學(xué)表達(dá)就可以了品嚣。
定義:對于n階方陣A,若存在正交矩陣Q和上三角矩陣R钧大,使得翰撑,則該式稱為矩陣A的完全QR分解或正交三角分解。(對于可逆矩陣A存在完全QR分解)拓型。
基于我們熟悉的Gram-Schmidt正交化额嘿,令:
由此有若記,則此時(shí)劣挫。其中是由Gram-Schmidt正交化得到的標(biāo)準(zhǔn)的正交基册养。
1.4 Cholesky 分解的原理是什么?
可以閱讀Cholesky分解維基百科獲取更加詳細(xì)的解釋压固,下面只是為了便于理解而簡單的進(jìn)行說明球拦。
直接先說Cholesky 分解是用于求解這個(gè)方程,然后具有等式:
從式子可以看到就和代數(shù)的平方一樣的效果帐我,常用在優(yōu)化里面作為誤差坎炼;另外這個(gè)分解方法的優(yōu)點(diǎn)是提高代數(shù)運(yùn)算效率(矩陣求逆)、蒙特卡羅方法等場合中十分有用拦键。
其中是一個(gè)階厄米特正定矩陣(Hermitian positive-definite matrix)谣光,是下三角矩陣。下面介紹推導(dǎo)過程:
所以我們的目的是為了求矩陣芬为,算法由開始萄金,令:
在步驟中,矩陣的形式如下:
其中表示的共軛轉(zhuǎn)置媚朦,若是實(shí)數(shù)矩陣氧敢, 就是轉(zhuǎn)置;代表維的單位矩陣询张。此時(shí)定義為:
只有可以將矩陣改寫為:
我們發(fā)現(xiàn)這個(gè)和我們熟悉的的特征值分解結(jié)構(gòu)相似,接下來可以得到的形式:
需要注意的是這里的是一個(gè)外積孙乖。
重復(fù)此步驟,直到從1到份氧。步之后唯袄,我們可以得到。因此下三角矩陣為:
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)系中阎抒。?蘿??號的位姿為: ,( 的第?項(xiàng)為實(shí)部)。這?的 和表達(dá)的是 消痛,也就是世界到相機(jī)的變換關(guān)系且叁。?蘿? ?號的位姿為 ,。現(xiàn)在秩伞,?蘿??號看到某個(gè)點(diǎn)在??的坐 標(biāo)系下逞带,坐標(biāo)為,求該向量在?蘿??號坐標(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)矩陣 歪赢,證明 且
3.1.1 正交矩陣性質(zhì):
- 正交矩陣的每一個(gè)列向量都是單位向量,且向量之間兩兩正交单料。
- 正交矩陣的行列式為1或者-1.
- (充要條件)
3.1.2 證明旋轉(zhuǎn)矩陣是正交矩陣且行列式為1
首先我們令空間的一個(gè)位置經(jīng)過旋轉(zhuǎn)和一次平移之后得到新的位置:
對于每一個(gè)有:
需要注意的是旋轉(zhuǎn)之后平移與平移之后旋轉(zhuǎn)是不同的例如:
由于旋轉(zhuǎn)矩陣可以對任意形式的(列)向量組和進(jìn)行變換埋凯,這里我們考慮沿著軸變換的情況看尼,也就是笛卡爾坐標(biāo)系上進(jìn)行變換:
因?yàn)橐婚_始的(列)向量組是正交的递鹉,并且是單位向量;所以最后的結(jié)果一定也是正交的單位向量組藏斩,然后一起組成了旋轉(zhuǎn)向量躏结。
之后考慮的轉(zhuǎn)置:
因?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">都是單位向量并且相互正交,也就是,因此:
由此可以得到 狰域,即證旋轉(zhuǎn)矩陣是正交的媳拴。
對于行列式,我們知道矩陣的行列式是向量的混合積: 兆览。這也表示以這些向量為邊構(gòu)成的平行六面體的體積屈溉。
<img src="https://i.loli.net/2020/04/03/ufa6XKNnlh7cxDI.png" alt="image-20200403184551535" />
由前面的結(jié)果可以得到是正交矩陣,因此行列式為 +1或者-1抬探;
emmm 看了很多解釋子巾,我覺得都不是很清除[Rotations and rotation matrices][ 2 ]帆赢,所以還是來算一下吧,具體看下面三種形式的旋轉(zhuǎn)矩陣:
簡單的計(jì)算一下线梗,例如按照第一行展開:
3.2 設(shè)有四元數(shù) 椰于,我們把虛部記為,實(shí)部記為 仪搔,那么 瘾婿。請說明 和 的維度[gaoxiang-cnblogs][3]
3.3 四元數(shù)運(yùn)算總結(jié):
這里可以簡單的擴(kuò)展一下四元數(shù)相關(guān)的知識:
一個(gè)四元數(shù)可以寫成如下形式:
其中:
這樣子我們就可以得到四元數(shù)的一些性質(zhì):
對于四元數(shù)的運(yùn)算(加法、乘法烤咧、共軛偏陪、模)有:
所以對于 是指四元數(shù)每個(gè)位置上的數(shù)值分別相乘經(jīng)過一頓操作可以寫成上面的式子的簡潔形式注意與點(diǎn)乘區(qū)分。
其中:
4煮嫌、羅德里格斯公式的證明
參考: https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
羅德里格斯公式提供了一種算法笛谦,可以計(jì)算從(的李代數(shù))到的指數(shù)映射,而無需實(shí)際計(jì)算完整矩陣指數(shù)立膛。令然后是一個(gè)單位向量揪罕,描述了根據(jù)右手定則圍繞其旋轉(zhuǎn)角度θ的旋轉(zhuǎn)軸,例如宝泵,那么旋轉(zhuǎn)向量的羅德里格斯公式的形式如下:
首先好啰,使用點(diǎn)積和叉積,向量可以分為與軸平行和垂直的分量:
其中與平行的分量為:
垂直于的分量是在上的向量投影:
向量可以看作是的副本儿奶,逆時(shí)針繞旋轉(zhuǎn)了框往,因此它們的大小相等,但方向垂直闯捎。
同理可得椰弊,向量 是將的副本繞逆時(shí)針旋轉(zhuǎn)到180°,因此和的大小相等瓤鼻,但方向相反秉版,所以是負(fù)號。
展開向量三乘積( vector triple product )可建立平行分量和垂直分量之間的連接茬祷,給定任何三個(gè)向量公式的公式為 祭犯。
平行于軸的分量在旋轉(zhuǎn)下不會改變大小或方向:
只有垂直分量會改變方向秸妥,但保持其大小:
由于 是平行的,因此他們的叉積為0:
旋轉(zhuǎn)分量的形式類似于笛卡爾基礎(chǔ)上二維平面極坐標(biāo)中的徑向矢量:
其中是其指示方向上的單位向量。
現(xiàn)在完整的旋轉(zhuǎn)向量為:
通過將和的定義代入等式最盅,得出:
參考鏈接:
- https://www.cnblogs.com/indulge-code/p/10492209.html "東電逸仙-cnblog"
- https://onlinelibrary.wiley.com/doi/pdf/10.1107/S0907444901012410 "Rotations and rotation matrices"
- https://www.cnblogs.com/gaoxiang12/p/5120175.html " gaoxiang-cnblogs"