zoukankan      html  css  js  c++  java
  • 《视觉SLAM十四讲》课后习题—ch3

    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=
    "<<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;
    }

      

      

  • 相关阅读:
    Mysql数据备份命令
    git命令大全
    git 安装及普通命令
    git学习资料
    php 魔术方法 和 魔术常量
    react事件处理(绑定)
    java父类、子类构造函数调用过程
    java字符串String的intern()方法,举例说明
    java关键字static使用的“坑”(准备、初始化)
    java虚拟机
  • 原文地址:https://www.cnblogs.com/cc111/p/9240545.html
Copyright © 2011-2022 走看看