背景

本人手头有一堆通过初始配准后的三维散乱点云，通过粗配后，全副点云根本对立在了同一个坐标系下，如图 1 和图 2 所示。为了获取更好的全局后果，须要对粗配后的点云进行全局配准优化。

上述点云是基于 pcl 进行显示，不同的色彩代表不同的点云块，从图 1 能够显著看出，全局点云 ” 交融 ”(其实并未交融，只是全副加载显示)，成果较差，从图 2 能够看出，针对 [耳朵] 局部呈现较大错位。

因为本人之前对 G2O 多多少少有一点理解，然而也并没有进行过多深刻的钻研。晓得G2O 能够用来解决全局优化问题，正好和本人要求解的问题十分十分类似。

因而，便毅然决然的抉择应用 G2O 来针对本人的问题构建解决方案。

G2O 的进一步理解

G2O 是将 非线性优化和图论 相结合的产物，大多时候用于解决 slam 后端优化问题，其本身携带的例子大多也与 slam 无关。g2o 中所谓的 图，就是一堆 节点 和边 依照肯定的关系链接而成。其中，节点是要优化的指标，边是各个优化指标之间的关系(也称其误差项，在这里本人更喜爱称为【关系】)。

基于 CMake + VS2017 实现 G2O 库的装置，装置过程没有做具体记录，根本百度可能解决。

装置完 g2o 后，依照习惯去翻看其本身源码中所携带的 example，以便寻找灵感，在 example 目录中一眼便看中了【gicp_demo】。

G2O–gicp_demo

g2o 的应用办法根本就是：

1. 申明一个优化器；
2. 抉择求解办法；
3. 结构图 – 顶点和边的关系；
4. 优化解决。

首次看到 g2o 的 gicp 时，自认为“该 gicp 办法是 全局 (global) 的”，然而事实并非如此，事实上，其甚至不能称为是一个残缺的 icp 问题

(对于上述红色字体的示意，目前仅仅是集体了解，或者有谬误，也请多多留言斧正，一起交流学习)

咱们晓得，ICP 求解是一个迭代计算过程，经典 ICP 求解的次要步骤为：

1. 输出两片点云 AB，求解对应点对(三维模型至多 3 个不共线的点)；
2. 基于对应点对，结构 A 到 B 的变换矩阵 M；
3. 将 M 作用于 A，失去 A *，并用 A * 代替 A；
4. 迭代终止条件(迭代次数或者最小误差度量)；
5. 反复步骤 1 –3，直到满足步骤 4，终止。
6. 输入变换矩阵 M。

然而细看 g2o 的 gicp_demo，其流程并不如经典 ICP 求解过程一样，而更像一个 ICP 中步骤 2 的求解问题。

再来深刻看看 g2o 中给出的 gicp_demo。

拆解 gicp_demo

首先还是间接先把 g2o 官网例子贴出来吧(尽管十分厌恶间接贴他人代码)，便于阐明。

在该 Demo 中，g2o 首先申明并设置了优化器 optimizer，并制作了蕴含 1000 个点的汇合true_points 作为源点云。

其次，为图增加了两个节点并设置节点 ID。这里的节点类型为 VertexSE3（class G2O_TYPES_SLAM3D_API VertexSE3 : public BaseVertex<6, Isometry3>），也是次要的优化指标。根据节点增加到图中的程序，将第一次增加的节点视为固定视角；vc->setEstimate(cam); 该代码段通知咱们，真正求解的后果是存储在 Eigen::Isometry3d 类型的相机位姿 (实质上是一个矩阵)，这里cam 参数的类型是Eigen::Isometry3d；这一步其实只是申明了两个空节点，节点参数只是单位Eigen::Isometry3d。

