第3讲 三维空间刚体运动
3.1 旋转矩阵
3.1.1 点和向量、坐标系
位姿=位置+姿态
位置是指相机在空间中的哪个地方
姿态则是指相机的朝向
内积可以描述向量间的投影关系
外积表示向量的旋转
3.1.2 坐标系间的欧式变换
旋转矩阵R：行列式为1的正交矩阵
旋转矩阵可以描述相机的旋转
用一个旋转矩阵R和一个平移向量t完整地描述一个欧式空间的坐标变换关系
3.1.3 变换矩阵和齐次坐标
多次坐标系的变换用旋转矩阵表示则过于复杂，引入变换矩阵T
以下就是齐次坐标：
3.2 实践：Eigen
eigen库封装了矩阵和向量的运算
- 安装eigen
sudo apt install libeigen3-dev
- 查看是否安装成功和安装路径
sudo updatedb //更新文件 locate eigen3 //定位查找文件
代码详解：
声明部分
Eigen::Matrix<float,2,3> matrix_23; //2*3的矩阵 Eigen::Vector3d v_3d; //3*1的向量 double类型 Eigen::Matrix<float,3,1> vd_3d; Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //3*3矩阵；初始化为0 double类型 Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic; //动态大小的矩阵 Eigen::MatrixXd matrix_x;
初始化部分
matrix_23 << 1,2,3,4,5,6; //2*3 v_3d << 3,2,1; //3*1 vd_3d << 4,5,6; Eigen::Matrix<double,2,1> res = matrix_23.cast<double>() * v_3d; //显示类型转换 Eigen::Matrix<float,2,1> res2 = matrix_23 * v_3d;
一些四则运算
matrix_33 = Eigen::Matrix3d::Random(); //随机数矩阵 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; //行列式
特征值和特征向量
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver (matrix_33.transpose() * matrix_33); cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl; cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;
解方程：matrix_NN * x = v_Nd
Eigen::Matrix<double, 50, 50> matrix_NN; matrix_NN = Eigen::MatirxXd::Random(50, 50); Eigen::Matrix<double, 50, 1> v_Nd; v_Nd = Eigen::MatrixXd::Random(50, 1); //1. 直接求逆 Eigen::Matrix<double, 50, 1> x = matrix_NN.inverse() * v_Nd; //2. QR分解求 x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
3.3 旋转向量和欧拉角
3.3.1 旋转向量
旋转矩阵R用9个量表示3自由度的旋转，太过冗余
我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。称为旋转向量（Axis-Angle）
3.3.2 欧拉角
欧拉角也可以表示旋转
ZYX：偏航角yaw，俯仰角pitch，滚转角roll
欧拉角存在一个问题：万向锁问题，即在俯仰角为90°时，第一次旋转和第三次旋转为同一个轴，使得系统丢掉一个自由度
3.4 四元数
3.4.1 四元数的定义
- 旋转矩阵R使用9个量描述3自由度的旋转，具有冗余性
- 欧拉角和旋转向量是紧凑的，但具有奇异性
我们能用单位四元数表示三维空间的任意一个旋转。
虚四元数能对应一个空间点
可以用一个标量+一个向量表示四元数：
3.4.3 用四元数表示旋转
3.6 实践：Eigen几何模块
角轴、旋转矩阵、欧拉角、四元数都可以表示旋转；且可以相互转换
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity(); //初始化为单位矩阵 Eigen::AngleAxisd rotation_vector (M_PI/4, Eigen::Vector3d(0,0,1)); //绕Z轴旋转45度 cout .precision(3); //输出保留三位小数 //角轴->旋转矩阵 rotation_matrix = rotation_vector.matrix() rotation_matrix = rotation_vector.toRotationMatrix(); //两者效果一样 cout << "rotation matrix = \n" << rotation_matrix << endl;
Eigen::Vector3d v(1,0,0); //3*1 Eigen::Vector3d v_rotated = rotation_vector * v; //用角轴进行坐标变换 v_rotated = rotation_matrix * v; //用旋转矩阵进行坐标变换
//旋转矩阵->欧拉角 Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0); cout << "yaw pitch roll = " euler_angles.transpose() << endl; //偏航角、俯仰角、滚转角
//变换矩阵 Eigen::Isometry3d T = Eigen::Isometry3d::identity(); T.rotate(rotation_vector); //旋转 T.pretranslate(Eigen::vector3d(1,3,4)); //平移 cout << "Transform matrix = \n" << T.matrix() << endl;
//用变换矩阵进行坐标变换 Eigen::Vector3d v_transformed = T * v; //相当于R*v+t cout << "v tranfromed = " << v_transformed.transpose() << endl;
//四元数 Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector); //角轴->四元数 q = Eigen::Quaterniond(rotation_matrix); //旋转矩阵->四元数 cout << q.coeffs() << endl; //使用四元数进行旋转 v_rotated = q * v;
3.7 可视化演示
这一小节有几个小问题
1. Pangolin的安装
请查看我的另一篇博客：
https://blog.csdn.net/qq_39236499/article/details/122537176?spm=1001.2014.3001.5501
2. 跑3.7.2的代码编译可能的报错情况
terminate called after throwing an instance of std::runtime_error what(): Pangolin X11: Unable to retrieve framebuffer options
我是报了上面这个错误
说明下情况：我是在虚拟机上的Ubuntu16.04
3. 对应报错解决方法
参考博客：https://blog.csdn.net/weixin_44671418/article/details/109232671
步骤流程：
- 安装个东西
sudo apt-get install mesa-utils
- 找到对应文件并打开
- 注释掉两行内容
staticint visual_attribs[] = { GLX_X_RENDERABLE , True, GLX_DRAWABLE_TYPE , GLX_WINDOW_BIT, GLX_RENDER_TYPE , GLX_RGBA_BIT, GLX_X_VISUAL_TYPE , GLX_TRUE_COLOR, GLX_RED_SIZE , 8, GLX_GREEN_SIZE, 8, GLX_BLUE_SIZE, 8, GLX_ALPHA_SIZE, 8, GLX_DEPTH_SIZE, 24, GLX_STENCIL_SIZE, 8, GLX_DOUBLEBUFFER , glx_doublebuffer ? True : False, //注释这一行GLX_SAMPLE_BUFFERS , glx_sample_buffers, //注释这一行 GLX_SAMPLES , glx_sample_buffers > 0 ? glx_samples : 0, None };
- 重新遍历Pangolin和安装
cd [path-to-pangolin] mkdir build cd build cmake .. make sudo make install
- 重新编译3.7.2的demo，成功实现！
