g2o使用bug总结

g2o进行3d2d优化的时候，设置优化图的边时，注意setVertex()中顶点的顺序。

void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); _vertices[i]=v;}

        p_Edge_SE3XYZ->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));

        p_Edge_SE3XYZ->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(idx2)));

第0类参数应为3D点，第1类参数应为相机位姿，顺序不能错，否则会报错：

buildSystem(): NaN within Jacobian for edge 0x8bd4f0 for vertex 0。