《视觉SLAM十四讲》课后习题—ch3

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

　　证明：旋转矩阵R=[e₁,e₂,e₃]^T[e^'₁,e^'₂,e^'₃]

　　　　　其中[e₁,e₂,e₃]^T是单位正交基，[e^'₁,e^'₂,e^'₃]是由[e₁,e₂,e₃]旋转得到的.

　　　　　=>RR^T=[e₁,e₂,e₃]^T[e'₁,e'₂,e'₃][e'₁,e'₂,e'₃]^T[e₁,e₂,e₃]=I(单位矩阵)

　　　　　=>R^-1=R^T=>旋转矩阵是正交矩阵

---

3.验证四元数旋转某个点后，结果是一个虚四元数（实部位为0），所以仍然对应到一个三维空间点

　　根据公式p'=qpq^-1，列出经四元数旋转后的p'，找出实部计算得0。

---

4.画表总结旋转矩阵、轴角、欧拉角、四元数的转换关系。

在网上看到的，整理的很好：https://blog.csdn.net/xuehuafeiwu123/article/details/74942989

代码实现见：旋转矩阵、旋转向量（轴角）、四元数、欧拉角之间相互转换的代码实现(利用Eigen实现)

---

 

5.假设有一个打的Eigen矩阵，想把它的左上角3X3的块取出来，然后赋值为I_{3x3  。}请编程实现。

// Eigen 几何模块
#include <Eigen/Dense>

#define Matrix_size 50

int main ( int argc, char** argv )
{
        Eigen::MatrixXd matrix_x;
        
        matrix_x=Eigen::MatrixXd::Random(Matrix_size,Matrix_size);
        
        //取出左上角3x3的块
        //方法1
        Eigen::Matrix3d m;
        m=matrix_x.topLeftCorner(3,3);
        cout<<m<<endl<endl;
        m=Eigen::Matrix3d::Identity(3,3);
/       cout<<m<<endl;
        //方法2：
        matrix_x.block<3,3>(0,0);
        cout<<matrix_x.block<3,3>(0,0).Identity()<<endl;
        return 0;
}

---

6.一般线性方程Ax=b有几种解法？你能在Eigen中实现吗？

#include <iostream>
using namespace std;

#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Dense>

#define Matrix_size 10

int main ( int argc, char** argv )
{
        Eigen::MatrixXd A=Eigen::MatrixXd::Random(Matrix_size,Matrix_size);
        Eigen::MatrixXd b=Eigen::MatrixXd::Random(Matrix_size,1);
        
        //1:直接求逆
        Eigen::Matrix<double,Matrix_size,1> x=A.inverse()*b;
        cout<<x.transpose()<<endl<<endl;
                
        //2:QR分解
        x=A.colPivHouseholderQr().solve(b);
        cout<<x.transpose()<<endl<<endl;
        
        //3:LLT分解
//        x=A.llt().solve(b);//要求矩阵正定
//        cout<<x.transpose()<<endl<<endl;
        
        //4: LDLT分解法
//        x=A.ldlt().solve(b);//要求矩阵正或负半定
//        cout<<x.transpose()<<endl<<endl;
        
        //5:
        x=A.fullPivHouseholderQr().solve(b);
        cout<<x.transpose()<<endl<<endl;
        
        //6:
        x=A.householderQr().solve(b);
        cout<<x.transpose()<<endl<<endl;
        
        //7:LU分解法
        x=A.householderQr().solve(b);
        cout<<x.transpose()<<endl<<endl;
        
        //8:
        x=A.partialPivLu().solve(b);
        cout<<x.transpose()<<endl<<endl;
        
        
        return 0;
}

---

 7.

　　主要是公式p_c=T_cwp_w的应用

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>


int main ( int argc, char** argv )
{
      Eigen::Quaterniond q1(0.2,0.3,0.1,0.35);

       Eigen::Isometry3d T=Eigen::Isometry3d::Identity();
       T.rotate(q1);
       T.pretranslate(Eigen::Vector3d(0.3,0.1,0.1));
//       cout<<"T=
"<<T.matrix()<<endl;
       Eigen::AngleAxisd v=Eigen::AngleAxisd(q1);
       Eigen::Matrix3d m=v.matrix();
       cout<<"m=
"<<m<<endl;
      Eigen::Vector3d p_c(0.5,0,0.2);
       Eigen::Vector3d p_w=T.inverse()*p_c;
      cout<<p_w<<endl;
       Eigen::Quaterniond q2(0.4,-0.1,0.2,-0.5);
       Eigen::Isometry3d T2=Eigen::Isometry3d::Identity();
      T2.rotate(q2);
       T2.pretranslate(Eigen::Vector3d(-0.1,0.5,0.3));
       cout<<"T2=
"<<T2.matrix()<<endl;
       Eigen::Vector3d p=T2*p_w;
      cout .precision(2);
      cout<<"p=
"<<p<<endl;
        return 0;
}