视觉slam学习笔记

程序员文章站 2022-06-04 12:45:22

...

写在最前面：
视觉slam学习笔记

SLAM特指：特指搭载传感器的主体，在没有环境先验的信息情况下，在运动过程中建立环境模型，通过估计自己的运动。
SLAM的目的是解决两个问题：1、定位 2、地图构建
也就是说，要一边估计出传感器自身的位置，一边要建立周围环境的模型
最终的目标：实时地，在没有先验知识的情况下进行定位和地图重建。
当相机作为传感器的时候，我们要做的就是根据一张张连续运动的图像，从中估计出相机的运动以及周围环境中的情况
需要大体了解的书籍：
1、概率机器人
2、计算机视觉中的多视图几何
3、机器人学中的状态估计
需要用的Eigen、OpenCV、PCL、g2o、Ceres
安装Kdevelop
http://blog.163.com/[email protected]/blog/static/16793820820122224332943/

一些常见的传感器
视觉slam学习笔记

单目（Monocular）、双目（Stereo）、深度相机（RGB-D）
深度相机能够读取每个像素离相机的距离
单目相机只使用一个摄像头进行SLAM的做法叫做单目SLAM（Monocular SLAM）,结构简单，成本低。
照片拍照的本质，就是在相机平面的一个投影，在这个过程当中丢失了这个场景的一个维度，就是深度（距离信息）
视觉slam学习笔记
单目SLAM 估计的轨迹和地图，与真实的轨迹和地图之间相差一个因子，这就是所谓的尺度（scale）由于，单目SLAM无法凭借图像来确定真实的尺度，又称为尺度不确定
单目SLAM 的缺点：1、只有平移后才能计算深度 2、无法确定真实的尺度。
双目相机：
视觉slam学习笔记
双目相机和深度相机的目的是，通过某种手段测量物体离我们之间的距离。如果知道这个距离，场景的三维结构就可以通过这个单个图像恢复出来，消除了尺度不确定性。
双目相机是由于两个单目相机组成，这两个相机之间的距离叫做基线（baseline）这个基线的值是已知的，我们通过这个基线的来估计每个像素的空间位置（就像是人通过左右眼的图像的差异，来判断物体的远近）
但是计算机双目相机需要大量的计算才能估计出每个像素点的深度。双目相机的测量到的深度范围与基线相关，基线的距离越大，能够检测到的距离就越远。双目的相机的缺点是：配置和标定都比较复杂，其深度测量和精度受到双目的基线和分辨率的限制，而且视差的计算非常消耗计算机的资源。因此在现有的条件下，计算量大是双目相机的主要问题之一。
深度相机是2010年左右开始兴起的一种相机，他的最大的特点就是采用红外结构光或者（Time-of-Flight）ToF原理，像激光传感器那样，主动向物体发射光并且接受返回的光，测量出物体离相机的距离。这部分并不像双目那样通过计算机计算来解决，而是通过物理测量的手段，可以节省大量的计算量。目前RGBD相机主要包括KinectV1/V2， Xtion live pro,Realsense,目前大多数的RGBD相机还存在测量范围小，噪声大，视野小，易受到日光干扰，无法测量透射材质等诸多问题。

经典视觉SLAM框架

视觉slam学习笔记
视觉slam的路程分成以下几步：
1、传感器信息的读取。
2、视觉里程计（Visual Odometry,VO）视觉里程计的任务是，估算相邻图像之间相机的运动，以及局部地图。VO也称为前端（Front End）.
3、后端优化（Optimization）后端是接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息。（Back End）
4、回环检测（loop Closing）。回环检测是判断机器人是否或者曾经到达过先前的位置。如果检测到回环，它会吧信息提供给后端进行处理
5、建图（Mapping），他根据估计出的轨迹，建立与任务要求的对应的地图。

视觉里程计

视觉里程计关心的是，相邻图像之间相机的移动，最简单的情况就是计算两张图像之间的运动关系。
为了定量的估计相机的运动，必须了解相机与空间点的几何关系。
如果仅仅用视觉里程计来轨迹轨迹，将不可避免的出现累计漂移，注意，这个地方说的是累计漂移，
视觉slam学习笔记，
为了解决漂移问题。我们还需要两种检测技术：后端优化和回环检测的。回环检测负责吧“”机器人回到原来的位置“”这件事情给检测出来。而后端优化则是根据这个信息，优化整个轨迹形状

