slam14讲上半本

2024/9/8 SLAM

李群

群可以看作是一个集合+一个运算，如果集合的元素是连续的就叫李群

旋转矩阵R与矩阵乘法构成一个李群\(SO(3)=\{R\in \R^{3\times3}|RR^T=I,det(R)=1 \}\)，变换矩阵T与矩阵乘法构成一个李群\(SE(3)=\{T=\begin{pmatrix}R&t\\0&1\end{pmatrix}\in \R^{4\times4}|R\in SO(3),t\in\R^3 \}\)

将\(R\)视作一个随时间变化的函数\(R(t)\)来描述相机的位姿，根据\(RR^T=I\)可以推导出 \[ R'(t)R(t)^T=\phi(t)^\wedge \\ R'(t)=\phi(t_0)^\wedge R(t)=\begin{pmatrix}0&-\phi_3 & \phi_2\\\phi_3 &0&-\phi_1\\-\phi_2&\phi_1& 0 \end{pmatrix}R(t)\overset{def}{=}\phi_0^\wedge R(t) \] 第一个式子可知，在任意时刻t，都会有一个旋转矩阵\(R\)都对应一个\(\phi^\wedge\)

第二个式子视作微分方程求解得到如下，可知给出一个\(\phi\)可以对应一个\(R=e^{\phi}\) \[ R(t)=e^{\phi_0^\wedge t } \] 也就是，\(R\)和\(\phi^\wedge\)差不多是一一对应（如果旋转度数限制在\(0\sim360\)就是一一对应）

李代数

李代数是一种集合+一种运算，一个李群会对应一个李代数

\(\phi\)就是一种李代数\(so(3)=\{\phi\in\R^3 ,\Phi=\phi^\wedge\in\R^{3\times3} \}\)，与李群\(SO(3)=\{R\in \R^{3\times3}|RR^T=I,det(R)=1 \}\)对应（向量和反对称矩阵一一对应，所以定义成向量\(\R^3\) or 反对称矩阵\(\R^{3\times3}\)均可）

同样，\(se(3)=\{\xi=\begin{pmatrix}\rho\\\phi \end{pmatrix} \in\R^6,\rho\in\R^3,\phi\in so(3),\xi^\wedge =\begin{pmatrix}\phi^\wedge &\rho\\ 0&0\end{pmatrix}\in\R^{4\times4} \}\)是一种李代数，与\(SE(3)=\{T=\begin{pmatrix}R&t\\0&1\end{pmatrix}\in \R^{4\times4}|R\in SO(3),t\in\R^3 \}\)对应

\(se(3)\)的\(\wedge\)不表示反对称矩阵，而表示\(\xi^\wedge =\begin{pmatrix}\phi^\wedge &\rho\\0&0\end{pmatrix}\in \R^{4\times4}\)

（2个李代数里的运算略，好像没用上）

李群与李代数互转

\(so(3)\)是3维向量的集合，一个向量\(\pmb{\phi}\in so(3)\)可以分成模长\(\theta\)与方向向量\(\pmb{a}\)的乘积\(\pmb{\phi}=\theta \pmb{a}\)

想要计算\(\pmb{R}=e^{\pmb{\phi}^\wedge}\)，可以用泰勒展开，并且\(\pmb{\phi}=\theta \pmb{a}\)中的\(\pmb{a}\)是方向向量所以可以优化幂次，最后得到下式 \[ \pmb{R}=e^{\theta\pmb{a}^\wedge}=cos\theta \mathbf{I}+(1-cos\theta)\mathbf{a}\mathbf{a}^T+sin\theta \mathbf{a}^\wedge \] 与Rodrigues公式（计算旋转矩阵与旋转向量互转）一样，所以\(so(3)\)实际就是旋转向量组成的空间，所以可以同Rodrigues公式一样从\(\pmb{R}\)计算\(\theta,\pmb{a}\)

