qq_44175983的博客

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。预测是根据上一时刻的状态量，由状态方程预测出下一时刻的状态量。加入观测，对预测值进行更新校正，得到最优后验估计。，以及状态量误差协方差的先验估计矩阵。以雷达追踪目标为背景，系统的状态方程为。是过程噪声，服从高斯分布，时刻的预测值，结合第。时刻的观测值，得到第。是上一时刻的最优估计。是过程噪声的协方差，是观测噪声的协方差。这是没有加观测值的。

一、基础矩阵、单应矩阵用像素坐标求解；本质矩阵、三角测量用归一化坐标求解。p2Hp1p2​Hp1​二、在 ORB-SLAM 中这几种方法的顺序为：提取两张图像的匹配点，利用对极几何计算HH矩阵和FF矩阵，从而恢复RRtt；用三角测量计算各关键点对应的 3D 坐标；

几个需要注意的点：（1）迭代形式为HΔxbHΔxb，也就是求出每次迭代步长Δx\Delta xΔx，三个算法可选：高斯牛顿、LM 和 Dog-Leg；（2）定义顶点：顶点也就是待优化变量，它继承自基础类，其中 D 为 int 类型，表示维度，T 为数据类型。例如g2o::BaseVertex<3, Eigen::Vector3d> // 三维点，Eigen::Vector3d 类型。

min⁡x12∑iρi∥fixi1xik∥2stlj⩽xj⩽ujxmin​21​i∑​ρi​∥fi​xi1​xik​∥2s.t.lj​⩽xj​⩽uj​其中ρi∥fixi1xik∥2ρi​∥fi​xi1​xik​∥2为残差块；f⋅f(\cdot)f⋅为代价函数，也即误差项；ρ⋅ρ⋅。

（1）最速下降法Δx∗−JxkΔx∗−Jxk​（2）牛顿法HΔx−JHΔx−J（3）高斯牛顿法JxJT⏟HxxΔx−Jxfx⏟gxHxJxJT​​xΔxgx−Jxfx​​需要注意的是，最速下降法和牛顿法属于同一类型，他们的JJ和HH都是针对FxFx（也就是整个目标函数）；而高斯牛顿法的JJ是针对fxfx（也就是误差项）。此外，为减小数值，目标函数FxF。

选择你想要的图里的节点与边的类型，确定它们的参数化形式；往图里加入实际的节点和边；选择初值，开始迭代；每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；求解稀疏线性方程HkΔx=−bk，得到梯度方向；继续用GN或LM进行迭代。如果迭代结束，返回优化值。实际上，g2o能帮我们做好第3-6步，只需要做前两步即可。

RANSAC （Random Sample Consensus，随机抽样一致）算法的基本假设是样本中包含正确数据(inliers即内点，可以被模型描述的数据)，也包含异常数据(outliers 即外点，偏离正常范围很远、无法适应数学模型的数据)，也就是说数据集中含有噪声。我们的目的就是找出使内点最多的模型参数（类似最小二乘法，最小二乘法试图找到满足所有点的参数，而 RANSAC 是为了消除误匹配，尽量找到更多内点，去除外点）。

太小，则说明实际减小的值远小于近似减小的值，即近似效果较差，需缩小近似范围，即增大阻尼系数。太大，则说明实际减小的值大于近似减小的值，则需放大近似范围，即减小。，需要计算的 step 就是这些待定系数的 step。其中，分子是实际下降的值，分母是近似下降的值。法（见《十四讲》），这里用阻尼法进行推导。那么问题转化为，对于每次迭代，求解最优的。接近于 1 ，则近似效果好；优化目标是待定系数，把他们看做。LM 算法有两种推导方式，即。的分母的计算（我的更快？可以看做是对于过大的。求导，并令其等于零，