后端优化

后端优化主要是指在处理SLAM噪声问题，虽然我希望所有的数据都是准确的，但是在现实当中，再精确的传感器也是有一定噪声的。后端只要的考虑的问题，就是如何从带有噪声的数据中，估计出整个系统的状态，以及这个状态估计的不确定性有多大（最大后验概率（MAP））
视觉里程计（前端）给后端提供待优化的数据。
后端负责整体的优化过程，后端面对的只有数据，并不关系数据到底是来自哪个传感器。
在视觉SLAM当中，前端和计算机视觉领域更为相关，例如：图像的特征提取与匹配。后端只要是滤波和非线性优化算法。

回环检测

回环检测（闭环检测（Loop Closure Detection））主要是解决位置估计随时间漂移的问题。为了实现回环检测，我们需要让机器人具有识别曾经到达过场景的能力。通过判断图像之间的相似性，来完成回环检测。如果回环检测成功，可以显著地减小累计误差。所以视觉回环检测，实际上是一种计算图像数据相似性的算法。

建图

建图（Mapping）是指构建地图的过程。地图是对环境的描述，但是这个描述并不是固定。（如果是激光雷达的地图，就是一个二维的地图，如果是其他视觉slam 三维的点云图）
地图可以分成度量地图和拓扑地图两种。
1、度量地图（Metric Map）
度量地图强调的是精确表示出地图中物体的位置关系，通常我们用稀疏（Sparse）与稠密（Dense）对他们进行分类。稀疏地图进行了一定程度的抽象，并不需要表达所有物体。例如：我们需要一部分有代表意义的东西（简称为路标landmark），
视觉slam学习笔记
这里的稀疏地图，就是又路标组成的地图。
相对于稀疏地图而言，稠密地图将建模所看到的所有的东西

对于定位而言：稀疏的路标地图就足够了。而要用于导航，我们往往需要稠密地图。
稠密地图通常按照分辨率，由许多个小块组成。二维的度量地图是许多小格子（Grid）,三维则是许多小方块（Voxel）。一般而言，一个小方块，有占据，空闲，未知三种状态，来表达这个格子内是不是有物体。
一些用于视觉导航的算法 A*, D*，算法这种算法需要地图能够存储每个格点的状态，浪费了大量的存储空间。而且大多数情况下，地图的很多细节是无用的。另外，大规模度量地图有的时候回出现一致性问题。很小的一点转向误差，可能会导致两间屋子之间的墙出现重叠，使得地图失效。
2、拓扑地图（Topological Map）
相比于度量地图的精准性，拓扑地图则更强调了地图元素之间的关系。拓扑地图是一个图。这个图是由节点和边组成，只考虑节点间的连通性。它放松了地图对精确位置的需要，去掉了地图的细节问题，是一种更为紧凑的表达方式。
如何在拓扑图中，进行分割形成结点和边，如何使用拓扑地图进行导航和路径规划。

slam 的数学表述

通常机器人会携带一个测量自身运动的传感器，比如说码盘，或者惯性传感器。运动方程可以表示为：
视觉slam学习笔记

与运动方程相互对应的观测方程。

假设机器人在平面中运动，他的位姿由两个位置和一个转角来描述，也就是
同时如果运动传感器能够检测到机器人每两个时间间隔位置和转角的变换量那么此刻的运动方程就可以具体化：
视觉slam学习笔记
这是比较理想的状态下。现实是，并不是所有的传感器都可以直接测量出位置和角度的变化。
假如机器人携带的是一个二维的激光雷达。我们知道激光传感器观测一个2D路标是时候，能够测量到两个量，一个是路标边到机器人之间的距离r 另一个是夹角视觉slam学习笔记，那么我们写出的观测方程就是：

