视觉SLAM十四讲-高翔 ch3

最新推荐文章于 2024-06-30 14:43:38 发布

博智同学Studying

最新推荐文章于 2024-06-30 14:43:38 发布

阅读量230

点赞数

文章标签：笔记

本文链接：https://blog.csdn.net/xiao_xian_233/article/details/132619650

版权

文章目录

前言
代码
- 实践1：
- 实践2：
遗留问题：

前言

第三章介绍了三维空间刚体运动，包括数理基础和程序实现。
个人认为最难啃的部分是数理基础。在读书的时候发觉上学期学的线代基本忘光了（明明感觉当时考的还不赖来着o(╥﹏╥)o）
但正好借此机会复习+实践学过的数学知识，能够把理论应用起来也挺好的。
不过也多亏上学期看的3b1b的《线性代数的本质》让我能够理解矩阵左乘对空间变换的意义（推荐看b站up主汉语配音的）
这一章啃了大概两天半才啃完（还没看带星号的部分）（包括复习上学期线代的时间）终于可以看下一章了！

代码

这次大部分知识点都写在代码注释里了。文字总结。。。以后再说吧

实践1：

#include<iostream>
#include<ctime>

using namespace std;

#include<Eigen/Core>
#include<Eigen/Dense>  //Eigen头文件，包含Eigen库里所有的类

#define MATRIX_SIZE 50;

int main( int argc, char** argv){

    Eigen::Matrix<float, 2, 3> matrix_23;//float类型的 2*3矩阵

    Eigen::Vector3d v_3d;//列向量 

    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
/*
Matrix3d：Eigen库中typedef的数据类型，即“3*3矩阵（d表示矩阵内部数据类型为double类型）”

rotation_matrix：该3*3矩阵的变量名
*/

    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;

    Eigen::MatrixXd matrix_x;
/*
动态矩阵在编译的时候不知道其大小，需要在运行的时候才能确定其大小。
typedef Matrix <double，Dynamic，Dynamic> MatrixXd;
例如我们可以这样定义一个动态矩阵：

MatrixXd m(3,4) ;  // 指定矩阵大小为3X4，也可以不指定

*/

    matrix_23 << 1, 2, 3, 4, 5, 6;

    //逗号初始化：
/*
  Eigen提供了一种逗号初始化器语法，该语法使用户可以轻松设置矩阵，向
量或数组的所有系数。只需列出系数，从左上角开始，从左到右，从上到下
移动。需要预先指定对象的大小。如果列出的系数太少或太多，编译器就会报错。
  此外，初始化列表的元素本身可以是向量或矩阵。通常的用途是将向量或矩
阵连接在一起。请记住，必须先设置大小，然后才能使用逗号初始化程序。

*/
    cout<< matrix_23 << endl;//Eigen重载了<<运算符，可直接输出矩阵的值
/*
重载运算符是指在C++中可以为已有的运算符重新定义新的行为。重载运算符的操作数至少
有一个是用户定义的类型，并且不能违反运算符原来的语法规则。需要注意的是，不能创建新
的运算符，而且有一些运算符是不能被重载的，例如sizeof。另外，=、()、[]、->操作符
只能被类的成员函数重载。
*/

    for(int i=0; i<1;i++)
        for(int j=0;j<2;j++)
            cout<<matrix_23(i,j)<<endl;//matrix_23(i,j)表示i行j列
    v_3d<< 3,2,1;
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;//cast将矩阵里的值的类型都转换成了double
    cout <<endl<<result << endl;
/* 矩阵和向量相乘（实际上仍是矩阵和矩阵）
   但是在这里你不能混合两种不同类型的矩阵，像这样是错的(matrix_23的值是float类型，v_3d的是double类型)
   Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
*/  
    Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
    matrix_33 = Eigen::Matrix3d::Random();
    cout<<matrix_33<<endl<<endl<<endl<<endl;

    cout<<matrix_33.transpose() <<endl;
    cout<<matrix_33.sum()<<endl;
    cout<<matrix_33.trace()<<endl;
    cout<<10*matrix_33<<endl;
    cout<<matrix_33.inverse()<<endl;
    cout<<matrix_33.determinant() <<endl<<endl<<endl;

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver (matrix_33.transpose()*matrix_33);
    cout<< "Eigen values = " << eigen_solver.eigenvalues() <<endl;
    cout<< "Eigen vectors = " << eigen_solver.eigenvectors() <<endl;


    
    return 0;
}