再同理可以得到\(SE(3)\)与\(se(3)\)的互转，下面是书上的总结图

啥

前面得出\(R,T\)可以与\(\phi,\xi\)一一对应互转，下面有\(R,T\)运算规则的转换 \[ \begin{align} \begin{cases} exp(\Delta\phi^\wedge)exp(\phi^\wedge)\approx exp((\pmb{J_l^{-1}}\Delta\phi+\phi)^\wedge)\\ exp(\pmb{J_l}\Delta\phi^\wedge)exp(\phi^\wedge)\approx exp((\Delta\phi+\phi)^\wedge)\\ \end{cases}\\ where\ \begin{cases} J_l^{-1}=\frac{\theta}{2}cot\frac{\theta}{2}\pmb{I}+(1-\frac{\theta}{2}cot\frac{\theta}{2})\pmb{a}\pmb{a}^T-\frac{\theta}{2}\pmb{a}^\vee\\ J_l=\frac{sin\theta}{\theta}\pmb{I}+(1-\frac{sin\theta}{\theta})\pmb{a}\pmb{a}^T+\frac{1-cos\theta}{\theta}\pmb{a}^\wedge \end{cases} \end{align} \] 对于一个旋转矩阵\(exp(\phi^\wedge)\)，如果乘一个新的旋转矩阵\(exp(\Delta\phi^\wedge)\)，约等于原本的李代数\(\phi\)加上\(\pmb{J}_l^{-1}\)倍的新的李代数\(\Delta\phi\)（但后续\(J_l\)用不上）

\(SE(3)\)与\(se(3)\)同理，但公式没细给

求导和扰动模型

李代数的作用是，李群没有定义加法只定义了乘法，乘法求导也会包含加法所以只有乘法就没法求导所以改为在李代数上求导

分直接求导（略）和扰动模型（下面）

假设有空间点\(\pmb{p}\)进行了旋转\(\pmb{R}\)，用李代数可以得出\(\frac{\partial (\pmb{R}\pmb{p})}{\partial \pmb{\phi}}=-(\pmb{R}\pmb{p})^\wedge\)（\(\phi\)是\(\Delta \pmb{R}\)对应的李代数）

假设有空间点\(\pmb{p}\)进行了变换\(\pmb{T}\)，同理得出\(\frac{\partial (\pmb{T}\pmb{p})}{\partial \pmb{\delta\xi}}=\begin{pmatrix}\pmb{I}&-(\pmb{R}\pmb{p}+\pmb{t})^\wedge\\0&0 \end{pmatrix}\)

上式解释：

\(\delta\xi\)是\(\Delta T\)对应的李代数；
结果的形状：结果是一个\(4\times 6\)的矩阵，\(-(\pmb{R}\pmb{p}+\pmb{t})^\wedge\)是\(4\times 4\)但第4行是全0（看前面对于\(\wedge\)的解释），所以那个\(\pmb{I}\)是3阶单位阵，下面全是0，所以这\(4\times 6\)是前3行有非0数，第4行是全0
非齐次坐标形式（BA推导用到）：这里\(\pmb{p}\)是齐次坐标因为\(\pmb{T}\)就是利用齐次坐标把旋转+平移变成一次矩阵乘法了，所以如果想要非齐次坐标的形式把第4行0去掉即可\(\frac{\partial (\pmb{T}\pmb{p})}{\partial \pmb{\delta\xi}}=\begin{pmatrix}\pmb{I}&-(\pmb{R}\pmb{p}+\pmb{t})^\wedge \end{pmatrix}\)

（\(\delta\xi\)是\(\Delta T\)对应的李代数，这里\(\pmb{p}\)是齐次坐标，结果是一个\(4\times 6\)的矩阵，\(-(\pmb{R}\pmb{p}+\pmb{t})^\wedge\)是\(4\times 4\)但第4行是全0（看前面对于\(\wedge\)的解释），所以那个\(\pmb{I}\)是3阶单位阵，下面全是0，所以这\(4\times 6\)是前3行有非0数，第4行是全0）