再次，为图增加了 1000 条边。在此过程中，首先依据节点 id 获取边所须要链接的两个顶点 (节点)，vp0 和vp1, 并基于 true_points “ 制作 ” 了蕴含噪声的两个三维点pt0 和 pt1（这一步其实曾经默认 pt0 和 pt1为对应点对）；而后 申明了一个 Edge_V_V_GICP 类型的图边构造，该边是一个 g2o 的二元边 (class G2O_TYPES_ICP_API Edge_V_V_GICP : public BaseBinaryEdge<3, EdgeGICP, VertexSE3, VertexSE3>)，该二元边别离链接vp0 和vp1；g2o 还提供了 EdgeGICP 类型作为观测值，EdgeGICP类型能够寄存对应点对。在该步骤中，肯定要 十分留神节点和三维坐标点的所属 – 对应关系。至此，根本可能将所有信息放入 g2o 的图中，该步骤次要关怀的在于如何将本人的三维点对放入到 g2o 图中。

最初，初始化图关系并进行了 N 次迭代优化，每个节点的优化后果存储在 optimizer.vertices() 返回值类型的哈希表中，该哈希表中：键 – 对应节点 id，值 – 对应节点，这里为 VertexSE3 类型，从 VertexSE3 取得的 Eigen::Isometry3d 类型是咱们真正关怀的后果数据。
该示例应该构建了如下一张超图，其中图有两个图节点，n1 为固定节点，n2 为变动的节点，节点之间有 1000 条边，每条边链接一对对应点，针对 ICP 问题，对应点中的固定点挂接图节点 n1，变动的点挂接图节点 n2：

void icp() {
    double euc_noise = 0.01;       // noise in position, m

    // 申明优化器
    SparseOptimizer optimizer;
    // 是否打印详细信息
    optimizer.setVerbose(false);
    
    // variable-size block solver
    // 设定一个求解办法
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<BlockSolverX>(g2o::make_unique<LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>()));

        /*g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverX>(g2o::make_unique<LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>()));*/

    // 设定优化器应用的优化办法
    optimizer.setAlgorithm(solver);

    // 随机点坐标
    vector<Vector3d> true_points;
    for (size_t i = 0; i < 1000; ++i)
    {
        // 这里从均匀分布中采样了一组数字
        true_points.push_back(Vector3d((g2o::Sampler::uniformRand(0., 1.) - 0.5) * 3,
            g2o::Sampler::uniformRand(0., 1.) - 0.5,
            g2o::Sampler::uniformRand(0., 1.) + 10));
    }

    // set up two poses
    // 猜想: 设定两个相机位姿
    int vertex_id = 0;
    for (size_t i = 0; i < 2; ++i)
    {
        // set up rotation and translation for this node
        // 平移向量
        Vector3d t(0, 0, i);
        // 四元数旋转
        Quaterniond q;
        q.setIdentity();

        // 李群(非凡欧拉群，蕴含旋转和平移，本人感觉和 4 * 4 矩阵相似)
        Eigen::Isometry3d cam; // camera pose
        cam = q;
        // 返回平移向量的可写表达式
        cam.translation() = t;

        // set up node
        // 李群。这里作为节点, 作为优化变量
        VertexSE3 *vc = new VertexSE3();
        // 设置顶点的估计值
        vc->setEstimate(cam);
        // 设置该节点在 图 中的 id，以便追踪
        vc->setId(vertex_id);      // vertex id

        // 打印节点的初始平移和旋转矩阵
        cerr << t.transpose() << "|" << q.coeffs().transpose() << endl;

        // set first cam pose fixed
        if (i == 0)
            vc->setFixed(true);

        // add to optimizer
        // 优化器增加节点
        optimizer.addVertex(vc);

        vertex_id++;
    }

    // set up point matches
    for (size_t i = 0; i < true_points.size(); ++i)
    {
        // get two poses
        // 获取前述增加的节点。/* optimizer.vertices()的返回值是一个哈希表 (Map) 类型，实质是 std::unordered_map<int, Vertex*>，*/
        VertexSE3* vp0 =
            dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
        VertexSE3* vp1 =
            dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);

        // calculate the relative 3D position of the point
        Vector3d pt0, pt1;
        pt0 = vp0->estimate().inverse() * true_points[i];
        pt1 = vp1->estimate().inverse() * true_points[i];

        // add in noise
        // 增加高斯噪声
        pt0 += Vector3d(g2o::Sampler::gaussRand(0., euc_noise),
            g2o::Sampler::gaussRand(0., euc_noise),
            g2o::Sampler::gaussRand(0., euc_noise));

        pt1 += Vector3d(g2o::Sampler::gaussRand(0., euc_noise),
            g2o::Sampler::gaussRand(0., euc_noise),
            g2o::Sampler::gaussRand(0., euc_noise));

        // form edge, with normals in varioius positions
        Vector3d nm0, nm1;
        nm0 << 0, i, 1;
        nm1 << 0, i, 1;
        nm0.normalize();
        nm1.normalize();

        //g20 的二元边
        Edge_V_V_GICP * e           // new edge with correct cohort for caching
            = new Edge_V_V_GICP();

        e->setVertex(0, vp0);      // first viewpoint
        e->setVertex(1, vp1);      // second viewpoint

        EdgeGICP meas;
        meas.pos0 = pt0;
        meas.pos1 = pt1;
        meas.normal0 = nm0;
        meas.normal1 = nm1;

        // 定义观测值
        e->setMeasurement(meas);
        //        e->inverseMeasurement().pos() = -kp;

        meas = e->measurement();
        // use this for point-plane
        // 束缚信息(协方差矩阵的逆) = 点面的精度矩阵信息
        e->information() = meas.prec0(0.01);

        optimizer.addEdge(e);
    }

    // move second cam off of its true position
    // 变换第二个点云。VertexSE3* vc =
        dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
    Eigen::Isometry3d cam = vc->estimate();
    cam.translation() = Vector3d(0, 0, 0.2);
    vc->setEstimate(cam);

    // 初始化整个图构造
    optimizer.initializeOptimization();
    // 计算所有边的误差向量
    optimizer.computeActiveErrors();

    // 输入优化前的误差平方
    cout << "Initial chi2(before opt) =" << FIXED(optimizer.chi2()) << endl;

    optimizer.setVerbose(true);

    optimizer.optimize(5);

    // 输入优化前的误差平方
    cout << "Initial chi2(after opt) =" << FIXED(optimizer.chi2()) << endl;

    // 打印变动矩阵
    cout << endl << "Second vertex should be near 0,0,1" << endl;
    cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second)
        ->estimate().translation().transpose() << endl;
    cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second)
        ->estimate().translation().transpose() << endl;}

