【SLAM14讲CH3】习题

CH3 习题

1.验证旋转矩阵是正交矩阵

正交矩阵：
1.行列式为1
2.每个向量都是单位向量且相互正交
3.其逆等于其转置

简单理解：先说一下什么是旋转矩阵？如图1所示，我们假设最开始空间的坐标系$X_A Y_A Z_A $就是笛卡尔坐标系，这样我们得到空间A的矩阵其实也可以看做是单位阵\ast \ast E\ast \ast 。进过旋转后，空间A的三个坐标系变成了图1中红色的三个坐标系$X_B Y_B Z_B $，得到空间B的矩阵$V_B=(X_B,Y_B,Z_B)^{T}$。我们将两个空间联系起来可以得到$V_B=R\cdot V_A$，这里R就是我们所说的旋转矩阵。

由于A=E，结合图2可以看出，旋转矩阵R就是由$X_B Y_B Z_B $三个向量组成的。讲到这里，大家应该会发现旋转矩阵R满足第一个条件，因为单位向量无论怎么旋转长度肯定不会变而且向量之间的正交性质也不会变。那么旋转矩阵就是正交阵！

更详细的说明：旋转矩阵的性质分析

2.寻找罗德里格斯公式的推导过程并理解它

罗德格里斯旋转公式推导
以后再看

3.验证四元数旋转某个角后，结果是一个虚四元数，所以仍然对应到一个三维空间点

四元数旋转运算过程

4.画表总结旋转矩阵，轴角、欧拉角、四元数对应关系

5.假设我有一个大的 Eigen 矩阵,我想把它的左上角 3 × 3 的块取出来,然后赋值为I3×3 。请编程实现此事。

提取块：matrix.block(i,j,p,q)提取起始于(i,j)大小为(p,q)的块

#include <iostream>
#include <Eigen/Core>
using namespace std;
using namespace Eigen;
#define MS 10
int main() {
    Matrix<double,MS,MS> M1=MatrixXd::Random(MS,MS);
    Matrix3d M2=M1.block(0,0,3,3);
    cout.precision(3);
    cout<<M1<<endl;
    cout<<M2<<endl;
    //分块赋值
    M1.block<3,3>(0,0)=MatrixXd::Identity(3,3);
    cout<<M1<<endl;
    return 0;
   }

6.一般线性方程组$Ax=b$有哪些做法

视觉slam十四讲课后习题ch3-6

7.小萝卜一号和小萝卜二号

小萝卜一号：位姿$q_1=[0.35,0.2,0.3,0.1],t_2=[0.3,0.1,0.1]$
小萝卜二号：位姿$q_2=[-0.5,0.4,-0.1,0.2],t_2=[-0.1,0.5,0.3]$
某点在小萝卜一号的坐标系下坐标为$p=[0.5,0,0.2]$
求该点在小萝卜二号坐标系下的坐标

思路 由$q_1,q_2$求对应旋转矩阵，由旋转矩阵和平移向量构造变换矩阵。由变换矩阵转换两个坐标系之间的坐标。

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;
#define MS 10
int main() {
    //定义两个四元数
    Quaterniond q1(0.35,0.2,0.3,0.1);
    Quaterniond q2(-0.5,0.4,-0.1,0.2);

    //定义两个平移向量
    Vector3d t1(0.3,0.1,0.1);
    Vector3d t2(-0.1,0.5,0.3);

    q1.normalize();
    q2.normalize();

    Isometry3d T1=Isometry3d::Identity();
    T1.rotate(q1);
    T1.pretranslate(t1);
    Isometry3d T2=Isometry3d::Identity();;
    T2.rotate(q2);
    T2.pretranslate(t2);
    
    Vector3d p(0.5,0,0.2);
    Vector3d p2=T2*T1.inverse()*p;
    cout<<p2.transpose()<<endl;
    return 0;
   }