相比于特征点法，直接法完全依靠优化来求解相机位姿。（1）直接法优点可以省去计算特征点、描述子的时间；只需要像素梯度即可，不需要特征点，因此可以在缺失特征点的场合下使用；可构件半稠密乃至稠密地图，这是特征点法做不到的。（2）直接法缺点非凸性：优化时容易进入极小，只有在运动很小的时候直接法才能成功，引入金字塔在一定程度上可减小非凸性影响。单个像素没有区分度：选点少时效果较差，一般用 500 个点以上；灰度不变假设是很强的假设，相机会自动调整曝光参数、或者光照变化，都会使得图像整体亮度发生变化。

注意残差块的维度：待优化变量分成两块，相机内外参和空间点坐标；残差为像素理论值与实际值之差，为2维。世界坐标系 --> 相机坐标系（外参）–> 归一化坐标 --> 去畸变 --> 像素坐标系（内参）两个顶点，分别表示相机内外参（9维）、空间点（3维），边为二元边，连接两个顶点。由此得到的估计值与实际观测到的像素坐标作差，得到二维误差项。原理见笔记 《后端 1》

几个需要注意的点：（1）迭代形式为HΔxbHΔxb，也就是求出每次迭代步长Δx\Delta xΔx，三个算法可选：高斯牛顿、LM 和 Dog-Leg；（2）定义顶点：顶点也就是待优化变量，它继承自基础类 BaseVertex<D, T>，其中 D 为 int 类型，表示维度，T 为数据类型。例如g2o::BaseVertex<3, Eigen::Vector3d> // 三维点，Eigen::Vector3d 类型。

min⁡x12∑iρi∥fixi1xik∥2stlj⩽xj⩽ujxmin​21​i∑​ρi​∥fi​xi1​xik​∥2s.t.lj​⩽xj​⩽uj​其中ρi∥fixi1xik∥2ρi​∥fi​xi1​xik​∥2为残差块；f⋅f(\cdot)f⋅为代价函数，也即误差项；ρ⋅ρ⋅。

（1）最速下降法Δx∗−JxkΔx∗−Jxk​（2）牛顿法HΔx−JHΔx−J（3）高斯牛顿法JxJT⏟HxxΔx−Jxfx⏟gxHxJxJT​​xΔxgx−Jxfx​​需要注意的是，最速下降法和牛顿法属于同一类型，他们的JJ和HH都是针对FxFx（也就是整个目标函数）；而高斯牛顿法的JJ是针对fxfx（也就是误差项）。此外，为减小数值，目标函数FxF。

选择你想要的图里的节点与边的类型，确定它们的参数化形式；往图里加入实际的节点和边；选择初值，开始迭代；每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；求解稀疏线性方程HkΔx=−bk，得到梯度方向；继续用GN或LM进行迭代。如果迭代结束，返回优化值。实际上，g2o能帮我们做好第3-6步，只需要做前两步即可。

RANSAC （Random Sample Consensus，随机抽样一致）算法的基本假设是样本中包含正确数据(inliers即内点，可以被模型描述的数据)，也包含异常数据(outliers 即外点，偏离正常范围很远、无法适应数学模型的数据)，也就是说数据集中含有噪声。我们的目的就是找出使内点最多的模型参数（类似最小二乘法，最小二乘法试图找到满足所有点的参数，而 RANSAC 是为了消除误匹配，尽量找到更多内点，去除外点）。

太小，则说明实际减小的值远小于近似减小的值，即近似效果较差，需缩小近似范围，即增大阻尼系数。太大，则说明实际减小的值大于近似减小的值，则需放大近似范围，即减小。，需要计算的 step 就是这些待定系数的 step。其中，分子是实际下降的值，分母是近似下降的值。法（见《十四讲》），这里用阻尼法进行推导。那么问题转化为，对于每次迭代，求解最优的。接近于 1 ，则近似效果好；优化目标是待定系数，把他们看做。LM 算法有两种推导方式，即。的分母的计算（我的更快？可以看做是对于过大的。求导，并令其等于零，