在考虑视觉SLAM时候，观测方程就是“对路标点拍摄后，得到了图像中的像素”的过程。
针对不同的传感器，这两个方程有不同的参数化。如果我们保持通用性，吧他们们抽象成通用的抽象形式，那么SLAM过程可总结为两个基本过程：
视觉slam学习笔记
将SLAM问题建模成一个状态估计问题。状态估计问题的求解，与这个两个方程的具体的形式有关，以及噪声服从那种分布有关。我们按照运动和观测方程是否为线性，噪声是否服从高斯分布，分成线性/非线性和高斯/非高斯系统。其中线性高斯系统（Line Gaussian,LG系统）是最简单的，他的无偏的最优估计可以由卡尔曼滤波器给出。而在非线性，非高斯的系统中，我们会使用扩展卡尔曼滤波器（EKF）和非线性优化两大类方法求解它。直到21世纪早期，以EKF为主，占据了SLAM的主导地位。到今天视觉SLAM主要都是以图优化来进行状态估计
机器人一般都是6*度，三维空间的运动由三个轴以及绕这个三个轴的旋转量组成。一共有6个*度。
CMakeLists.txt

#“#”表示注释
#声明要求cmake的最低的版本
cmake_minimum_required(VERSION 2.8)
#声明一个工程
project（HelloSlam）
#添加一个可执行文件
#语法

第三讲三维空间的刚体运动

视觉slam学习笔记

什么叫旋转向量：使用一个向量，他的方向和旋转轴一致，长度等于旋转角。（任意的一个旋转，都可以用旋转轴和旋转角来表示）这就是轴角Axis-Angle

 Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();

任意的旋转都可以使用角轴来描述，那么前面的这个参数表示的旋转的角度，后面的这个参数旋转向量

然后我们现在已知了旋转矩阵，通过视觉slam学习笔记公式求得特殊正交群。
在代码中是这样写的

Sophus::SO3 SO3_R(R);   // Sophus::SO(3)可以直接从旋转矩阵构造 由旋转矩阵得到的

当然还有以下几种写法

 Sophus::SO3 SO3_R(R);      // Sophus::SO(3)可以直接从旋转矩阵构造 由旋转矩阵得到的
    Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // 亦可从旋转向量构造
    Eigen::Quaterniond q(R);            // 或者四元数 这里是将R变成一个四元数
    Sophus::SO3 SO3_q( q );

如何输出一个四元数

    cout<<"q="<<q.coeffs()<<endl;

这里coeffs输出的顺序是（x,y,z,w）前面的x,y,z为虚部 w是实部
视觉slam学习笔记

    Eigen::Vector3d t(1,0,0);           // 沿X轴平移1
    Sophus::SE3 SE3_Rt(R, t);           // 从R,t构造SE(3)
    Sophus::SE3 SE3_qt(q,t);            // 从q,t构造SE(3)
    cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;
    cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;
    cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;
    cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;

总结：
视觉slam学习笔记

一个刚体在三维空间中的运动是如何描述的。
平移是比较好描述的，旋转这件事比较困难。如何来描述旋转，我们使用：旋转矩阵、四元数、欧拉角
刚体：有位置信息，还需要有自身的姿态（姿态：就是相机的朝向）
结合起来：相机为与空间上（0,0,0）处，面朝正前方
这是关于坐标系，我们通过坐标系来描述一个向量。
视觉slam学习笔记
外积就是将外积表达式拆开成为第2个式子，然后再a拆开，b表示列向量就变成a^b
a^表示把a这个向量变成一个矩阵，这个矩阵是a的反对称矩阵，
反对称矩阵就是矩阵a的转置等于-a
这里a与b的外积是一个行列式，
视觉slam学习笔记
外积的物理意义是a和b组成的平行四边形的的面积

现在存在一个问题，同一个向量在不同的坐标系里面坐标该怎么表示？

在机器人运动过程中，我们设定的是惯性坐标系（世界坐标系），可以认为它是固定不动的
ZW,XW,YW是世界坐标系，ZC,XC,YC是相机的坐标系，P在相机坐标系中的坐标是PC，在世界坐标系中的坐标是PW，首先我们得到在相机坐标系中的 pc 然后通过变换矩阵T 来变换到PW

正交矩阵就是逆等于转置

视觉slam学习笔记
什么叫欧式变换？就是说同一个向量，在各个坐标系下的长度和夹角都不会发生改变。

欧式变换由一个旋转和一个平移两个部分组成（前提是刚体）
可以用旋转矩阵，来描述相机的旋转，旋转矩阵是一个行列式为1的正交矩阵。我们可以吧所有的旋转矩阵的集合定义如下：
视觉slam学习笔记
SO（n）是特殊正交群，这个集合由n维空间旋转矩阵组成，其中SO(3)就是三维空间的旋转，根据正交矩阵的性质，正交矩阵的逆和转置是一样的。
因此欧式空间的坐标变换可以写成：（一次旋转+一次平移）

