1. 程式人生 > >SLAM十四講第二次作業-深藍學院

SLAM十四講第二次作業-深藍學院

在這裡插入圖片描述

  1. r(A)=n:矩陣A的秩等於未知數的個數。
  2. ⾼斯消元法:通過用初等行變換將增廣矩陣化為行階梯陣,然後通過回代求解線性方程組的解。原理是將方程組中每個方程含有的未知數的個數降到最低,並且最下面的方程含有的未知數的個數最少。
  3. QR分解:把矩陣分解成一個列向量正交矩陣與一個上三角矩陣的積。原理是將矩陣每個列作為一個基本單元,將其化為正交的基向量與在這個基向量上的投影長度的積。
  4. Cholesky 分解:將一個對稱正定矩陣分解成一個下三角矩陣與其共軛轉置之乘積。
  5. 程式碼見:eigenMatrix.cpp與CMakeLists.txt
    eigenMatrix.cpp檔案
/*求解 A * x = B 這個方程*/

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Cholesky>

using namespace std;
using namespace Eigen;

#define MATRIX_SIZE 100

int main( int argc,char** argv )
{
    MatrixXd A_pre = MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE )
; MatrixXd A = A_pre.transpose()*A_pre ; //使得A為正定對稱矩陣,才能使得cholesky分解成功 VectorXd B = VectorXd::Random( MATRIX_SIZE ); VectorXd x = A.colPivHouseholderQr().solve(B); //呼叫QR分解求解 VectorXd y = A.llt().solve(B); //呼叫cholesky分解求解 cout <<
"A*x=B方程的解為\n"<< x << endl; cout <<"A*y=B方程的解為\n"<< y << endl; }

CMaleLists.txt檔案

cmake_minimum_required( VERSION 2.8 )
project( useEigen )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )

# 新增Eigen標頭檔案
include_directories( "/usr/include/eigen3" )

add_executable( eigen eigenMatrix.cpp )

在這裡插入圖片描述
程式碼見:useGeometry.cpp與CMakeLists.txt

useGeometry.cpp檔案

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

using namespace std;
using namespace Eigen;

int main(int arcg,char** argv)
{
    Quaterniond q1 = Quaterniond(0.55,0.3,0.2,0.2).normalized();   //定義並歸一化四元數
    Quaterniond q2 = Quaterniond(-0.1,0.3,-0.7,0.2).normalized();
    Vector3d t1 ,t2,p1,p,p2;
    t1 << 0.7,1.1,0.2;
    t2 << -0.1,0.4,0.8;
    p1 << 0.5,-0.1,0.2;
  

    Isometry3d T_cw1 = Isometry3d::Identity();
    T_cw1.rotate ( q1 );
    T_cw1.pretranslate ( t1 );

    Isometry3d T_cw2 = Isometry3d::Identity();
    T_cw2.rotate ( q2 );
    T_cw2.pretranslate ( t2 );



    p = T_cw1.inverse() *p1;   //不能用轉置,因為這裡不是純旋轉,見書42頁
    p2 = T_cw2 *p ;

    cout<<"p2 = "<<p2.transpose()<<endl;
}

CMakeLists.txt檔案

cmake_minimum_required( VERSION 2.8 )
project( geometry )

# 新增Eigen標頭檔案
include_directories( "/usr/include/eigen3" )

add_executable( eigenGeometry eigenGeometry.cpp )

在這裡插入圖片描述

  1. 證明
    設某個單位正交基 ( e 1 , e 2 , e 3 ) (e_1,e_2,e_3) 經過一次旋轉變成了 ( f 1 , f 2 , f 3 ) (f_1,f_2,f_3) ,那麼旋轉矩陣
    R = [ e 1 T e 2 T e 3 T ] [ f 1 f 2 f 3 ] [ e 1 T e 2 T e 3 T ] [ e 1 e 2 e 3 ] = I R T R = [ f 1 T f 2 T f 3 T ] [ e 1 e 2 e 3 ] [ e 1 T e 2 T e 3 T ] [ f 1 f 2 f 3 ] = [ f 1 T f 2 T f 3 T ] I [ f 1 f 2 f 3 ] = I R T R = I R = \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \cdot \begin{bmatrix} f_1 &amp; f_2 &amp;f_3 \end{bmatrix} \\ \because \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \cdot \begin{bmatrix} e_1&amp; e_2 &amp;e_3 \end{bmatrix} = I \\ \begin{aligned}\therefore R^T \cdot R &amp;= \begin{bmatrix} f_1^T \\ f_2^T \\ f_3^T \end{bmatrix} \cdot \begin{bmatrix} e_1 &amp; e_2 &amp; e_3 \end{bmatrix} \cdot \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \cdot \begin{bmatrix} f_1 &amp; f_2 &amp;f_3 \end{bmatrix} \\ &amp;= \begin{bmatrix} f_1^T \\ f_2^T \\ f_3^T \end{bmatrix} \cdot I \cdot \begin{bmatrix} f_1 &amp; f_2 &amp;f_3 \end{bmatrix} \\ &amp;= I \end{aligned} \\ \therefore R^T \cdot R=I