定位：导航：机器人在地图中进行路径规划；避障重建交互：人与地图之间的互动（2）几类地图稀疏地图稠密地图语义地图。

前端提供特征点的提取和轨迹、地图的初值，后端负责数据的优化。但是如果像视觉里程计那样只考虑相邻时间上的关键帧，则会出现累积误差，无法构建全局一致的轨迹和地图。回环检测的关键是有效地检测出相机经过同一个地方这件事。

（2）放开思路，我们完全可以构建一个只有轨迹的图优化，而位姿节点的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。这样的方式，省去了大量特征点优化的计算，只保留了关键帧的轨迹，构建了所谓的。（1）随着时间的流逝，机器人的运动轨迹会越来越长，地图规模也会越来越大，BA 的计算效率就会下降。同时我们发现，经过若干次迭代后，收敛的特征点位置变化很小，发散的外点则已被剔除，因此在后续优化中没有必要再将收敛点考虑进来，而是只把他们当做位姿估计的约束。了解雅克比求导后，剩下的部分就是普通的图优化。

矩阵划分为四个区域，不难看出，左上角为对角块矩阵，且每个对角块元素的维度与相机位姿维度相同，同样的，右下角也是对角块矩阵，且每个对角块元素的维度与路标点维度相同。在前面的 BA 问题中，我们将最小化误差项的二范数平方和作为目标函数，这样虽然直观，但是如果出现误匹配，该误差项会很大，从而将对整体函数产生较大影响，进而影响最终优化结果。对此，我们将原先误差的二范数度量替换成一个增长没那么快的函数，同时保证光滑性质，使得优化结果更加稳健（减小误匹配项的影响），这样的函数称为。对其他相机变量和路标点的偏导都为零。

（1）两种处理方式批量式：使用过去和未来的信息来更新自己的状态的处理方式；渐进式：仅适用过去的甚至仅是前一个时刻的信息来更新自己的状态的处理方式。

6.4.1 直接线性变换（DLT）已知空间中某点 P=[X,Y,Z,1]T\boldsymbol{P}=[X, Y, Z, 1]^TP=[X,Y,Z,1]T，在图像中的归一化坐标 为 [u,v,1]T[u, v, 1]^T[u,v,1]T，求解相机运动 R\boldsymbol{R}R、t\boldsymbol{t}t。定义增广矩阵 [R∣t][\boldsymbol{R}|\boldsymbol{t}][R∣t] 为一个 3×43\times43×4 矩阵，包含了旋转和平移信息（区别于变换矩阵）。满足

文章目录06 视觉里程计 16.1 特征点法6.1.1 特征提取6.1.2 特征匹配6.1.3 计算相机运动6.2 2D-2D：对极几何6.2.1 对极约束6.2.2 本质矩阵6.2.3 单应矩阵6.2.4 小结6.3 三角测量6.4 3D-2D: PnP06 视觉里程计 1一个 SLAM 系统分为前端和后端，前端也称为视觉里程计，它根据相邻图像的信息估计出粗略的相机运动，给后端提供较好的初始值。包括特征点法和直接法两大类。6.1 特征点法（1）视觉 SLAM 中，将图像特征点作为路标，从而估计出相

