第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,成功实现!