1.证明旋转矩阵是正交矩阵
证明:旋转矩阵R=[e1,e2,e3]T[e'1,e'2,e'3]
其中[e1,e2,e3]T是单位正交基,[e'1,e'2,e'3]是由[e1,e2,e3]旋转得到的.
=>RRT=[e1,e2,e3]T[e'1,e'2,e'3][e'1,e'2,e'3]T[e1,e2,e3]=I(单位矩阵)
=>R-1=RT=>旋转矩阵是正交矩阵
3.验证四元数旋转某个点后,结果是一个虚四元数(实部位为0),所以仍然对应到一个三维空间点
根据公式p'=qpq-1,列出经四元数旋转后的p',找出实部计算得0。
4.画表总结旋转矩阵、轴角、欧拉角、四元数的转换关系。
在网上看到的,整理的很好:https://blog.csdn.net/xuehuafeiwu123/article/details/74942989
代码实现见:旋转矩阵、旋转向量(轴角)、四元数、欧拉角之间相互转换的代码实现(利用Eigen实现)
5.假设有一个打的Eigen矩阵,想把它的左上角3X3的块取出来,然后赋值为I3x3 。请编程实现。
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.
主要是公式pc=Tcwpw的应用
#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; }