《视觉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  。}请编程实现。

 1 // Eigen 几何模块
 2 #include <Eigen/Dense>
 3 
 4 #define Matrix_size 50
 5 
 6 int main ( int argc, char** argv )
 7 {
 8         Eigen::MatrixXd matrix_x;
 9         
10         matrix_x=Eigen::MatrixXd::Random(Matrix_size,Matrix_size);
11         
12         //取出左上角3x3的块
13         //方法1
14         Eigen::Matrix3d m;
15         m=matrix_x.topLeftCorner(3,3);
16         cout<<m<<endl<endl;
17         m=Eigen::Matrix3d::Identity(3,3);
18 /       cout<<m<<endl;
19         //方法2：
20         matrix_x.block<3,3>(0,0);
21         cout<<matrix_x.block<3,3>(0,0).Identity()<<endl;
22         return 0;
23 }

 

---

 

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

 1 #include <iostream>
 2 using namespace std;
 3 
 4 #include <Eigen/Core>
 5 // Eigen 几何模块
 6 #include <Eigen/Dense>
 7 
 8 #define Matrix_size 10
 9 
10 int main ( int argc, char** argv )
11 {
12         Eigen::MatrixXd A=Eigen::MatrixXd::Random(Matrix_size,Matrix_size);
13         Eigen::MatrixXd b=Eigen::MatrixXd::Random(Matrix_size,1);
14         
15         //1:直接求逆
16         Eigen::Matrix<double,Matrix_size,1> x=A.inverse()*b;
17         cout<<x.transpose()<<endl<<endl;
18                 
19         //2:QR分解
20         x=A.colPivHouseholderQr().solve(b);
21         cout<<x.transpose()<<endl<<endl;
22         
23         //3:LLT分解
24 //        x=A.llt().solve(b);//要求矩阵正定
25 //        cout<<x.transpose()<<endl<<endl;
26         
27         //4: LDLT分解法
28 //        x=A.ldlt().solve(b);//要求矩阵正或负半定
29 //        cout<<x.transpose()<<endl<<endl;
30         
31         //5:
32         x=A.fullPivHouseholderQr().solve(b);
33         cout<<x.transpose()<<endl<<endl;
34         
35         //6:
36         x=A.householderQr().solve(b);
37         cout<<x.transpose()<<endl<<endl;
38         
39         //7:LU分解法
40         x=A.householderQr().solve(b);
41         cout<<x.transpose()<<endl<<endl;
42         
43         //8:
44         x=A.partialPivLu().solve(b);
45         cout<<x.transpose()<<endl<<endl;
46         
47         
48         return 0;
49 }

---

 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=\n"<<T.matrix()<<endl;
       Eigen::AngleAxisd v=Eigen::AngleAxisd(q1);
       Eigen::Matrix3d m=v.matrix();
       cout<<"m=\n"<<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=\n"<<T2.matrix()<<endl;
       Eigen::Vector3d p=T2*p_w;
      cout .precision(2);
      cout<<"p=\n"<<p<<endl;
        return 0;
}

　　

　　

 

转载于:https://www.cnblogs.com/cc111/p/9240545.html