AI.PLAY

第二种坐标变换: 再分别计算这2个节点的时间在里程计数据队列中插值得到的2个里程计位姿, 计算这2个里程计位姿间的坐标变换。节点与节点间在global坐标系下的相对坐标变换 与 相邻2个节点在local坐标系下的相对坐标变换的差值作为残差项。节点与gps坐标系原点在global坐标系下的相对坐标变换与通过gps数据进行插值得到的相对坐标变换的差值作为残差项。节点与节点间在global坐标系下的相对坐标变换与通过里程计数据插值出的相对坐标变换 的差值作为残差项。

假设原始栅格地图尺寸为[h, w]，经过width的滑动窗口处理后，将采样后地图尺寸变为[h+width-1, w+width-1]。的操作,核B与图形卷积,即计算核B覆盖的区域的像素点的最大值,并把这个最大值赋值给参考点指定的像素。这个图直接让我想起了图像处理中的膨胀操作。滑窗操作会导致地图变大。

做法是根据位姿队列中最后一个位姿乘以上次添加位姿时的姿态预测到time时刻的姿态变化量，得到预测的最新里程计数据时刻 tracking frame 在 local 坐标系下的姿态，首先是根据加入的pose，单独计算出线速度和角速度linear_velocity_from_poses_，angular_velocity_from_poses_。所谓裁剪，就是丢弃过时的IMU数据，并保持保持imu队列中第二个数据的时间要大于最后一个位姿的时间, imu_date_最少是1个。

在本段中，使用了公式进行了栅格概率的更新，无非就是使用上一时刻的空闲概率[0, 1~32768]，结合当前的hit概率，得到当前的空闲概率[0, 1~32768]，hit的概率是固定值(0.55),那么就可以建立一张表，当上一时刻的空间概率为[0, 1~32768]任意值时，能快速查到到当前空闲概率。这两张表也称概率更新表，概率更新表存储着更新后的值，而对应该行的索引就是更新前的值。当上一状态为已知时，定义上一时刻的栅格空闲概率为M_old[0, 0.1~0.9]，栅格的概率更新公式如下。

如果不调用correlative_scan_matching，target_translation和initial_pose_estimate是一样的correlative_scan_matching 目的是对先验位姿进行校准，先验位姿来自于位姿推测器。问题：如果不使用correlative_scan_matching，那么ceres里面的残差不就为0了？那还怎么优化，其实相当于只使用了地图的残差？？？

后端主要实现的是位姿图优化。根据global坐标计算初值，然后通过分支丁界算法粗匹配与ceres的精匹配，获取校准后的位姿，最后计算local坐标系下，子图原点指向校准后的节点间的坐标变换。利用pose，以及传入的里程计，imu数据计算出线速度和角速度，不同的情况选择参与运算的传感器不同。图中，三角表示子图，圆圈表示节点，由于传感器的一次扫描数据可能插入到多个不同的子图，所以同一个节点可能和多个子图之间存在一定的匹配关系。关键帧指的是子图关键帧，而子图是由连续的若干个激光关键帧拼接到一起形成的子地图。

输入:世界坐标系下的n个3D点坐标以及与之对应的 2D 像点坐标、相机内参K。输出:恢复影像的位姿R和t。

特征点三角化(Triangulation)是是双目视觉中一个基础算法，也是VSLAM中一个非常基础的问题,它是根据特征点在多个相机下的投影恢复出特征点的3D坐标。使用SVD求解方程组。

一元边只需要设置一个顶点，二元边需要设置两个顶点。这一步设置的顶点，需要跟上一步添加的顶点一一对应起来，边添加完成后，顶点和边会构成一个图。其中linearizeOplus()函数为计算雅各比矩阵的函数，对于非线性优化问题，雅各比矩阵的自动求导可以提高优化的速度。在添加顶点的时候，会设置一个初始估计值，后续的误差计算会拿出顶点的当前估计值参与运算。添加顶点的时候，每个顶点为设置一个ID，在添加边的时候可以使用ID来查找顶点。构建G2O优化是一种图优化，所以需要准备顶点和边。边有一元边，二元边和多元边，

对于一个方阵而言，采用的是特征分解，参考《

关于求解特征值和特征向量参考《

这个误差指的是两个向量的不平行程度，和夹角的正弦成正比，叉乘=a*b*sin(角度),二两个向量又归一化了，所以就和sin(角度成正比)，进而反应了陀螺仪估计的姿态和加速度计测量的姿态误差。而单位向量的向量叉积在物理意义上在小角度意义就是轴角，轴角本身就刻画了旋转，也就是叉乘的结果可以看成是旋转误差，在这个物理意义下比任何误差表示都要合理和切合实际系统。在使用一个算法之前首先要弄清楚数据数据的单位是什么，在互补滤波算法中，角速度的单位是“弧度每秒”，加速度是数据是归一化后的数据，无单位。

所产生的脉冲数为 total_num = cylinder_num * linesNum * 2。那么小车原地旋转一周的走过的路径为pi * whell_dist，小车的姿态变化为360度。所以对应小车姿态变化一度为total_num / 360。linesNum为一圈的脉冲数=变比*脉冲数。假设 轮子的周长为circumference。Circumference 为轮周长。将移动距离转换到车体坐标系。从车体坐标系转到世界坐标系。轮距为whell_dist。

四元数和欧拉角四元数四元数在姿态解算中用来表示旋转。四元数的乘积可表示三维空间上任意的伸缩旋转变换。Quaternionsdonotsufferfromgimballock.Withathree-angle(roll,pitch,yaw)system,therearealwayscertainorientationsinwhichthere......

卡尔曼滤波卡尔曼滤波的核心卡尔曼滤波的核心是预测+测量反馈。以一个匀速直线运动的过程为例：目标系统(线性系统)的状态转移方程：