但是这个表达式是非线性的，因此我们需要重写
1、引入齐次坐标（也是用4个数来表达三维向量的做法）
视觉slam学习笔记
在三维向量的末尾添加1，成为4维向量，称为齐次坐标
这里要区分特殊欧式群和特殊正交群

实践部分，添加eigen的库

#include_directories("/usr/include/eigen3")

不需要加入target_link

声明部分

Eigen只需要关心前面3个参数，分别是：数据类型，行，列
声明一个2行3列的float类型的矩阵
Eigen::Matrix<float,2,3>matrix_23;
声明一个3行1列的向量 这里的d表示double
Eigen::Vector3d v_3d;
声明一个double类型的3*3的矩阵,并且初始化为零
Eigen::Matrix3d matrix_33=Eigen::Matrix3d::zero();
当不确定矩阵的大小的时候，可以申请动态矩阵
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamix> matrix_dynamc;
同样，可以表示为：
Eigen::MatrixXd matrix_x;

赋值部分

为矩阵赋值
matrix_23<<1,2,3,4,5,6;

访问矩阵内部存储的数值

for(int i=0;i<2;i++){
    for(int j=0;j<3,j++){
        cout<<matrix_23(i,j)<<"\t";
        }
    cout<<endl;
}

矩阵的四则运算

v_3d<<3,2,1;
vd_3d<<4,5,6;
矩阵的类型是一样
Eigen::Matrix<double,2,1>result=matrix_23.cast<double>()*v_3d;//输入想要改变矩阵的类型
在一个矩阵的后面 .cast<double>() 将原来的float转化的double类型
Eigen::Matrix<float,2,1>result2=matrix_23*vd_3d;//默认条件下矩阵的类型

矩阵的性质

产生随机数矩阵
matrix_33=Eigen::Matrix3d::Random();
求这个矩阵的转置
cout<<matrix_33.transpose()<<endl;
求这个矩阵各个元素的和
cout<<matrix_33.sum()<<endl;
求这个矩阵的迹（主对角线元素之和）
cout<<matrix_33.trace()<<endl;
求这个矩阵的数乘
cout<<matrix_33*10<<endl;
求这个矩阵的逆
cout<<matrix_33.inverse()<<endl;
求这个矩阵的行列式
cout<<matrix_33.determinant()<<endl;
求解特征值和特征向量
Eigen::SelgAdjointEigenSlover<Eigen::Matirx> eigen_solver(matrix_33);
cout<<eigen_solver.eigenvalues()<<endl;
cout<<eigen_solver.eigenvectors()<<endl;
求解方程：例如求解 NN*X=V_Nd 这个方程
首先定义这几个变量
 Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
Eigen::Matrix< double, MATRIX_SIZE,  1> v_Nd;
v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 );
clock_t time_stt=clock();//用来计时
第一种方法：直接求逆
Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
然后输出计算的时间
cout <<"time use in normal invers is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;
第二种方法：使用QR分解来求解
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout <<"time use in Qr compsition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

当矩阵的规模庞大的时候，使用QR分解更加高效
QR分解也叫做正交三角分解，把矩阵分解成一个正交矩阵与一个上三角矩阵的积。通常用来求解线性最小二乘法的问题
QR求解的方法：

Givens变换求矩阵的QR分解
Householder变换又称为反射变换或者镜像变换
Gram-Schmidt正交化

由于旋转矩阵是用9个量来描述旋转的，并且有正交性约束和行列式值约束
我们希望能够一种更加紧凑的方式来系统的描述旋转和平移
因此，我们换了一种方式进行比较，就是需旋转轴和旋转角因此我们使用一个向量，这个向量的方向和旋转轴一致，长度等于旋转角，这种向量，称为旋转向量（或者轴角，Axis-Angle），也可以叫做角轴, 这种方法的优势在于，只需要一个三维的向量，就可以描述旋转。
作为一个承上启下的部分，旋转向量和旋转矩阵是如何转化呢？假设有一个旋转轴为n，角度为A，那么他对应的旋转向量是An，由轴角到旋转矩阵的过程由于罗德里格斯公式表明：
视觉slam学习笔记
因此旋转矩阵到轴角的表示方法：

