LM算法参数优化实例——残差函数、雅可比矩阵

【目标】：使用LM算法估计$y=exp(a{t}^2+bt+c)$参数a, b, c。
已知量：$t$及与之对应的含有噪声的量测值$\hat{y}$；同时，模型已知（仅未知参数）。

仿真数据生成

取参数为a=1.0, b=2.0, c=1.0

	double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N = 100;                        // 数据点数目
    double w_sigma= 1.;                 // 噪声Sigma值（方差）

    std::default_random_engine generator;
    std::normal_distribution<double> noise(0.,w_sigma);

	// 构造 N 次观测
    for (int i = 0; i < N; ++i) {
        double t = i/100.;             // t的取值在0.0~1.0之间
        double n = noise(generator);
        double y = std::exp( a*t*t + b*t + c ) + n; //  // 含噪声的观测 y
    }

确定待优化变量、残差函数、及损失函数

待优化变量（Vertex）： 参数a, b, c，此时放在一个$3\times 1$的向量$x$中；
残差函数（Edge）： 以第k步迭代参数的估计值$x_k$（参数）和第i个自变量代入已知模型得到的预测值${\hat{y}}_k$，和第k步迭代的含有噪声的量测值$y_k$的差值作为残差。记为$f_i(x_k)$
损失函数（Problem）： $F(x_k)={\Sigma }_{i=1}^N\Vert f_i(x_k){\Vert }_2$

【代码1-待优化变量】：

    shared_ptr< CurveFittingVertex > vertex(new CurveFittingVertex());
    vertex->SetParameters(Eigen::Vector3d (0.,0.,0.));

其中，CurveFittingVertex继承自Vertex，且在构造函数中指明了一个3维向量：

// 曲线模型的顶点，模板参数：优化变量维度
class CurveFittingVertex: public Vertex{
public:
    CurveFittingVertex(): Vertex(3) {}  // abc: 三个参数， Vertex 是 3 维的
};

而Vertex在构造函数中，其成员变量VecX Parameters_;被resize为一个3维向量：

explicit Vertex(int num_dimension, int local_dimension = -1);//类内的声明
Vertex::Vertex(int num_dimension, int local_dimension) {    // 类外的定义
    parameters_.resize(num_dimension, 1);
    id_ = global_vertex_id++; // 初始值有：unsigned long global_vertex_id = 0;
}

注：VecX实际上是一个别名typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VecX;。

【代码2-残差项】
对于每一个观测量，构造其对应的残差（此时还并未真正去计算）：

// 每个观测对应的残差函数
shared_ptr< CurveFittingEdge > edge(new CurveFittingEdge(t,y));

// CurveFittingEdge 构造函数为：
class CurveFittingEdge: public Edge{
public:
    CurveFittingEdge( double x, double y ): Edge(1,1, std::vector<std::string>{"abc"}) {
        x_ = x;
        y_ = y;
    }
    ...
}

父类Edge的构造函数为：

Edge::Edge(int residual_dimension, int num_verticies,
           const std::vector<std::string> &verticies_types) {
    residual_.resize(residual_dimension, 1); // 残差向量
    jacobians_.resize(num_verticies);        // 雅可比矩阵
    // 信息矩阵
    Eigen::MatrixXd information(residual_dimension, residual_dimension);
    information.setIdentity();
    information_ = information;
}
...
补充其中用到的几个类型
    VecX residual_;                 // 残差
    std::vector<MatXX> jacobians_;  // 雅可比，每个雅可比维度是 residual x vertex[i]
    MatXX information_;             // 信息矩阵
    VecX observation_;              // 观测信息

同时，对于该观测量及其对应的残差，指定它们与哪些待优化的变量（参数）有关！

std::vector<std::shared_ptr<Vertex>> edge_vertex;
edge_vertex.push_back(vertex);  //本次曲线拟合例题中仅一个顶点（其他或许多个）
edge->SetVertex(edge_vertex);  // 指定该edge对应的顶点

// 该成员函数的定义：
bool SetVertex(const std::vector<std::shared_ptr<Vertex>> &vertices) {
    verticies_ = vertices;
    return true;
}
// 其中，成员变量的定义：
std::