背景

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

图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：

图 3 g2o--gicp

可能早有大神预料到会是如上后果！！不得不说，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中图构造的特点，图边应该满足 [粗配后全局点云的特点] 的形容，伪图如下：


全局icp 伪图

如上图所示，假如：全局有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 部分细节

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

执行过程中截图如下：

图 6 图 7 图 8 图 9

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

该二元超图构造如下：

图10 二元图

测试二

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

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

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

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

总结&题外话

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

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

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

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

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

啰里啰唆......

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