我对求导和扰动模型的理解：书上先介绍了直接求导的方法，又介绍了扰动模型的方法。这俩的区别就是：

直接求导是对李群求导\(\frac{\partial(\pmb{R}\pmb{P})}{\partial \pmb{R}}\)，但推导的时候是全部转到李代数上推导了；

扰动模型是对李代数求导\(\frac{\partial(\pmb{R}\pmb{P})}{\partial \pmb{\phi}}\)，还是全部转到李代数上推导

因为李代数和李群一一对应，所以扰动模型不是严格意义的求导，求出的是某个函数\(F\)关于李代数的导

用处就是（BA就是这么用的），想要求\(F(\pmb{R})\)的极值点的时候，改成\(F(exp(\pmb{\xi}))\)，然后求\(\frac{\partial F}{\partial \pmb{\xi}}\)，得到\(\pmb{\xi}_0\)是极值点，则\(R_0=exp(\pmb{\xi}_0)\)就是\(F\)的极值点

总结

这段得出来，每个李群会与一个李代数一一对应，得出了\(SO(3)\)与\(so(3)\)的互转公式、\(SO(3)\)的运算怎么在\(so(3)\)上进行、\(SO(3)\)求导怎么求。\(SE(3)\)与\(se(3)\)相同

单目相机跳

双目相机

假设左眼相机、右眼相机水平放置，光心距离是基线记为\(b\)

任意空间点\(P\)在左右眼相机的像素坐标坐标只有水平坐标有差别，记作\(u_L,u_R\)，记\(z\)是P到光心的距离，如下图

则有关系 \[ \frac{z-f}{z}=\frac{b-u_L+u_R}{b}\\ \Rightarrow z=\frac{fb}{d}, d=u_L-u_R \] z是空间点P到光心的距离，f是焦距也就是成像平面到光心的距离，d称为视差

视差最小为一个像素因此双目深度有最大值

一张左图一张右图建立深度图建立点云：代码没看懂，需要StereoSGBM计算出视差图（视差图怎么实现稠密的，特征点很稀疏），然后用双目相机模型（即上面的公式）画出点云图（主要就是视差图怎么得到的不知道，貌似是有假设相同像素点or块都在同一水平线上，然后分块匹配的所以视差图全是折线）

非线性优化

运动方程、观测方程 \[ \begin{align} \pmb{x}_k&=f(\pmb{x}_{k-1},\pmb{u}_k)+,\pmb{w}_k\\ z_{k,j}&=h(\pmb{y}_j,\pmb{x}_k)+\pmb{v}_{k,j} \end{align} \] \(x\in SE(3)\)，\(\pmb{y}\)是路标（即空间点的世界坐标，所以观测方程就是针孔相机模型），\(\pmb{w},\pmb{v}\)是噪声，\(z\)是观测的某个路标的像素位置

我好像了解贝叶斯和最大似然估计了 \[ \underbrace{P(B|A)}_{后验}=\frac{P(A|B)P(B)}{P(A)}\propto \underbrace{P(A|B)}_{似然}\underbrace{P(B)}_{先验}\\ \Rightarrow B_{MEL}=argmax\ P(B|A)=argmax\ P(A|B) \] （SLAM14讲P116）在有了观测A的情况下估计B，就是找在A的情况下最可能发生的B即\(P(B|A)\)最大的那个B，贝叶斯公式得出等于找B的概率\(\times\)在B发生的情况下A发生概率最大的那个A。如果先验没有，就是找最大似然概率

SLAM14讲的状态估计问题就是把运动方程和观测方程代入了最大似然估计，其他没啥了

非线性最小二乘

求解\(min_\pmb{x}\ F(\pmb{x})=\frac{1}{2}\parallel f(\pmb{x})\parallel_2^2\)

