视觉SLAM14精讲——非线性优化4.1

视觉SLAM14精讲

1. 三维空间刚体运动1.0
2. 三维空间刚体运动1.1
3. 三维空间刚体运动1.2
4. 李群与李代数2.1
5. 相机与图像3.1
6. 相机与图像3.2
7. 相机与图像3.3

状态估计问题引出的SLAM与SfM比较问题

在高博讨论滤波针对0～k时刻的轨迹和地图时，涉及到如果将所有时刻的数据进行统一处理，则引出SfM这一技术。除了这一点不同，SLAM和SfM还有更大的区别

SLAM与SfM的区别与联系

SLAM与SfM的区别：
- 实时性：SLAM要求实时性与预测，SfM不做此要求。
- 方向：SLAM为ROBOTICS方向，SfM为CV方向。
- 目标：SLAM为的是定位，SfM为的是重建。
- 时序行问题：SLAM有时序性的问题。SfM可使用时间上完全无关系的图像。

SLAM与SfM的联系：
- 基本都讨论《计算机多视角几何》问题。
- 技术类似：
- 特征提取与匹配
- 误差优化
- 回环/全局对齐

马式距离扩展理解

定义

马氏距离（Mahalanobis Distance）是由印度统计学家马哈拉诺比斯提出的，表示数据的协方差距离。它考虑了数据特征之间的联系，并且是尺度无关的（scale-invariant），即独立于测量尺度。马氏距离的定义为：
$$D_M(x)=\sqrt{(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )}$$
其中，( x ) 是数据点，( $\mu$ ) 是样本均值，( $\Sigma$ ) 是协方差矩阵。如果协方差矩阵是单位矩阵，即各维度独立同分布，马氏距离就变成了欧氏距离。

引入马氏距离的目的

引入其目的在于：

1. 消除尺度相关性
2. 消除量纲的影响

总的来说，一个数据来源可能包含不用属性的物理量数据。

- 比如年龄（0-100），性别（男0, 女1），收入（月薪3000～N代XX人的传承）。
- 又比如在SLAM中，位置（单位m或mm）与旋转角（degree或四元数）放在一起优化。

如果忽略量纲归一化，基数小变化小的量被归一化非常小的数值，基数大变化大的最终归一化非常大的数值。这样会导致某一属性对最终结果的影响过大或过小。比如年龄30和年收入1000000， 归一化时就会产生年龄对最终收入的影响不大的结论。
此外，位移xyz虽然均采用同一尺度单位，但是相互之间独立，并不存在相关性。
马氏距离在公式中多了协方差矩阵的逆 ( ${\Sigma }^{-1}$ )。这一项的作用是排除变量之间相关性的干扰，并且排除测量量纲的影响。

欧氏距离在处理不同量纲的数据时，可能会因为量纲的不同而导致错误的距离判断，而马氏距离通过协方差矩阵的逆进行归一化处理，使得不同量纲的数据在计算距离时能够得到更准确的结果。

Ceres针对四元数进行优化

在ceres中使用四元数表示旋转时，需要使用不同的参数化类LocalParameterization，并实现该类所有纯虚函数。

ceres::LossFunction* loss_func = new ceres::HuberLoss(1.0);
ceres::LocalParameterization* qlp = new EigenQuaternionParameterization;
for(auto point:pointset){
	ceres::CostFunction * cost = ErrorTypes::Create(image_points[i]);
	problem->addResidualBlock(cost,
							loss_func,
							q.coeffs().data(),
							t.data(),
							point.data());
	problem->setParameteriation(q.coffs().data());
	problem->setParameterBlockConstant(point.data());
}  
ceres::Solver::options Op;
op.max_num_iterations = 1000;
op.linear_colver_type = ceres::DENSE_QR;