实践2：

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

#include<Eigen/Core>
#include<Eigen/Geometry>//此模块提供了各种旋转和平移的表示

// 几何模块的用法

int main(){

    //3d旋转矩阵直接使用Matrix3d或matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    //Matrix3d::Identity()：在定义该矩阵变量时，创建一个同尺寸同数据类型的单位阵，对其初始化。
    //线性代数有时把单位阵用I表示，应该就取自Identity的首字母吧？ 

    //旋转向量使用AngleAxis,它底层不直接是 Matrix ，但运算可以当作矩阵（因为重载了运算符)
    Eigen::AngleAxisd rotation_vector (M_PI/4, Eigen::Vector3d (0,0,1));//沿Z轴旋转45°
    cout .precision(3);
/*
    代表有效数字是3位
    想要保留有效的数据位数，通过cout.precision(var);   var 代表有效位数
*/

    cout<<"rotation matrix =\n"<<rotation_vector.matrix()<<endl;//用.matrix()转换成矩阵
    rotation_matrix = rotation_vector;

    rotation_matrix = rotation_vector.toRotationMatrix();
    //用旋转向量给旋转矩阵赋值

    Eigen::Vector3d v (1,0,0);
    Eigen::Vector3d v_rotated = rotation_vector * v;//用旋转向量表示旋转 


    cout<<"(1,0,0) after rotation ="<<v_rotated.transpose()<<endl;
    
    v_rotated = rotation_matrix * v;//用旋转矩阵表示旋转.
    cout<<"(1,0,0) after rotation ="<<v_rotated.transpose()<<endl;

    //可用旋转矩阵直接转换成欧拉角    
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles( 2,1,0 );//按ZYX顺序转   euler即欧拉

    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;//为什么去掉.transpose()就编译错误？？？？

    //欧氏变换矩阵使用Eigen::Isometry3d
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();//虽为3d，实际上是 4*4
    T.rotate (rotation_vector) ;//按照rotation_vector进行旋转
    T.pretranslate(Eigen::Vector3d ( 1,3,4 ) );//按照（ 1，3，4 ）进行平移
    cout << "Transform matrix = \n"<< T.matrix()<<endl;
    
    //用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T*v;
    cout<<"v transformed = "<<v_transformed.transpose()<<endl;

    //四元数
    //可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond (rotation_vector);
    cout<<"quaternion = \n"<<q.coeffs()<<endl;//coeffs的顺序是(x,y,z,w),w为实部，x、y、z为虚部
    
    //亦可将旋转矩阵赋给它
    q = Eigen::Quaterniond( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs()<<endl;

    //使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v;// 数学上是qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;


    return 0;
}
/*

Eigen 中对各种形式的表达方式总结如下。请注意每种类型都有单精度和双精度两种
数据类型，而且和之前一样，不能由编译器自动转换。下面以双精度为例，你可以把最后
的 d 改成 f，即得到单精度的数据结构。
• 旋转矩阵（3 × 3）
：Eigen::Matrix3d。
• 旋转向量（3 × 1）
：Eigen::AngleAxisd。
• 欧拉角（3 × 1）
：Eigen::Vector3d。
• 四元数（4 × 1）
：Eigen::Quaterniond。
• 欧氏变换矩阵（4 × 4）
：Eigen::Isometry3d。
• 仿射变换（4 × 4）
：Eigen::Affine3d。
• 射影变换（4 × 4）
：Eigen::Projective3d。

*/