如果\(f(\pmb{x})\)比较简单，则可以对\(F(\pmb{x})\)求导令=0

如果\(f(\pmb{x})\)不够简单，则泰勒展开，用简单的多项式近似原函数，然后迭代求解（因为此时是近似所以每次使得目标函数变小后重新近似）。迭代方法是从一个初始值\(\pmb{x}_0\)开始，每次找一个增量\(\Delta\pmb{x}_k\)使得\(F(\pmb{x}_k+\Delta\pmb{x}_k)\)达到最小值，直到\(\Delta\pmb{x}_k\)足够小

假设第k次迭代，将\(F(\pmb{x})\)泰勒在\(\pmb{x}_k\)处展开得到\(F(\pmb{x})\)的近似 \[ F(\pmb{x}_k+\Delta\pmb{x}_k)\approx F(\pmb{x}_k)+\pmb{J}(\pmb{x}_k)^T\Delta\pmb{x}_k+\frac{1}{2}\Delta\pmb{x}_k^T\pmb{H}(\pmb{x}_k)\Delta\pmb{x}_k \] 选择保留一阶项，此时目标函数相当于\(\Delta\pmb{x}_k\)的一次函数，则取梯度的反方向\(\Delta\pmb{x}_k=-\pmb{J}(\pmb{x}_k)\)就可以使函数值下降，称为最速下降法

选择保留二阶项，则此时相当于\(\Delta\pmb{x}_k\)的二次函数，求导令=0得到\(\pmb{H}\Delta\pmb{x}_k=-\pmb{J}\)，称为牛顿法

最速下降法和牛顿法相当于用一次 or 二次函数近似原函数，最速下降法迭代次数可能较多，牛顿法需要计算\(\pmb{H}\)算得慢

还可以展开\(f(\pmb{x})\)而不是\(F(\pmb{x})\)有\(f(\pmb{x}+\Delta\pmb{x})\approx f(\pmb{x})+\pmb{J}(\pmb{x})^T\Delta\pmb{x}\)。按迭代方法现在需要找\(\Delta\pmb{x}\)使得\(\frac{1}{2}\parallel f(\pmb{x})+\pmb{J}(\pmb{x})^T\Delta\pmb{x}\parallel_2^2\)达到极小值，展开平方项依然是\(\Delta\pmb{x}\)的二次函数所以求导令=0得到\(\underbrace{\pmb{J}(\pmb{x})\pmb{J}^T(\pmb{x})}_{\pmb{H}(\pmb{x})}\Delta\pmb{x}=\underbrace{-\pmb{J}(\pmb{x})f(\pmb{x})}_{\pmb{g}(\pmb{x})}\)即\(\pmb{H}\Delta\pmb{x}=\pmb{g}\)，称为高斯牛顿法

高斯牛顿法的迭代过程变为：初始选\(\pmb{x}_0\)，第k次求解\(\pmb{H}\Delta\pmb{x}_k=\pmb{g}\)得到\(\Delta\pmb{x}_k\)，然后\(\pmb{x}_{k+1}=\pmb{x}_k+\Delta\pmb{x}_k\)，直到\(\Delta\pmb{x}_k\)足够小

高斯牛顿法与牛顿法对比，有点像用\(\pmb{H}=\pmb{J}\pmb{J}^T\)近似真正的Hessian矩阵\(\pmb{H}\)（二者等式右边也不一样）。但这个近似只有在距离展开点足够近效果才好，所以可以对\(\Delta\pmb{x}_k\)添加范围限制称为信赖区域，用原函数的差值与近似的差值的比来评价近似的好坏\(\rho=\frac{f(\pmb{x}+\Delta\pmb{x})-f(\pmb{x})}{\pmb{J}(\pmb{x}^T\Delta\pmb{x})}\)，只有比例接近1是最好的（书上的没看懂，为什么\(\rho\)比较大是放大近似范围？不是应该只要离1远就减小近似范围吗？），高斯牛顿法加上信赖区域称为列文伯格-马夸尔特优化

