SLAM十四讲第二节课习题

VSLAM十四讲第二节课习题

1.矩阵A为满秩矩阵
2.利用初等行变换将方程组的增广矩阵转换为行阶梯阵，再通过回代求出线性方程组的解
3.对于n阶方阵A，若存在正交矩阵Q和上三角矩阵R，使得A = QR，则该式称为矩阵A的完全QR分解或正交三角分解。原理是将矩阵每个列作为一个基本单元，将其化为正交的基向量与在这个基向量上的投影长度的积
4.把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解
5.

useEigen.cpp

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

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 );
    clock_t time_str_1=clock();
    VectorXd x = A.colPivHouseholderQr().solve(B);//调用QR分解求解
    cout<<"time use in Qr composition is "<<1000*(clock()-time_str_1)/(double)CLOCKS_PER_SEC<<" ms"<<endl;
    clock_t time_str_2=clock();
    VectorXd y = A.llt().solve(B);                          //调用cholesky分解求解
    cout<<"time use in LLT composition is "<<100*(clock()-time_str_2)/(double)CLOCKS_PER_SEC<<" ms"<<endl;
    cout <<"A*x=B方程的解为\n"<< x << endl;
    cout <<"A*y=B方程的解为\n"<< y << endl;
    return 0;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( useEigen )
include_directories( "/usr/include/eigen3" )
add_executable( eigen 5.cpp )

xiaoluobo.cpp

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

using namespace std;
using namespace Eigen;

int main(int argc,char** argv)
{
    Quaternion<double> q1(0.55,0.3,0.2,0.2);//小萝卜一号相对于世界坐标系的四元数
    Quaternion<double> q2(-0.1,0.3,-0.7,0.2);//小萝卜二号相对于世界坐标系的四元数
    
    q1.normalize();//四元数归一化,四元数在使用前需要归一化
    q2.normalize();
   
    Vector3d t1(0.7,1.1,0.2);//小萝卜一号相对于世界坐标系的平移向量
    Vector3d t2(-0.1,0.4,0.8);//小萝卜二号相对于世界坐标系的平移向量
    
    Matrix3d R1=q1.toRotationMatrix();//将四元数转换为旋转矩阵
    Matrix3d R2=q2.toRotationMatrix();
    
    Isometry3d T1=Isometry3d::Identity();//定义并初始化小萝卜一号的欧式变换矩阵
    Isometry3d T2=Isometry3d::Identity();//定义并初始化小萝卜二号的欧式变换矩阵
    
    T1.rotate(R1);
    T1.pretranslate(t1);
    T2.rotate(R2);
    T2.pretranslate(t2);
   
    Vector3d p1(0.5,-0.1,0.2);
    Vector3d pw=T1.inverse()*p1;//点在世界坐标系的向量表示
    Vector3d p2=T2*pw;
    
    cout<<p2.transpose()<<endl;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( useEigen )
include_directories( "/usr/include/eigen3" )
add_executable( eigen xiaoluobo.cpp)