测试

执行 g2o 自带的上述例子，最终可能打印出变换矩阵。而后将该例子独自摘出来，换入本人的数据(对应点对)，也能输入变换矩阵，而后将变换矩阵作用于点云 , 后果如图 3：

可能早有大神预料到会是如上后果！！不得不说，g2o 的优化后果也太差强人意 ……

真是这样吗？……

小结

如果依照上述流程，按照 g2o 的官网例子，后果真是那样！！！然而和 pcl 的 icp 比照，后果是真的差，问题出在哪里？

问题出在上文中红字局部，这里仍然用红字揭示本人 —g2o 的 gicp 并不能算齐全的 ICP 求解计划。

通过剖析官网代码例子能够发现，其在求解前，实质上曾经默认了输出点对是对应的，在此基础上进行迭代计算，实质是根据同一组对应点对，迭代计算该组对应点之间的最优变换矩阵，对于整体两片点云来说，这样其实只是实现了一次 icp 计算，后果当然不现实。

换句话说，该 Demo 脱漏 (或者 g2o 本就如此设计，或是本人理解不够)ICP 迭代计划中对应点对的计算过程，也就是短少了 步骤 1 ，g2o–gicp 只是单纯的计算了 步骤 2 ，只失去了单次的最优变换矩阵。对于整体两片点云的 icp 求解问题，在进行第 2 次求解时，对应点对的对应关系曾经产生了变动，因而 g2o—gicp_demo 失去的矩阵只是单次的最优矩阵，所以后果也就如图 3 所示。

那么如何失去整体最优后果呢？当然是将上述步骤放入大循环中，每计算完一次 g2o–gicp，变换点云，从新求解对应关系，顺次迭代计算。后果图省略 …

