slam(刚体运动和矩阵变换)

作业:

r(A)=n:矩阵A的秩等于未知数的个数。
⾼斯消元法：通过用初等行变换将增广矩阵化为行阶梯阵，然后通过回代求解线性方程组的解。原理是将方程组中每个方程含有的未知数的个数降到最低，并且最下面的方程含有的未知数的个数最少。
QR分解：把矩阵分解成一个列向量正交矩阵与一个上三角矩阵的积。原理是将矩阵每个列作为一个基本单元，将其化为正交的基向量与在这个基向量上的投影长度的积。
Cholesky 分解：将一个对称正定矩阵分解成一个下三角矩阵与其共轭转置之乘积。
代码如下：

#include <iostream>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace Eigen;
#define MATRIX_SIZE 100
/*求解 A * x = B 这个方程*/
int main(int argc,char **argv)
{

    Eigen::Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_A=MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    Eigen::Matrix<double,MATRIX_SIZE,1> matrix_B=MatrixXd::Random(MATRIX_SIZE,1);
    Eigen::Matrix<double,MATRIX_SIZE,1> matrix_X;

    //直接求逆
    matrix_X=matrix_A.inverse()*matrix_B;
    cout<<matrix_X<<endl;
    cout<<"求逆"<<endl;
    //QR分解
    matrix_X=matrix_A.colPivHouseholderQr().solve(matrix_B);
    cout<<matrix_X<<endl;
    cout<<"QR"<<endl;
    //llt分解
    matrix_X=matrix_A.llt().solve(matrix_B);
    cout<<matrix_X<<endl;
    cout<<"llt"<<endl;



    return 0;
}

cmake_minimum_required( VERSION 2.8 )
project(test1)
set( CMAKE_BUILD_TYPE "Debug" )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
add_executable(test1 src/main.cpp)

第2题

#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;
}

其实问题转化成已知两个坐标系相对世界坐标系的变换矩阵，且已知一点在某个坐标系的坐标，求在另外一个坐标系下的该点的坐标。

p*A=p~*B,求解p~=p*A*B_1;

上面的程序的思路也是如此，先将4元素转换为旋转矩阵，在转换为转换矩阵。

第3题