电子工程师技术服务社区
公告
登录
|
注册
首页
技术问答
厂商活动
正点原子
板卡试用
资源库
下载
文章
社区首页
文章
【7天搞定视觉SLAM】第二天——三维转换
分 享
扫描二维码分享
【7天搞定视觉SLAM】第二天——三维转换
slam
hero_chao
关注
发布时间: 2020-04-20
丨
阅读: 1160
# 一、什么是三维空间? ## 1.三维空间简介 > 三维空间,日常生活中可指由长、宽、高三个维度所构成的空间,是我们看得见感受得到的空间。三维的东西能够容纳二维。三维空间的长、宽、高三条轴是说明在三维空间中的物体相对原点O的距离关系。将一些橡皮绳按经纬线的样式编成一张网,将之张平,我们可以将之近似看作是二维平面,然后将一个小球放在网上,橡皮网在小球的重力作用下凹陷,这就形成了三维空间 ![](https://cf03.ickimg.com/bbsimages/202004/ae90aff55fb363b7c80507982822c3e5.png) ## 2.三维空间和SLAM的关系 在空间中的三维点就是我们需要找到如何去进行立体建模的点空间中的三维点,通过一系列的转换。就可以达到我们的相机的二维坐标点,这就是需要我们在视觉slam中首先做的工作及前端视觉里程计的基础。 由于在视觉slam构建当中,我们需要关注相机所在位置和这个相机的姿态。那么我们可以用一组点和向量来表示相机,点就是这个相机所在的空间的位置。而这个向量就是我这个相机所对应的朝向,这样就可以表现出我们相机的一个位置描述。 # 二、什么是刚体运动? ## 1.刚体运动简介 > 刚体运动(rigid motion)在三维空间中, 把一个几何物体作旋转, 平移的运动,称之为刚体变换。 刚体运动也可以理解为保持长度,角度,面积等不变的仿射变换, 即保持内积和度量不变。 从坐标变换上看, 旋转对应行列式为1的正交矩阵。 此外,刚体变换下, 具有物理意义的量,如梯度,散度和旋度都保持不变。 从群的角度看,刚体变换全体构成一个群。 著名的克莱因(Klein)纲领,就是说要把所有不同的几何学性质 看成是在某种群作用下不变的东西--即不变量。 人体各个部位的运动是刚体运动;而人整体的运动是非刚体运动; ![](https://cf03.ickimg.com/bbsimages/202004/f5084ff90dcfe768174e8cd4a796b2ce.png) ## 2.旋转矩阵 我们将空间中的三维点在的坐标系称为世界坐标系。我们将以相机所看出的空间的坐标系为相机坐标系。 从空间坐标写从世界坐标系到相机坐标系的转换,我们可以用一个大写的T来表示,而这个T就是一个转换矩阵。由刚体运动我们知道坐标系的转换,这个大写的矩阵T即是由旋转矩阵和平移矩阵构成。 > 世界坐标系是系统的绝对坐标系,在没有建立用户坐标系之前画面上所有点的坐标都是以该坐标系的原点来确定各自的位置的。 ![](https://cf03.ickimg.com/bbsimages/202004/410ca5329482bcaa47c3bee9a30c39ec.png) ![](https://cf03.ickimg.com/bbsimages/202004/9ffa684e039a9026e1d8842354e3066e.png) ![](https://cf03.ickimg.com/bbsimages/202004/c7c5a9434ab2e8543a52727add6896d1.png) 以上三幅图可以帮我们很好的理解刚体运动是旋转矩阵和平移矩阵的构成,想必大家已经有了深刻的理解,如果对这方面还数学推论有更感兴趣可以搜索相关资料,这里就不做过多介绍。 # 三、什么是欧拉角? ## 1.旋转 刚体运动是由一个旋转矩阵和平移矩阵构成,那任何一个旋转其实都是可以用一个旋转轴加一个旋转角来描述这个旋转矩阵的。 从旋转向量到旋转矩阵的转换,大家可以参考罗德里格斯公式,那里详细的描述了如何从一个旋转向量(一个长度为n的向量加一个角度为O的值),旋转向量到一个旋转矩阵转换有详尽的描述,感兴趣的朋友可以参考一下罗德里格斯公式。 ## 2.欧拉角 > 欧拉角是用来唯一地确定定点转动刚体位置的三个一组独立角参量,由章动角θ、进动角ψ和自转角φ组成,为L.欧拉首先提出,故得名。 欧拉角的描述有多种多样的描述,以一种非常简单明白的方式描述了旋转,就是把它分成三个分离的转角,通过绕这个三个轴的旋转,来描述一个旋转。如果大家曾经玩过无人机,应该对这方面比较了解,比如我们可以举一个欧拉角的例子,绕物体的z轴旋转,我们可以得到一个偏航角,绕旋转之后的y轴继续旋转,我们就可以得到他的俯仰角。绕旋转之后的x轴继续旋转,我们就可以得到它的滚转角,通过这几个角度就可以描述一个旋转,而这也可以方便的来描述旋转问题。 # 四、什么是四元素? ## 1.四元数 > 四元数是简单的超复数。 复数是由实数加上虚数单位 i 组成,其中i^2 = -1。 相似地,四元数都是由实数加上三个虚数单位 i、j、k 组成,而且它们有如下的关系: i^2 = j^2 = k^2 = -1, i^0 = j^0 = k^0 = 1 , 每个四元数都是 1、i、j 和 k 的线性组合,即是四元数一般可表示为a + bi+ cj + dk,其中a、b、c 、d是实数。 对于i、j、k本身的几何意义可以理解为一种旋转,其中i旋转代表X轴与Y轴相交平面中X轴正向向Y轴正向的旋转,j旋转代表Z轴与X轴相交平面中Z轴正向向X轴正向的旋转,k旋转代表Y轴与Z轴相交平面中Y轴正向向Z轴正向的旋转,-i、-j、-k分别代表i、j、k旋转的反向旋转。 三维的旋转我们可以用单位四元数来表示,四元数是由一个实部加三个虚部构成类似于我们的向量,但是它的虚部从一个变成了三个。相关的四元数运算规则,具体这边不做过分介绍,大家感兴趣的可以去搜索一下,四元数的运算方式和复数运算方式类似,但有些许不同。 为什么要用四元数呢?因为旋转矩阵实际上我们用了九个量来描述三个轴、三个自由度的旋转,它这个数据量是有一些多,我们希望用一个紧凑的描述方式来描述这个旋转。而复数我们曾经学过,可以很好解决的这个问题,回忆一下。给虚数的虚部乘以一个复数i就相当于我们旋转了90°的一个向量,那如果我们三个虚部,我们就可以用四元数很好的描述这个旋转问题了。 # 总结 今天这一章我们主要介绍了一些关于三维钢体运动的概念性问题。没有做太多关于数学上的推导,因为对于大多数人来讲,理解和应用为主,数学如果大家感兴趣可以自行查找,网上也有大量的资源和资料。我将会单开一篇番外来介绍一下相关旋转矩阵平移矩阵的编程实现问题,帮助大家更好理解以及应用相关的基础数学的推论过程。在下一章节我会介绍一下李群和李代数的概念,这也是在slam中非常重要的基础知识之一。 # 续 # Eigen ## 1.介绍 > Eigen目前最新的版本是3.4,除了C++标准库以外,不需要任何其他的依赖包。Eigen使用的CMake建立配置文件和单元测试,并自动安装。如果使用Eigen库,只需包特定模块的的头文件即可。 - 矩阵的定义:Eigen中关于矩阵类的模板函数中,共有六个模板参数,常用的只有前三个。其前三个参数分别表示矩阵元素的类型、行数和列数。 - 矩阵类型:Eigen中的矩阵类型一般都是用类似MatrixXXX来表示,可以根据该名字来判断其数据类型,比如”d”表示double类型,”f”表示float类型,”i”表示整数,”c”表示复数;Matrix2f,表示的是一个2*2维的,其每个元素都是float类型。 - 数据存储:Matrix创建的矩阵默认是按列存储,Eigen在处理按列存储的矩阵时会更加高效。如果想修改可以在创建矩阵的时候加入参数。 - 矩阵和向量的算术运算:在Eigen中算术运算重载了C++的+、-、* 从http://eigen.tuxfamily.org/index.php?title=Main_Page 下载稳定版本,解压缩; ## 2.实践 打开ubuntu系统,打开终端,首先我们新建一个项目 ```cpp sudo gedit useGeometry.cpp ``` 复制如下内容 ```cpp #include
#include
using namespace std; #include
#include
using namespace Eigen; // 本程序演示了 Eigen 几何模块的使用方法 int main(int argc, char **argv) { // Eigen/Geometry 模块提供了各种旋转和平移的表示 // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f Matrix3d rotation_matrix = Matrix3d::Identity(); // 旋转向量使用 AngleAxis, 它底层不直接是Matrix,但运算可以当作矩阵(因为重载了运算符) AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1)); //沿 Z 轴旋转 45 度 cout.precision(3); cout << "rotation matrix =\n" << rotation_vector.matrix() << endl; //用matrix()转换成矩阵 // 也可以直接赋值 rotation_matrix = rotation_vector.toRotationMatrix(); // 用 AngleAxis 可以进行坐标变换 Vector3d v(1, 0, 0); Vector3d v_rotated = rotation_vector * v; cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl; // 或者用旋转矩阵 v_rotated = rotation_matrix * v; cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl; // 欧拉角: 可以将旋转矩阵直接转换成欧拉角 Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序,即yaw-pitch-roll顺序 cout << "yaw pitch roll = " << euler_angles.transpose() << endl; // 欧氏变换矩阵使用 Eigen::Isometry Isometry3d T = Isometry3d::Identity(); // 虽然称为3d,实质上是4*4的矩阵 T.rotate(rotation_vector); // 按照rotation_vector进行旋转 T.pretranslate(Vector3d(1, 3, 4)); // 把平移向量设成(1,3,4) cout << "Transform matrix = \n" << T.matrix() << endl; // 用变换矩阵进行坐标变换 Vector3d v_transformed = T * v; // 相当于R*v+t cout << "v tranformed = " << v_transformed.transpose() << endl; // 对于仿射和射影变换,使用 Eigen::Affine3d 和 Eigen::Projective3d 即可,略 // 四元数 // 可以直接把AngleAxis赋值给四元数,反之亦然 Quaterniond q = Quaterniond(rotation_vector); cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部,前三者为虚部 // 也可以把旋转矩阵赋给它 q = Quaterniond(rotation_matrix); cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl; // 使用四元数旋转一个向量,使用重载的乘法即可 v_rotated = q * v; // 注意数学上是qvq^{-1} cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl; // 用常规向量乘法表示,则应该如下计算 cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl; return 0; } ``` 大家一定要仔细分析一下。 ```cpp sudo gedit CMakeLists.txt ``` 复制如下 ```cpp cmake_minimum_required( VERSION 2.8 ) project( geometry ) # 添加Eigen头文件 include_directories( "/usr/include/eigen3" ) add_executable(useGeometry useGeometry.cpp) ``` 以下程序也可帮助理解。 ```cpp #include "stdafx.h" #include
#include
template
static void matrix_mul_matrix(T* p1, int iRow1, int iCol1, T* p2, int iRow2, int iCol2, T* p3) { if (iRow1 != iRow2) return; //列优先 //Eigen::Map< Eigen::Matrix
> map1(p1, iRow1, iCol1); //Eigen::Map< Eigen::Matrix
> map2(p2, iRow2, iCol2); //Eigen::Map< Eigen::Matrix
> map3(p3, iCol1, iCol2); //行优先 Eigen::Map< Eigen::Matrix
> map1(p1, iRow1, iCol1); Eigen::Map< Eigen::Matrix
> map2(p2, iRow2, iCol2); Eigen::Map< Eigen::Matrix
> map3(p3, iCol1, iCol2); map3 = map1 * map2; } int main(int argc, char* argv[]) { //1. 矩阵的定义 Eigen::MatrixXd m(2, 2); Eigen::Vector3d vec3d; Eigen::Vector4d vec4d(1.0, 2.0, 3.0, 4.0); //2. 动态矩阵、静态矩阵 Eigen::MatrixXd matrixXd; Eigen::Matrix3d matrix3d; //3. 矩阵元素的访问 m(0, 0) = 1; m(0, 1) = 2; m(1, 0) = m(0, 0) + 3; m(1, 1) = m(0, 0) * m(0, 1); std::cout << m << std::endl << std::endl; //4. 设置矩阵的元素 m << -1.5, 2.4, 6.7, 2.0; std::cout << m << std::endl << std::endl; int row = 4; int col = 5; Eigen::MatrixXf matrixXf(row, col); matrixXf << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20; std::cout << matrixXf << std::endl << std::endl; matrixXf << Eigen::MatrixXf::Identity(row, col); std::cout << matrixXf << std::endl << std::endl; //5. 重置矩阵大小 Eigen::MatrixXd matrixXd1(3, 3); m = matrixXd1; std::cout << m.rows() << " " << m.cols() << std::endl << std::endl; //6. 矩阵运算 m << 1, 2, 7, 3, 4, 8, 5, 6, 9; std::cout << m << std::endl; matrixXd1 = Eigen::Matrix3d::Random(); m += matrixXd1; std::cout << m << std::endl << std::endl; m *= 2; std::cout << m << std::endl << std::endl; std::cout << -m << std::endl << std::endl; std::cout << m << std::endl << std::endl; //7. 求矩阵的转置、共轭矩阵、伴随矩阵 std::cout << m.transpose() << std::endl << std::endl; std::cout << m.conjugate() << std::endl << std::endl; std::cout << m.adjoint() << std::endl << std::endl; std::cout << m << std::endl << std::endl; m.transposeInPlace(); std::cout << m << std::endl << std::endl; //8. 矩阵相乘、矩阵向量相乘 std::cout << m*m << std::endl << std::endl; vec3d = Eigen::Vector3d(1, 2, 3); std::cout << m * vec3d << std::endl << std::endl; std::cout << vec3d.transpose()*m << std::endl << std::endl; //9. 矩阵的块操作 std::cout << m << std::endl << std::endl; std::cout << m.block(1, 1, 2, 2) << std::endl << std::endl; std::cout << m.block<1, 2>(0, 0) << std::endl << std::endl; std::cout << m.col(1) << std::endl << std::endl; std::cout << m.row(0) << std::endl << std::endl; //10. 向量的块操作 Eigen::ArrayXf arrayXf(10); arrayXf << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; std::cout << vec3d << std::endl << std::endl; std::cout << arrayXf << std::endl << std::endl; std::cout << arrayXf.head(5) << std::endl << std::endl; std::cout << arrayXf.tail(4) * 2 << std::endl << std::endl; //11. 求解矩阵的特征值和特征向量 Eigen::Matrix2f matrix2f; matrix2f << 1, 2, 3, 4; Eigen::SelfAdjointEigenSolver
eigenSolver(matrix2f); if (eigenSolver.info() == Eigen::Success) { std::cout << eigenSolver.eigenvalues() << std::endl << std::endl; std::cout << eigenSolver.eigenvectors() << std::endl << std::endl; } //12. 类Map及动态矩阵的使用 int array1[4] = { 1, 2, 3, 4 }; int array2[4] = { 5, 6, 7, 8 }; int array3[4] = { 0, 0, 0, 0}; matrix_mul_matrix(array1, 2, 2, array2, 2, 2, array3); for (int i = 0; i < 4; i++) std::cout << array3[i] << std::endl; return 0; } ``` 然后按照cmake编译流程编译即可。 这里就实现了我们的实践实例,希望可以帮助大家理解概念。
原创作品,未经权利人授权禁止转载。详情见
转载须知
。
举报文章
点赞
(
0
)
hero_chao
关注
评论
(0)
登录后可评论,请
登录
或
注册
相关文章推荐
MK-米客方德推出工业级存储卡
Beetle ESP32 C3 蓝牙数据收发
Beetle ESP32 C3 wifi联网获取实时天气信息
开箱测评Beetle ESP32-C3 (RISC-V芯片)模块
正点原子数控电源DP100测评
DP100试用评测-----开箱+初体验
Beetle ESP32 C3环境搭建
【花雕体验】16 使用Beetle ESP32 C3控制8X32位WS2812硬屏之二
X
你的打赏是对原创作者最大的认可
请选择打赏IC币的数量,一经提交无法退回 !
100IC币
500IC币
1000IC币
自定义
IC币
确定
X
提交成功 ! 谢谢您的支持
返回
我要举报该内容理由
×
广告及垃圾信息
抄袭或未经授权
其它举报理由
请输入您举报的理由(50字以内)
取消
提交