欧拉角

优势：直观
欧拉角使用的三个分离的转角，把一次旋转分解成三次绕不同的轴的旋转
其中较为常用的ZYX轴旋转
欧拉角会出现奇异值（万向锁问题）

物体绕Z轴旋转之后得到偏航角yaw
绕Y轴之后得到俯仰角pitch
绕X轴旋转之后得到滚动角roll
因此我们可以使用[r，p，y]来表示任意的旋转 rpy角的旋转顺序是ZYX

RPY的参考链接：
http://blog.csdn.net/heroacool/article/details/70568858
缺陷：这种方法会碰到万向锁的问题，在俯仰角pitch为+- 90度的时候，第一次旋转和第三次旋转使用的同一个轴，使得系统丢失一个*度。也成为万向锁
由于这个问题，欧拉角不适合用于插值和迭代，往往只是用于人机交互中，同样也不会在滤波和优化中使用欧拉角来表达旋转（因为具有奇异性）

四元数

旋转矩阵用9个量，来描述3*度的旋转，具有冗余性，而欧拉角和旋转向量具有奇异性
在工程当中使用的四元数来存储机器人轨迹和旋转
例如，当我们想要用二维的量来表示地球表面的时候，必然会出现奇异性，例如，经纬度在+-90度的地方，毫无意义，因此想要表示三维的刚体的时候，我们就打算用四元数。四元数的优势在于1、紧凑，2、没有奇异性
一个四元数拥有一个实部和3个虚部本质上是一种扩展复数，描述3维的量，用四元数
视觉slam学习笔记

如何用四元数来表示三维刚体的旋转
假设，某个旋转是绕单位向量

结论在四元数中，任意旋转都可以有两个互为相反数的四元数来表示。
四元数到旋转矩阵的变换方式

反之，由旋转矩阵到四元数的变换如下：假设矩阵R={mij},其对应的四元数是：
视觉slam学习笔记

相似、仿射、射影变换

这三种变换都会改变原来物体的外形
相似变换比欧式变换对了一个*度，它允许物体进行均匀的缩放，其矩阵表示为：
视觉slam学习笔记
其中旋转矩阵部分多了一个缩放因子s，可以将x,y,z在三个坐标轴上面进行缩放
仿射变换

A只是要求是一个可逆矩阵，不要求是正交矩阵，仿射变换后的立方体就不再是方的买单时各个面仍然是平行四边形。
射影变换
视觉slam学习笔记
这是一个最一般的形式，一个正方形经过射影变换，最后变成一个规则的四边形
总结：

从真实世界到相机照片变换的过程就是一个射影变换。如果相机的焦距为无穷远的时候，那么这个变换是仿射变换。
实践部分：Eigen的几何模块

首先添加头文件#include<Eigen/Geometry>
Eigen/Geometry提供了各种旋转和平移的表示
声明一个3维的旋转矩阵
Eigen::Matrix3d rotation_matrix=Eigen::Matrix3d::Identity();
声明旋转向量,并且沿Z轴旋转45度
Eigen::AngleAxisd rotation_vecotr (M PI/4,Eigen::Vector3d(0,0,1));
cout .precision(3);//输出返回3位有效数字
cout<<rotation_vector.matrix()<<endl;
将旋转向量赋值给另外一个向量
rotation_matrix=rotation_vector.toRotationMatrix();
通过跟一个向量相乘从而进行坐标变换
Eigen::Vector3d v(1,0,0);
Eigen::Vector3d v_rotated=rotation_vector *v;
cout<<v_rotated.transport()<<endl;//将（0,0,1）旋转之后的向量转置输出
将一个向量（1,0,0）使用旋转矩阵之后的输出
v_rotated=rotation_matrix*v;//其实说白了，就是让这个旋转矩阵和这个向量相乘，然后重视[3*3]*[3*1]最后输出的结果就是3*1
cout<<v_rotated.transport()<<endl;//将（0,0,1）旋转之后的向量转置输出
可以将旋转矩阵直接转化成欧拉角，其实欧拉角存在的意义，只是为了方便人们理解，在程序当中，我们处理问题一般是用的四元数
Eigen::Vector3d  euler_angles=rotation_matrix.eulerAngles(2,1,0);//这是输出的默认的数据是roll pitch yaw
如果逆向输出的顺序是yaw pitch roll，那么
cout<<"yaw pitch roll"<<euler_angles.transport()<<endl;
欧式变换阵 使用Eigen::Isometry
说的是三维空间下的变换Isometry3d
Eigen::Isometry T=Eigen::Isometry3d::Identity();
T.rotate(rotation_vector);//按照rotation_vector的方式进行旋转
T.pretranslate(Eigen::Vector3d(1,3,4));//吧这个向量进行平移
cout<<T.matrix()<<endl;
几种坐标变换的方式：1、用变换矩阵进行坐标变换
Eigen::Vector3d v_transformed=T*v;
关于四元数
Eigen::Quaterniond q=Eigen::Quaterniond(rotation_vector);
cout<<q.coeffs()<<endl;//这里coeffs输出的顺序是（x,y,z,w）前面的x,y,z为虚部 w是实部
同样也可以把旋转矩阵赋给他
q=Eigen::Quateriond(rotation_matrix)