code for 手写

假设\(f(x)=exp(ax^2+bx+c)+w,\ w\sim(0,\sigma^2)\)，自己找个abc的值作为真实值，再加上噪声生成观测值，然后对abc取个初值开始迭代。会使用一批观测值进行迭代，类似mini batch

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
    double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
    double ae = 2.0, be = -1.0, ce = 0.65;        // 估计参数值
    int N = 1000;                                 // 数据点
    double w_sigma = 1.0;                        // 噪声Sigma值
    double inv_sigma = 1.0 / w_sigma;
    cv::RNG rng;                                 // OpenCV随机数产生器

    vector<double> x_data, y_data;      // 数据
    for (int i = 0; i < N; i++) {
        double x = (double)i / N;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
    }

    // 开始Gauss-Newton迭代
    int iterations = 100;    // 迭代次数
    double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for (int iter = 0; iter < iterations; iter++) {

        Matrix3d H = Matrix3d::Zero();             // Hessian = J^T W^{-1} J in Gauss-Newton
        Vector3d b = Vector3d::Zero();             // bias
        cost = 0;

        for (int i = 0; i < N; i++) {
            double xi = x_data[i], yi = y_data[i];  // 第i个数据点
            double error = yi - exp(ae * xi * xi + be * xi + ce);
            Vector3d J; // 雅可比矩阵
            J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
            J[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/db
            J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc

            H += inv_sigma * inv_sigma * J * J.transpose();
            b += -inv_sigma * inv_sigma * error * J;

            cost += error * error;
        }

        // 求解线性方程 Hx=b
        Vector3d dx = H.ldlt().solve(b);
        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost >= lastCost) {
            cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
            break;
        }

        double step=0.1;
        ae += step*dx[0];
        be += step*dx[1];
        ce += step*dx[2];

//        ae += dx[0];
//        be += dx[1];
//        ce += dx[2];

        lastCost = cost;

        cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
             "\t\testimated params: " << ae << "," << be << "," << ce << endl;
    }

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    return 0;
}

手写的代码原始的没写step，要拟合的是一个二次曲线，如果把ce改一下就容易从这边一步跨到那一边，就会如图1直接break了，添上step=0.1就可以正确拟合了

code for ceres

书上的ceres拟合曲线

//
// Created by xiang on 18-11-19.
//

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

// 代价函数的计算模型
struct CURVE_FITTING_COST {
    CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}

    // 残差的计算
    template<typename T>
    bool operator()(
            const T *const abc, // 模型参数，有3维
            T *residual) const {
        residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c)
        return true;
    }

    const double _x, _y;    // x,y数据
};

int main() {
    double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
    double ae = 5, be = -1, ce = -3;        // 估计参数值
    int N = 1000;                                 // 数据点
    double w_sigma = 1.0;                        // 噪声Sigma值
    double inv_sigma = 1.0 / w_sigma;
    cv::RNG rng;                                 // OpenCV随机数产生器

    vector<double> x_data, y_data;      // 数据
    for (int i = 0; i < N; i++) {
        double x = (double)i / N;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
    }

    double abc[3] = {ae, be, ce};

    // 构建最小二乘问题
    ceres::Problem problem;
    for (int i = 0; i < N; i++) {
        problem.AddResidualBlock(     // 向问题中添加误差项
                // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
                new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
                        new CURVE_FITTING_COST(x_data[i], y_data[i])
                ),
                nullptr,            // 核函数，这里不使用，为空
                abc                 // 待估计参数
        );
    }

    // 配置求解器
    ceres::Solver::Options options;     // 这里有很多配置项可以填
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;  // 增量方程如何求解
    options.minimizer_progress_to_stdout = true;   // 输出到cout

    ceres::Solver::Summary summary;                // 优化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve(options, &problem, &summary);  // 开始优化
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

    // 输出结果
    cout << summary.BriefReport() << endl;
    cout << "estimated a,b,c = ";
    for (auto a:abc) cout << a << " ";
    cout << endl;

    return 0;
}