文章目录05 非线性优化5.1 批量状态估计与最大后验估计5.2 最小二乘的引出5.3 非线性最小二乘5.3.1 一阶和二阶梯度法5.3.2 高斯牛顿法5.3.3 列文伯格-马夸尔特方法5.4 实践：曲线拟合问题05 非线性优化5.1 批量状态估计与最大后验估计（1）两种状态估计方法：增量/渐进式（滤波器）：数据是随时间逐渐到来的；批量式：一次给定所有的数据，估计所有的变量。（2）经典的 SLAM 模型由一个运动方程和一个观测方程组成{xk=f(xk−1,uk)+wkzk,j=h(

但实际相机得到的图像并不是倒像，我们等价地将成像平面对称地放到相机前方，这样就可以把负号去掉，对于径向畸变，离中心距离越远，畸变越严重；针孔模型描述了一束光线通过针孔后，在针孔背面投影成像的关系（类似小孔成像原理）。将世界坐标转换到相机坐标后，再除掉最后一维的数值，这相当于把最后一维作。RGB-D 相机容易受到日光或其他传感器的干扰，因此不能在室外使用。（2）在相机组装过程中，透镜和成像平面无法完全平行，会产生。将三维空间上的点投影到归一化平面，得到坐标。其中，负号表示成的像是倒立的。

（1）每个李群都有与之对应的李代数。李代数描述了李群单位元附近的正切空间性质。（2）李代数由一个集合V\mathbb{V}V，一个数域F\mathbb{F}F，和一个二元运算 [ , ] （称为李括号）组成。满足以下性质：封闭性双线性自反性雅克比等价例如，三维向量R3R3上定义的叉乘×\times×是一种李括号，因此gR3R3×gR3R3×构成了李代数。

（1）2D 情况下，可用单位复数表达旋转，在复数平面中，结合欧拉公式，如下图，向量zz旋转 90° ，相当于乘上iii。（2）类似的，在三维空间中，采用四元数描述旋转。四元数有一个实部，三个虚部（分别对应 x、y、z轴）。qq0q1iq2jq3k(2-14)\tag{2-14}qq0​q1​iq2​jq3​k2-14写成向量形式qsvsq0∈Rvq1q2q3T∈R3(2-15)

（1）SLAM 是 Simultaneous Localization and Mapping 的缩写，即同时定位与地图构建。它是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型。（2）两大基本问题：定位：我在什么地方？建图：周围环境是什么样的？（3）准确的定位需要精确的地图，精确的地图来自准确的定位。（4）传感器内质的（放置在机器人身上，用于感受机器人本体信息）：IMU（惯性测量单元）、激光、相机外质的（安装于环境中，监测机器人的运动状态）：二维码、GPS导轨。

文章目录CH3-8 证明旋转后的四元数虚部为零，实部为罗德里格斯公式结果CH4 李群与李代数CH4-1 SO(3) 上的指数映射CH4-2 SE(3) 上的指数映射CH4-3 李代数求导对极几何：本质矩阵奇异值分解矩阵内积和迹CH3-8 证明旋转后的四元数虚部为零，实部为罗德里格斯公式结果前面已经推导过v′=pvp∗=pvp−1v'=pvp^*=pvp^{-1}v′=pvp∗=pvp−1其中，v=[0,v⃗]v=[0,\vec{v}]v=[0,v]，p=[cos⁡θ2,sin⁡θ2u⃗]p=[\

CH3-5 四元数表示旋转三维空间中任意点均可用一个纯虚四元数表示即p=[0,v]T\boldsymbol{p}=[0,\boldsymbol{v}]^Tp=[0,v]T，经一个单位四元数q\boldsymbol{q}q的旋转后，得到p′\boldsymbol{p'}p′，则p′=qpq−1(3-5-1)\boldsymbol{p'}=\boldsymbol{q}\boldsymbol{p}\boldsymbol{q^{-1}}\tag{3-5-1}p′=qpq−1(3-5-1)最终p′\bol

文章目录CH3 三维空间刚体运动CH3-1 旋转矩阵的推导CH3-2 旋转矩阵是正交矩阵的证明CH3-3 变换矩阵的逆的推导CH3-4 罗德里格斯公式推导CH3 三维空间刚体运动CH3-1 旋转矩阵的推导（1）二维空间中的旋转矩阵 易得{x′=∣OP′∣cos(θ+β)=∣OP∣(cosθ⋅cosβ−sinθ⋅sinβ)=xcosβ−ysinβy′=∣OP′∣sin(θ+β)=∣OP∣(sinθ⋅cosβ+cosθ⋅sinβ)=ycosβ+xsinβ\left\{\begin{ma