visualize_Geometry

#include<iomanip>

这个头文件对cin和cout这种操作符进行控制，例如可以控制输出位数，左对齐，右对齐等。

#include <pangolin/pangolin.h>

pangolin是SLAM可视化的openGL的库

第四讲李群李代数

由于相机的位姿是未知的，我们需要解决什么样的相机位姿最符合当前关键数据这样的问题
通过李群李代数之间的转换关系，我们希望把位姿估计变成无约束的优化问题。
特殊正交群SO(3)，是由三维旋转矩阵构成的；特殊欧式群SE（3）是变换矩阵构成的
特殊正交群SO(3)就是，3*3的矩阵，然后正交，行列式为1，其定义是：
视觉slam学习笔记
特殊欧式群SE(3)，是在这个3*3矩阵基础上，转化成一个齐次的矩阵，为了避免非齐次带来的不便，其定义如下

加法的封闭性a属于集合A，b也属于集合A，如果a+b也是属于集合A的话，这就叫做加法的封闭性
但是旋转矩阵和变换矩阵是没有加法封闭性的，但是他们的乘法是封闭的，也就是说
视觉slam学习笔记
两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个运算的集合，我们把他叫做群。
群
群是一种集合加上一种集合运算的代数结构，我们把集合记作A，运算符号记作 · ，那么一个群可以记作 G=(A, ·),群这种运算要求满足以下几个条件：
视觉slam学习笔记
s.t.表示约束条件
总结：旋转矩阵和矩阵的乘法运算构成旋转矩阵群；变换矩阵和矩阵乘法构成变换矩阵群
李群 Lie Group
李群，是指的具有连续（光滑）性质的群，SO(n),SE(n）是李群。因为每个李群都有对应的李代数因此，我们先引出李代数so(n)
李代数的引出
我们每次想要求一个旋转矩阵的导数，就可以左乘一个矩阵视觉slam学习笔记即可，
我们根据导数的定义，在R(t)的0附近进行一阶泰勒展开式

他是推到了，如果一个导数等于他本身，那么这个导数就是e^x这个函数，即：

每个李群都对应一种李代数
由于每个李群都有李代数，李代数描述了李群的局部性质。
李代数由一个集合V，一个数域F,和一个二以下元运算[,]组成，如果他们满足以下几条性质，称（V,F,[,]）为一个李代数，记作 g
视觉slam学习笔记
[ ]这个运算符叫做李括号，要求元素和自己做李括号这种运算之后为0，也即是[x,x]=0，李括号直观上来讲是表达了两个元素之间的差异
李代数so(3)
在这个式子总的就是SO(3)对应的李代数，记作so(3)
so(3)是一个三维的向量组成的集合，每个向量都对应着一个反对称的矩阵，来表达旋转矩阵的导数
视觉slam学习笔记
SO（3）和so(3)之间的映射关系是：

同理：SE(3)也是有对应的李代数的se(3)

他是一个6维的向量，前三维表示平移，记作，后面三维表示旋转，记作：，

SO（3）上的指数映射的物理意义就是旋转向量
为什么我们要有李代数，因为我们需要解决李群上面只有乘法没有加法这个问题。
利用BCH线性近似，来推导so(3)和se(3)上的导数和扰动波形，通常情况下，扰动模型比较简洁，更加常用。

实践部分：使用sophus库
编译部分：到高博的3rdparty，然后解压sophus，然后mkdir build cd build cmake .. make 就可以了。
cmake .. 编译的时候出现这幅图是正常的。当还以为有问题呢？看来是多虑的
视觉slam学习笔记

视觉SLAM第5讲

视觉slam学习笔记
opencv各个版本的github上的链接：
https://github.com/opencv/opencv/releases
安装opencv3.1.0遇到的问题：

找到的解决方案：
http://blog.csdn.net/huangkangying/article/details/53406370
然后

// 关于 cv::Mat 的拷贝
    // 直接赋值并不会拷贝数据
    cv::Mat image_another = image;
    // 修改 image_another 会导致 image 发生变化
    image_another ( cv::Rect ( 0,0,100,100 ) ).setTo ( 0 ); // 将左上角100*100的块置零
    cv::imshow ( "image", image );
    cv::waitKey ( 0 );

    // 使用clone函数来拷贝数据
    cv::Mat image_clone = image.clone();
    image_clone ( cv::Rect ( 0,0,100,100 ) ).setTo ( 255 );
    cv::imshow ( "image", image );
    cv::imshow ( "image_clone", image_clone );
    cv::waitKey ( 0 );

第一种方式，拷贝数据的话是浅拷贝，就是说，你如果该数据，那么原始的数据也会改变。
第二种方式，在拷贝数据的时候用image.clone()那么这样不会修改原始的数据。
Ubuntu16.04须知：
安装pcl

sudo apt-get install libpcl-dev pcl-tools

并且在使用的时候，cmakelist需要这么写：
视觉slam学习笔记
joinMap.cpp
用来读写文件的

    #include<fstream>

将图像都放到一个容器当中，<这里面试数据类型>

    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d> poses;         // 相机位姿

利用fin来读取txt当中的参数

    ifstream fin("./pose.txt");//导入相机的位置和姿态的数据

读取数据流
视觉slam学习笔记

为了使用boost::format

#include <boost/format.hpp>

boost::format格式化字符串

视觉slam学习笔记
这个两个的效果是一模一样的。

boost::format fmt( "./%s/%d.%s" ); //图像文件格式

容器的基本使用：http://blog.csdn.net/ws_20100/article/details/50829327
这里.push_back是在这个容器的尾部插入、push_back(图片)

 colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
 depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像

这里用来遍历data的

 double data[7] = {0};//初始化
 for ( auto& d:data )
 fin>>d;

参考链接：C++11 for http://blog.csdn.net/hackmind/article/details/24271949
将图像的外参

 for ( int i=0; i<5; i++ )
    {
        boost::format fmt( "./%s/%d.%s" ); //图像文件格式
        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像

        //将相机的外参数，都给了T
        double data[7] = {0};
        for ( auto& d:data )
            fin>>d;//将上面的每个图片的外参都给了d
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        Eigen::Isometry3d T(q);//定义一个欧式变换矩阵，
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
        poses.push_back( T );//将这里的数据，添加的相机的位子当中，进行重新的排序
    }

在点云拼接部分：

opencv当中颜色通道是bgr的顺序

        cv::Mat color = colorImgs[i]; 
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];
     for ( int v=0; v<color.rows; v++ )
            for ( int u=0; u<color.cols; u++ )
            {
                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
                if ( d==0 ) continue; // 为0表示没有测量到
                Eigen::Vector3d point; //声明一个3维的列向量
                point[2] = double(d)/depthScale; 
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy; 
                Eigen::Vector3d pointWorld = T*point;
                //XYZRGB，在opencv当中，                
                PointT p ;
                p.x = pointWorld[0];
                p.y = -pointWorld[1];
                p.z = pointWorld[2];
                p.b = color.data[ v*color.step+u*color.channels() ];
                p.g = color.data[ v*color.step+u*color.channels()+1 ];
                p.r = color.data[ v*color.step+u*color.channels()+2 ];
                pointCloud->points.push_back( p );
            }
    }