code for g2o

运行不了，但也可以把代码看完纸上谈兵的

对虚函数的复习：

一个类里可以定义虚函数和普通函数，二者都可以被派生类重新定义，区别在于：重新定义虚函数，则会有多态性质，也就是说派生类中会同时包含原本的虚函数和重新定义的虚函数，根据传入参数的不同决定调用哪个函数；重新定义普通函数，则没有多态性质，派生类里只有重新定义的函数了

声明虚函数和重新定义虚函数的实例：

class Base {
public:
    // 声明一个虚函数
    virtual void myVirtualFunction() {
        // 这里是函数的实现
    }
};

class Derived : public Base {
public:
    // 重新定义基类中的虚函数
    void myVirtualFunction() override {
        // 这里是派生类中的实现
    }
};

g2o实现非线性最小二乘：

#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>

using namespace std;

// 派生g2o的basevertex作为顶点类
// 本次的顶点是(ae,be,ce)这3个参数组成一个顶点，所以整张图只有一个顶点，顶点包含的数据的维数是3，数据类型是Vector3d
// 需要重写4个父类的函数
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 问gpt的：EIGEN_MAKE_ALIGNED_OPERATOR_NEW 是一个宏，用于为类重载 operator new 和 operator delete，以确保内存对齐，特别是对于使用 Eigen 库的类，这很重要

    // 重置
    virtual void setToOriginImpl() override {
        _estimate << 0, 0, 0;
    }

    // 更新
    virtual void oplusImpl(const double *update) override {
        _estimate += Eigen::Vector3d(update);
    }

    // 存盘和读盘：留空
    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}
};

// 派生g2o的BaseUnaryEdge作为边类
// 本次的边是一个观测点和真实点的误差，所以一条边包含的数据是1维，数据类型是double
// 需要重写父类的4个函数
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}

    // 计算曲线模型误差
    virtual void computeError() override {
        const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
    }

    // 计算雅可比矩阵
    virtual void linearizeOplus() override {
        const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
        _jacobianOplusXi[0] = -_x * _x * y;
        _jacobianOplusXi[1] = -_x * y;
        _jacobianOplusXi[2] = -y;
    }

    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}

public:
    double _x;  // x 值， y 值为 _measurement
};

int main(int argc, char **argv) {
    double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
    double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
    int N = 100;                                 // 数据点
    double w_sigma = 1.0;                        // 噪声Sigma值
    double inv_sigma = 1.0 / w_sigma;
    cv::RNG rng;                                 // OpenCV随机数产生器

    vector<double> x_data, y_data;      // 数据
    for (int i = 0; i < N; i++) {
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
    }

    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型

    // 梯度下降方法，可以从GN, LM, DogLeg 中选
    auto solver = new g2o::OptimizationAlgorithmGaussNewton(
            g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm(solver);   // 设置求解器
    optimizer.setVerbose(true);       // 打开调试输出

    // 往图中增加顶点
    CurveFittingVertex *v = new CurveFittingVertex();
    v->setEstimate(Eigen::Vector3d(ae, be, ce));
    v->setId(0);
    optimizer.addVertex(v);

    // 往图中增加边
    for (int i = 0; i < N; i++) {
        CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
        edge->setId(i);
        edge->setVertex(0, v);                // 设置连接的顶点
        edge->setMeasurement(y_data[i]);      // 观测数值
        edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆
        optimizer.addEdge(edge);
    }

    // 执行优化
    cout << "start optimization" << endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

    // 输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout << "estimated model: " << abc_estimate.transpose() << endl;

    return 0;
}

g2o是怎么确定点和边的？

LOADING