构建优化图

在充沛了解了 g2o 自带 icp 例子后，回到最后本人要求解的问题。有 N 片粗配后的散乱点云，想要对 全局点云 进行全局优化配准。

所谓粗配后的全局点云，具备以下特点：

1. 相邻两片点云之间具备较高重叠率；
2. 相隔 (至多一块点云) 点云之间有或没有重叠；
3. 每块点云能够有一个、零个 (这个条件能够存在，结构 g2o 图时与条件 1 并不抵触) 或者多个高重叠率的点云；
4. 点云之间的重叠关系是对应的(即：A 与 B、C、D 重叠，那么 BCD 的重叠点云中肯定也有 A)。

优化指标

上述谈到的优化指标只是咱们理性上的意识，然而还必须要将优化指标转化为数学表白。口述如下：优化指标 = 求解 –N 片三维散乱点云，以点云 A 为指标点云，计算所有三维散乱点云配准到 A 的变换矩阵，该变换矩阵使得所有对应点对的欧式间隔获得最小，或者达到指定的迭代变换次数。

上述优化指标隐含：若 A 与 C 没有重叠，则 C 无奈间接向 A 进行配准对齐，然而 A 与 B，B 与 C 有重叠，则 C 变换到 A 则须要先变换到 B，由 B 的矩阵再变动到 A，而且要保障 A 与 B，B 与 C 均为最优变换，换句话来说，就是 B 最优变换到 A，C 最优变换到 B，则实现了 A B C 之间最优变换。

确认优化指标的数学表白后，还须要确认点云间的重叠率表白，口述如下：重叠率 = 两两点云对应点对的数量与该两片点云均匀点数的比值。

实现上述两个数学定义后，程序的总体流程应该如下：

1. 计算全局点云相互之间的对应点对与重叠率；
2. 重叠点云筛选(重叠率较低的点云认为无重叠，不参加 g2o 图中边的构建)；

3. g2o 全局构图：

   • 图节点
   • 边（重点）
   • 优化器与优化算法
4. 基于步骤 3 的输入矩阵，更新全副点云的坐标；
5. 反复步骤 1 -4，直到满足终止条件。

最终输入为优化后的全局点云及对应的变换矩阵。

程序实现

工程中应用了 PCL 点云库和 g2o(废话)，其中 pcl 次要用于计算点云重叠率。

通过上述【g2o–gicp_demo】可知，g2o 中曾经为 ICP 计划提供了定义好的图节点类 VertexSE3 和图二元边类Edge_V_V_GICP 以及边类EdgeGICP，这里间接拿来应用，省去了自定义图边、图节点麻烦(当然，若深刻学习 g2o，还是倡议多做更多摸索)。

点云数据结构

通过前述剖析，可知，某点云构造应该蕴含如下必要内容：

1. 点集；
2. 是否固定；
3. 变换矩阵；
4. 邻接信息；
5. 必要办法(最近邻点云、对应点对计算)。

结构如下：

typedef pcl::PointCloud<pcl::PointXYZ> pointcloud;

class MyPnts
{
public:
    MyPnts() ;
    ~MyPnts();
    int id;
    int v_number;
    
    vector<Vector3d> pts;

    bool fixed = false;
    
    Isometry3d pose; // 该点云的初始位姿，也是优化指标
    
    // 该点云的所有邻接信息
    vector<OutgoingEdge*> neighbours;
    
    // 以后点云在所有点云序列中的 k 个最近邻点云(重叠率最高的前 K 个)
    void computeKNNearPnts(vector< std::shared_ptr<MyPnts>>& frames, int k);
    
    // 对应点对计算
    void computeClosestPointsToNeighbours(vector< shared_ptr<MyPnts>>& frames, float thresh);
    
    /*
    * Method:    calCorrespond  计算两片点云的互相对应点对的索引
    * Access:    public
    * Returns:   std::unordered_map<int, std::vector<int>> first 0->src,1 -> tar;
    * Parameter: pointcloud::Ptr src
    * Parameter: pointcloud::Ptr tar
    */
    std::unordered_map<int, std::vector<int>> calCorrespond(MyPnts src, MyPnts tar,double dst = 10.0);

private:
};

