0
点赞
收藏
分享

微信扫一扫

第3讲 三维空间刚体运动


第3讲 三维空间刚体运动

3.1 旋转矩阵

3.1.1 点和向量、坐标系

位姿=位置+姿态

位置是指相机在空间中的哪个地方

姿态则是指相机的朝向

内积可以描述向量间的投影关系

外积表示向量的旋转



3.1.2 坐标系间的欧式变换

旋转矩阵R:行列式为1的正交矩阵

旋转矩阵可以描述相机的旋转

用一个旋转矩阵R和一个平移向量t完整地描述一个欧式空间的坐标变换关系



3.1.3 变换矩阵和齐次坐标

多次坐标系的变换用旋转矩阵表示则过于复杂,引入变换矩阵T

以下就是齐次坐标:

第3讲 三维空间刚体运动_3d


3.2 实践:Eigen

eigen库封装了矩阵和向量的运算

  1. 安装eigen

sudo apt install libeigen3-dev

  1. 查看是否安装成功和安装路径

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讲 三维空间刚体运动_矩阵_02

第3讲 三维空间刚体运动_矩阵_03

可以用一个标量+一个向量表示四元数:

第3讲 三维空间刚体运动_旋转矩阵_04



3.4.3 用四元数表示旋转

第3讲 三维空间刚体运动_线性代数_05


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的安装


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. 对应报错解决方法


步骤流程:

  1. 安装个东西

sudo apt-get install mesa-utils

  1. 找到对应文件并打开
  2. 注释掉两行内容

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

  1. 重新遍历Pangolin和安装

cd [path-to-pangolin]
mkdir build
cd build
cmake ..
make
sudo make install

  1. 重新编译3.7.2的demo,成功实现!



举报

相关推荐

0 条评论