G2O and multi-view point cloud global registration optimization

background

I have a bunch of three-dimensional scattered point clouds after initial registration. After rough matching, all the point clouds are basically unified in the same coordinate system, as shown in Figure 1 and Figure 2. In order to obtain better global results, it is necessary to perform global registration optimization on the point cloud after rough matching.

Figure 1 Point cloud bottom surface

Figure 2 Partial details

The above-mentioned point cloud is displayed based on pcl, and different colors represent different point cloud blocks. It can be clearly seen from Figure 1 that the global point cloud "fusion" (in fact, it is not fused, it is just loaded and displayed), and the effect is poor. It can be seen from Figure 2 that there is a large dislocation for the [ear] part.

Because I G2O before, but I haven't done much in-depth research. Knowing that G2O can be used to solve the global optimization problem , which happens to be very similar to the problem I asked to solve.

Therefore, he resolutely chose to use G2O to build solutions to his own problems.

Further understanding of G2O

G2O is a combination of nonlinear optimization and graph theory . It is mostly used to solve slam back-end optimization problems, and most of its own examples are also related to slam. The so-called graph is a pile of nodes and edges according to a certain relationship. Among them, the node is the goal to be optimized, and the edge is the relationship between each optimization goal (also called its error term, I prefer to call it [relationship] here).

Based on CMake + VS2017 to complete the installation of the G2O library, the installation process is not recorded in detail, and basically Baidu can solve it.

After installing g2o, follow the habit to look through the example carried in its own source code in order to find inspiration. I found it in the example directory at a glance [ gicp_demo ].

G2O--gicp_demo

The basic method of using g2o is:

1. Declare an optimizer;
2. Choose a solution method;
3. Construct a graph-the relationship between vertices and edges;
4. Optimized processing.

first time, they thought that "the gicp method is global", but this is not the case. In fact, it cannot even be called a complete icp problem

(Regarding the indication in the red font, it is only a personal understanding at present, there may be errors, please leave a message to correct me, and exchange and study together)

We know that ICP solution is an iterative calculation process, and the main steps of classic ICP solution are:

1. Input two point clouds AB to solve the corresponding point pairs (at least 3 non-collinear points in the three-dimensional model);
2. Based on the corresponding point pairs, construct the transformation matrix M from A to B;
3. Act M on A to get A*, and replace A with A*;
4. Termination condition of iteration (number of iterations or minimum error metric);
5. Repeat steps 1--3 until step 4 is met, and terminate.
6. Output transformation matrix M.

But looking closely at g2o's gicp_demo, its process is not the same as the classic ICP solution process, but more like the solution of step 2 in ICP.

Let's take a closer look at the gicp_demo given in g2o.

Disassemble gicp_demo

First of all, let's post the official g2o example directly (although it is very annoying to post other people's code directly) for easy explanation.

In this Demo, g2o first declared and set the optimizer optimizer , and made a set of 1000 points true_points as the source point cloud.

Second, add two nodes to the graph and set the node ID. The node type here is VertexSE3 ( class G2O_TYPES_SLAM3D_API VertexSE3 : public BaseVertex<6, Isometry3> ), which is also the main optimization goal. According to the order in which the nodes are added to the graph, the node added for the first time is regarded as a fixed perspective; vc->setEstimate(cam); This code segment tells us that the real solution result is stored in Eigen::Isometry3d (essentially a matrix), here The type of the cam Eigen::Isometry3d ; this step actually only declares two empty nodes, and the node parameter is just the unit Eigen::Isometry3d .

Again, 1000 edges are added to the graph. In this process, first obtain the two vertices (nodes) that the edge needs to link according to the node id, vp0 and vp1 , and "make" two three-dimensional points that contain noise true_points pt0 and pt1 (this step has actually defaulted to pt0 And pt1 are corresponding point pairs); then Edge_V_V_GICP declared, which is a binary edge of class G2O_TYPES_ICP_API Edge_V_V_GICP : public BaseBinaryEdge<3, EdgeGICP, VertexSE3, VertexSE3> (061a7167b54d1b ), which links vp0 and vp1 respectively; 1e7 also provides observations EdgeGICP Value, EdgeGICP type can store the corresponding point pair. In this step, must pay great attention to the belonging of the node and the three-dimensional coordinate point-the corresponding relationship . At this point, you can basically put all the information into the g2o graph. This step is mainly concerned with how to put your own three-dimensional point pairs into the g2o graph.

Finally, initialize the graph relationship and perform N iterations of optimization. The optimization result of each node is stored in optimizer.vertices() . In the hash table: key--corresponding to node id, value--corresponding to node, Here is the type VertexSE3 VertexSE3 obtained from Eigen::Isometry3d is the result data we really care about.
This example should construct the following hypergraph, where the graph has two graph nodes, n1 is a fixed node, n2 is a variable node, there are 1000 edges between the nodes, and each edge links a pair of corresponding points. For the ICP problem, the corresponding The fixed point in the point is connected to the graph node n1, and the variable point is connected to the graph node n2:

node--edge

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;
}

test

Execute the above example that comes with g2o, and finally print out the transformation matrix. Then extract the example separately, change into your own data (corresponding point pairs), and also output the transformation matrix, and then apply the transformation matrix to the point cloud. The result is shown in Figure 3:

Figure 3 g2o--gicp

Maybe some great gods had expected it to be the result above! ! I have to say that the optimization results of g2o are also far from satisfactory...

Is that true? ......

summary

If you follow the above process and follow the official example of g2o, the result is really like that! ! ! But compared with pcl's icp, the result is really bad, what's the problem?

in the scarlet part above, here I still use scarlet letters to remind myself --- g2o's gicp is not a complete ICP solution .

By analyzing the official code example, it can be found that before the solution, the input point pair is essentially corresponding to the default. On this basis, the iterative calculation is based on the same set of corresponding point pairs, and the iterative calculation between the corresponding points of the set For the entire two point clouds, this is actually only one icp calculation, and the result is of course not ideal.

In other words, the Demo missed (or g2o was designed in this way, or you don’t know enough about it) the calculation process of the corresponding point pairs in the ICP iteration scheme, that is, the lack of step 1 , g2o--gicp is just a simple calculation Step 2 , only a single optimal transformation matrix is obtained. For the icp solution problem of the overall two point clouds, when the second solution is performed, the corresponding relationship between the corresponding point pairs has changed, so the matrix obtained by g2o---gicp_demo is only a single optimal matrix, so the result is also As shown in Figure 3.

So how to get the overall optimal result? Of course, the above steps are put into a big loop, every time g2o--gicp is calculated, the point cloud is transformed, the correspondence relationship is solved again, and the calculation is iterative in turn. The result figure is omitted...

Build an optimized graph

After fully understanding the g2o's built-in icp example, I return to the problem I asked to solve at the beginning. There are N pieces of scattered point clouds after rough matching, and I want to perform global optimization registration global point cloud

The so-called global point cloud after rough configuration has the following characteristics:

1. There is a high overlap rate between two adjacent point clouds;
2. There is or no overlap between the separated point clouds (at least one point cloud);
3. Each point cloud can have one, zero (this condition can exist, and it does not conflict with condition 1 when constructing the g2o map) or multiple point clouds with high overlap rate;
4. The overlap relationship between the point clouds is corresponding (ie: A overlaps with B, C, and D, then there must be A in the overlapped point cloud of the BCD).

optimize the target

The optimization goals mentioned above are only our perceptual knowledge, but the optimization goals must also be transformed into mathematical expressions. The dictation is as follows: Optimization goal = Solve--N pieces of three-dimensional scattered point clouds, with point cloud A as the target point cloud, calculate the transformation matrix of all three-dimensional scattered point clouds registered to A, and the transformation matrix makes all corresponding point pairs European The distance is the smallest, or the specified number of iterations is reached.

The above optimization goal implies: if A and C do not overlap, then C cannot directly register and align with A, but A and B, B and C overlap, then the transformation from C to A needs to be transformed to B first. The matrix is changed to A again, and it is necessary to ensure that A and B, B and C are all optimal transformations. In other words, B is optimally transformed to A, and C is optimally transformed to B, then the most optimal transformation between ABC and ABC is completed.优 Transformation.

After confirming the mathematical expression of the optimization goal, it is also necessary to confirm the expression of the overlap ratio between the point clouds, dictated as follows : overlap ratio = the ratio of the number of corresponding point pairs of the two point clouds to the average point number of the two point clouds.

After completing the above two mathematical definitions, the overall flow of the program should be as follows:

1. Calculate the corresponding point pairs and overlap ratio between the global point clouds;
2. Overlapping point cloud screening (point clouds with a low overlap rate are considered to be non-overlapping and do not participate in the construction of edges in the g2o graph);

3. g2o global composition:

   • Graph node
   • side (emphasis)
   • Optimizer and optimization algorithm
4. Based on the output matrix of step 3, update the coordinates of all point clouds;
5. Repeat steps 1-4 until the termination conditions are met.

The final output is the optimized global point cloud and the corresponding transformation matrix.

Program realization

The PCL point cloud library and g2o (nonsense) are used in the project, and pcl is mainly used to calculate the point cloud overlap ratio.

From the above [g2o--gicp_demo], we can see that g2o has provided the defined graph node class VertexSE3 and graph binary edge class Edge_V_V_GICP and edge class EdgeGICP for the ICP scheme, which are used directly here, eliminating the need for custom graph edges , Graph nodes are troublesome (Of course, if you learn g2o in depth, it is still recommended to do more exploration).

Point cloud data structure

Through the foregoing analysis, it can be known that a certain point cloud structure should contain the following necessary content:

1. Point set
2. Is it fixed;
3. Transformation matrix
4. Adjacency information
5. Necessary methods (nearest neighbor point cloud, corresponding point pair calculation).

The structure is as follows:

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:
};

Among them, the calculation of the overlap relationship between the current point cloud and other global point clouds (except the current point cloud) and the corresponding point pairs are all computeKNNearPnts() function 061a7167b55292.

Map

This is the most important and extremely error-prone place.

The [coarsely with a global point cloud characteristics], determines the characteristics of the structure of FIG G2 o, FIG side should satisfy [coarse global characteristics of the point cloud] description, pseudo FIG follows :

Global icp pseudo image

As shown in the figure above, suppose: there are 5 point clouds globally, then the g2o graph has 5 nodes, n1 is a fixed node, n1 and n2 overlap, n2 and n5 overlap, n1 and n5 also overlap, but n1 and n3, There is no overlap between n1 and n4, n2 and n4.

Note: The overlap relationship here is a constraint relationship from g2o's point of view. Further, it is the link relationship between the edges between two point clouds. This edge may consist of hundreds or thousands of corresponding point pairs. The edges with arrows in the figure above are only indicative, not real graph edges.

After clarifying the structure of the diagram, the structure diagram code is as follows:

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];
        }
    }
}

In the above code, not only the graph structure is constructed and the relationship between each node is set, but the coordinates of the points are also updated, which facilitates the calculation of corresponding point pairs in the next iteration.

At this point, the framework based on g2o to solve the problem mentioned at the beginning is basically completed, and the final main program code is as follows:

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);
}

Experimental test

After completing the above preparation and code programming, enter your own data (point cloud shown in Figure 1 and Figure 2) into the optimization system.

Test one

In this test, computeNumbers(pnts, 2) second parameter is 2, that is, to build a two yuan hypergraph (so-called two yuan hypergraph , but simply own definition, completely with any other The g2o program or code or tutorial is consistent, and does not have real mathematical meaning. It is also not applicable to other g2o learning processes. The so-called binary hypergraph here simply means that each node in the graph has two relational nodes. Similarly, each There are only two constraint relationships for each node), and the final result is as follows:

Figure 4 Point cloud bottom surface

Figure 5 Partial details

Comparing Fig. 4 and Fig. 5 with Fig. 1 and Fig. 2 respectively, the visual effect has been improved a lot.

The screenshots during execution are as follows:

Figure 6

Figure 7

Figure 8

Figure 9

Observe Figure computeNumbers(pnts, 2); , because the parameter passed in 061a7167b555a7 is 2, so here only the two points with the highest overlap rate of each point cloud are calculated, and the printed information can be seen that the point cloud with the id of 0 has the highest overlap rate It is the point cloud with id 1 and id 2. The point cloud with id 3 has the highest overlap rate of id 2 and id 4, that is to say, the current point cloud with the highest overlap rate is its two adjacent points Cloud, this is in line with your actual situation; continue to see, as the number of iterations increases, the overlap rate between each point cloud is constantly changing, which is also in line with the actual situation; finally, look at the error parameter estimation ch2 The output change of 62w-->24w-->8w-->5w (globally about 20w corresponding point pairs) gradually decreases, that is to say, the global sum of squared Euclidean distances between corresponding point pairs is gradually decreasing, which is also in line with the actual situation.

The structure of the binary hypergraph is as follows:

Figure 10 Binary graph

Test two

computeNumbers(pnts, N); parameter N of 061a7167b5561c is set to 3, a ternary hypergraph is constructed.

Under the ternary hypergraph, the final effect is similar to that shown in Figure 4 and Figure 5. It is also considered that the global optimization has been completed; during this period, the information output during program execution is as follows:

According to the above printing information, the structure of the constructed ternary graph is as follows:

So far, I have basically completed my original purpose, I am delighted...

Summary & digressions

I am currently just an ordinary villager in g2o novice village, and the above description is not really a slam problem. The description is only to explore ideas for the problems I face, not pedagogical, and not authoritative (some exaggerated). Make personal records and exchanges of interest.

The solution and ideas described above should be able to solve the multi-view global registration optimization problem of scattered point clouds, and it should also be regarded as a general solution (not limited to overlapping of adjacent point clouds).

G2O is used more in the slam backend. Most of the official demos are slam2d, slam3d, etc., but they are not limited to this. Because I did not extend it in the slam direction for the problems I faced, I did not start with the slam2d routines as described in other tutorials, but chose the ICP field I am more familiar with.

But then again, from the perspective of the overall solution, the problem you are facing can be regarded as a typical slam optimization problem: a three-dimensional point cloud with a closed loop of multiple viewing angles is known, and the global camera pose can be solved by finding landmarks. .

In short, I know: g2o can be used to solve the nonlinear optimization problem .

Lai Lai...

(By the way, the picture layout is really bad)