其中，计算以后点云与全局其余点云 (除以后点云外) 重叠关系、对应点对均在 computeKNNearPnts() 函数中实现。

造图

这是最重要也是及其容易出错的中央。

前述 [粗配后全局点云的特点]，决定了 g2o 中图构造的特点，图边应该满足 [粗配后全局点云的特点] 的形容，伪图如下：

如上图所示，假如：全局有 5 片点云，则 g2o 图有 5 个节点，n1 为固定节点，n1 和 n2 有重叠，n2 和 n5 有重叠，n1 和 n5 也有重叠，但 n1 和 n3、n1 和 n4、n2 和 n4 等几个节点之间不存在重叠关系。

留神：这里的重叠关系，在 g2o 看来是束缚关系，进一步的，是两片点云之间的边的链接关系，此边可能有成千盈百个对应点对形成。上图中的带箭头的边仅示意示意，并非真正的图边。

在分明了图构造后，结构图代码如下：

void G2oPCL::global_icp2(std::vector<std::shared_ptr<MyPnts>> &pnts)
{
    using namespace g2o;
    using namespace std;
    using namespace Eigen;
    g2o::SparseOptimizer optimizer;
    optimizer.setVerbose(false);
    g2o::OptimizationAlgorithmLevenberg* solver = new                         g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<g2o::BlockSolverX>(g2o::make_unique<g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>()));
optimizer.setAlgorithm(solver);

// 节点
for (int i = 0; i < pnts.size(); ++i) {g2o::VertexSE3 *vc = new VertexSE3();

    vc->setEstimate((*pnts[i]).pose);
    vc->setId(i);      // vertex id
    if (i == 0) {vc->setFixed(true);
        (*pnts[i]).fixed = true;
    }
    optimizer.addVertex(vc);
}

// 结构全局边关系
for (int i = 0; i < pnts.size(); ++i) {std::shared_ptr<MyPnts> &current = pnts[i];
    int current_nebor = current->neighbours.size();
    for (int j = 0; j < current_nebor; ++j) {OutgoingEdge *oe = current->neighbours[j];
        int nearIdx = oe->neighbourIdx;
        std::shared_ptr<MyPnts> &dst = pnts[nearIdx];
        g2o::VertexSE3* vp0 =
            dynamic_cast<g2o::VertexSE3*>(optimizer.vertices().find(nearIdx)->second); //dstCloud
        g2o::VertexSE3* vp1 =
            dynamic_cast<g2o::VertexSE3*>(optimizer.vertices().find(current->id)->second); //srcCloud
        for (auto cor : oe->correspondances) {g2o::Edge_V_V_GICP * e = new g2o::Edge_V_V_GICP();
            e->setVertex(0, vp0);      
            e->setVertex(1, vp1);   
            g2o::EdgeGICP meas;
            meas.pos0 = dst->pts[cor.second];
            meas.pos1 = current->pts[cor.first];

            e->setMeasurement(meas);
            //use this for point-point
            e->information().setIdentity();
            
            optimizer.addEdge(e);    
        }    
    }
}

optimizer.initializeOptimization();

optimizer.computeActiveErrors();
double chiInit = optimizer.chi2(); //stop innerround if we get 100% better
    
optimizer.optimize(100);
cout << "round:" << "s" << "chi2:" << FIXED(chiInit) << endl;
for (int i = 0; i < pnts.size(); ++i) {VertexSE3 *res = dynamic_cast<VertexSE3*>(optimizer.vertices().find(i)->second);

    Isometry3d transAndRot = res->estimate();

    MyPnts &mypnts = *pnts[i];

    for (int j = 0; j < mypnts.v_number; ++j) {mypnts.pts[j] = transAndRot * mypnts.pts[j];
        }
    }
}

在上述代码中，不仅结构了图构造，设置了各个节点之间的互相关系，而且还更新了点的坐标，为不便下次迭代中计算对应点对提供了不便。

