统一最小二乘标准问题形式

一. 残差看成向量形式的推导

 这里的误差是符合卡方误差准则的。

再对使用VIO----相关公式解释说明_AutoGalaxy的博客-CSDN博客 常用的一阶泰勒展开

其中的J如下所示

其中的 如下所示

 把一阶泰勒展开代入F(x)得到：

 上面最下面那个式子就叫做正则方程。

对于LM算法，在上面的基础之上还要添加阻尼因子：如下所示

 再进行求导：

 Note：

1. 阻尼因子非常大，接近最速下降法。

2. 阻尼因子非常小，接近高斯牛顿法。

阻尼因子的选取，有一些专用的策略：

1 . 策略一 （经验）

2. 策略二. （定量分析）

由此衍生出来：

其中 

首先要明白：由来近似

                       那么就是由来近似

当 更新成时，损失函数实际变化的数值是 

而L函数是用来近似F的函数

因此：

我们以为的下降的数值则是 

因此

因此比例因子的物理意义则是：实际损失函数变化的值  与  我们以为下降的数值  之比

 为什么比例因子较小的时候，需要增大阻尼呢？

首先，我们的目标肯定是希望，F(x)下降得越多越好，因此分母要尽可能大，分子要尽可能小。

非线性优化整理-3.Levenberg-Marquardt法(LM法)_boksic的博客-CSDN博客_lm法

二. LM算法编程实现

2.1 使用深蓝学院的优化库

公共代码解读：

Problem类：

1. 添加顶点代码

typedef std::map<unsigned long, std::shared_ptr> HashVertex;
 
HashVertex verticies_;
 
bool Problem::AddVertex(std::shared_ptr vertex) {
    if (verticies_.find(vertex->Id()) != verticies_.end()) {
        // LOG(WARNING) << "Vertex " << vertex->Id() << " has been added before";
        return false;
    } else {
        verticies_.insert(pair<unsigned long, shared_ptr>(vertex->Id(), vertex));
    }
 
    return true;
}

首先 verticies_是一个map容器类，if语句用来判断，此顶点是否已经被添加，如果已经添加，则不再添加了。找到元素返回对应元素的迭代器，否则返回最后一个元素的迭代器。

否则，就插入这个顶点，以及对应的id号，要注意顶点插入的格式要借助pair类型的模板。

2.1.1 以普通优化问题为例子（根据数据点拟合曲线