视觉slam学习笔记

opencv当中的 .step参数表示的实际像素的宽度 .channel()表示的是，通道的提取

关于图像上下翻转问题，是因为opencv定义的坐标系和pcl_viewer显示坐标系不同，opencv是x右y下，而pcl显示是x右y上。解决方法：找到群主程序函数中，把计算点空间坐标的公式的p.y值添加负号，这样y方向就可以正常显示了,so easy。(或许还有别的方法)
视觉slam学习笔记
最后保存文件

cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );

补充：归一化平面
视觉slam学习笔记

视觉SLAM和激光SLAM的区别在于，观测模型不同。（他们的运动模型是相同的）
观测模型当中最常用的就是针孔相机模型
相机能够将三维世界中的坐标点（单位米）映射到二维图像平面（单位像素）的过程用一个几何模型进行讨论。

针孔相机模型

针孔相机模型，就是利用了相似三角形的知识
视觉slam学习笔记
径向畸变由透镜形状引起的畸变，就是径向畸变
主要分成两大类：桶性畸变和枕形畸变
桶形畸变是由于图像放大率随着离光轴的距离的增加而减小

视觉slam学习笔记

枕形畸变是图像的放大率随着光轴距离的增加而增加

视觉slam学习笔记

切向畸变：相机在组装的过程中由于透镜和成像平面不是严格平行的导致的切向畸变