至此，基于 g2o 解决开始提到的问题框架根本实现，最终主程序代码如下：

int main()
{std::unique_ptr<G2oPCL> gp = std::make_unique<G2oPCL>();
    
    clock_t begin, end;
    begin = clock();

    std::vector<shared_ptr<MyPnts>> pnts;
    loadPnts(pnts, "");

    // 计算与指标点云重叠率最高的两片点云
    computeNumbers(pnts, 2);

    int N = 40;
    
    for (int i = 0; i <N ; i++) {
    
        std::cout << "\n=== 这是第" << i << "次全局优化 ===\n" << std::endl;
        // 对应点对束缚间隔为 5.0
        computeClosestPoints(pnts, 5.0);
        gp->global_icp2(pnts);
    
    }
    end = clock();
    double t = (end - begin) / 1000.0;
    std::cout << "\n\n 执行工夫是：" << t << std::endl;

    std::cout << "保留后果到本地" << std::endl;
    savePnts(pnts);
}

试验测试

实现上述筹备与代码编程后，将本人的数据（图 1 和图 2 所示点云），输出优化零碎中。

测试一

在此次测试中，computeNumbers(pnts, 2)第二个参数为 2，也就是构建了一个 二元超图 （所谓 二元超图，只单纯是本人的定义，齐全不与其余任何 g2o 程序或代码或教程相符合，也不具备实在数学意义，同样不适用于其余 g2o 学习过程，在这里所谓二元超图，只单纯示意图中每个节点有两个关系节点，同样，每个节点只有两个束缚关系），最终后果如下：

将图 4 图 5 别离与图 1 图 2 进行比照，肉眼可见成果晋升了很多。

执行过程中截图如下：

察看图 6 – 图 9，因为在 computeNumbers(pnts, 2); 传入的参数为 2，所以这里只计算了每块点云重叠率最高的两块，打印信息可是看出，id 为 0 的点云重叠率最高的是 id 为 1 和 id 为 2 的，id 为 3 的点云重叠率最高的是 id 为 2 和 id 为 4 的，也就是说，以后带点云重叠率最高的是其两片相邻的点云，这是合乎本人的理论状况的；持续看，随着迭代过程次数的减少，每块点云之间的重叠率是一直变动的，这也同样符合实际状况；最初，看误差参数估计 ch2 的输入变动 62w-->24w-->8w-->5w(全局大略 20w 左右的对应点对)，逐渐减小，也就是说对应点对之间的全局欧式间隔平方和在逐渐减小，同样符合实际状况。

该二元超图构造如下：

测试二

设置 computeNumbers(pnts, N); 参数 N 为 3 时，构建一张三元超图。

在三元超图下，最终成果和图 4 图 5 相似，同样认为实现了全局优化；期间，程序执行过程中信息输入如下：

根据上述打印信息可知，结构的三元图构造如下所示：

至此，根本实现了本人最后的目标，欣慰 ….

总结 & 题外话

本人目前也只是 g2o 新手村一般村民，且上述形容也并非真正意义上的 slam 问题，所述形容中只是本人针对所面临问题探究思路，不具备教学性、更不具权威性(有些夸张)，仅作集体记录与趣味交换。

上述形容的求解计划和思路，应该能够用来求解散乱点云多视角全局配准优化问题，而且也应该算是一个通用 (不仅仅限于相邻点云重叠) 的求解计划。

G2O 在 slam 后端中应用的比拟多，官网提供的 demo 大多也是 slam2d、slam3d 等，但又不仅限于此。因为针对本人所面临的问题，并没有将其向 slam 方向深刻扩大，所以也并没有从其余教程所述那样，从 slam2d 例程动手，而是抉择了本人较为相熟的 ICP 畛域。

不过话又说回来，本人所面的问题从整体解决思路上看，又能够作为是一个典型的 slam 优化问题：已知多个视角闭环的三维点云，通过求取路标点，求解全局相机位姿。

总之，我晓得：g2o 能够用来解决非线性优化问题。

啰里啰唆 ……

(顺便吐槽图片排版是真烂)