视觉slam学习笔记

像素坐标系的定义：
像素坐标系与成像平面之间的差别：
相差了一个缩放和原点的平移
视觉slam学习笔记

通常我们把中间的量组成的矩阵称为相机的内参数矩阵K。通常认为相机在内参在出厂之后是固定不变的，有的却没有，这个时候就是需要标定一下，但是我觉得标定这件事

畸变

RGB-D相机模型

目前的RGB-D相机按照原理可以分成两类
1、通过红外光结构（structured light）来测量像素的距离，例如：Kinect1 Project Tango1、Intel Realsense
视觉slam学习笔记
2、通过飞行时间法（Time-of-light,ToF）原理来测量像素的的距离。例如有KInect2和一些Tof传感器

标定K就是相机的内参

视觉SLAM第6讲状态估计的问题

总结

视觉slam学习笔记

http://blog.csdn.net/david_han008/article/details/53560736

上一篇：请问一个有关问题，怎么把txt导入数据库

下一篇：数据结构与算法 | 希尔排序

视觉slam学习笔记

经典视觉SLAM框架

视觉里程计

后端优化

回环检测

建图

slam 的数学表述

第三讲三维空间的刚体运动

欧拉角

四元数

相似、仿射、射影变换

第四讲李群李代数

视觉SLAM第5讲

针孔相机模型

畸变

RGB-D相机模型

视觉SLAM第6讲状态估计的问题

Yii框架学习笔记之应用组件操作示例

Python学习笔记整理3之输入输出、python eval函数

JDBC学习笔记二

Python中Random和Math模块学习笔记

html5中canvas学习笔记1-画板的尺寸与实际显示尺寸

PHP 面向对象程序设计（oop）学习笔记 (五) - PHP 命名空间

Python学习笔记之读取文件、OS模块、异常处理、with as语法示例

js零基础快速学习教程（知识与语法整理笔记）

JSP学习笔记

iOS学习笔记-139.RunLoop07——Runloop处理流程

视觉slam学习笔记

经典视觉SLAM框架

视觉里程计

后端优化

回环检测

建图

slam 的数学表述

第三讲 三维空间的刚体运动

欧拉角

四元数

相似、仿射、射影变换

第四讲 李群李代数

视觉SLAM第5讲

针孔相机模型

畸变

RGB-D相机模型

视觉SLAM第6讲状态估计的问题

Yii框架学习笔记之应用组件操作示例

Python学习笔记整理3之输入输出、python eval函数

JDBC学习笔记二

Python中Random和Math模块学习笔记

html5中canvas学习笔记1-画板的尺寸与实际显示尺寸

PHP 面向对象程序设计（oop）学习笔记 (五) - PHP 命名空间

Python学习笔记之读取文件、OS模块、异常处理、with as语法示例

js零基础快速学习教程（知识与语法整理笔记）

JSP学习笔记

iOS学习笔记-139.RunLoop07——Runloop处理流程

第三讲三维空间的刚体运动

第四讲李群李代数