(i) Slam topology map (map generation and display)

First, what is the topology map in Slam.
Topology maps are made up of nodes and edges.
The following figure:

So how do you generate this topology map?
The main purpose of this paper is to generate a simple topological map and display it in G2o_viewer. 1. Generation of topological maps 1.1 Installing G2o_viewer

Refer to my previous blog post
http://blog.csdn.net/ktigerhero3/article/details/75457432 1.2 Generating topology map

This article refers to the create_sphere.cpp in the G2O package to generate the nodes and edges of the topology map.
and refer to the examples in the following blog post
Http://blog.csdn.net/heyijia0327/article/details/47686523#reply
The resulting topology map structure is as follows

The specific implementation is as follows:
(1) Loading G2O library functions using CMake
New Createmap Project
CmakeLists.txt as follows
Note Copy the Cmake_modules folder from the G2O installation package to the current project directory.

Project (CREATEMAP) cmake_minimum_required (VERSION 2.8) LIST (APPEND Cmake_module_path ${project_source_dir}/cmake_ Modules) Message ("Cmake_module_path" ${cmake_module_path}) Find_package (Eigen3 REQUIRED) find_package (csparse REQUIRED) find_package (g2o REQUIRED) IF (g2o_found) include_directories (${g2o_include_dir}) message ("G2o Lib is fou Nd: "${g2o_include_dir}") ENDIF (g2o_found) message ("EIGEN3 Lib is FOUND:" ${eigen_include_dir}) IF (Eigen3_found) INCLUDE
_directories (${eigen3_include_dir}) message ("Eigen3_include_dir" ${eigen3_include_dir}}) ENDIF (Eigen3_found) Include_directories (${csparse_include_dir}) SET (g2o_libs g2o_cli g2o_ext_freeglut_minimal g2o_simulator g2o_solver_ Slam2d_linear G2O_TYPES_ICP g2o_types_slam2d g2o_core g2o_interface g2o_solver_csparse g2o_solver_structure_only g2o_ TYPES_SBA g2o_types_slam3d g2o_csparse_extension g2o_opengl_helper g2o_solver_dense g2o_stuff g2o_types_sclam2d g2o_ Parser G2O_SOLVER_PCG g2o_types_data g2o_types_sim3 cxsparse) AUX_SOURCE_directory (. src_list) add_executable (${project_name} ${src_list}) target_link_libraries (${project_name} ${G2O_LIBS})

(2) using code to generate nodes and edges (saved as *.g2o files)

#include <iostream> #include <fstream> #include <vector> #include <cmath> #include <eigen/ core> #include <Eigen/Dense> #include <Eigen/Geometry> #include <Eigen/StdVector> #include "g2o/ Types/slam3d/vertex_se3.h "#include" g2o/types/slam3d/edge_se3.h "#include" g2o/stuff/sampler.h "#include" G2o/stuff
/command_args.h "#include" g2o/core/factory.h "using namespace std;
using namespace G2o;
    int main () {vector<vertexse3*> vertices;
    Vector<edgese3*> edges;
    Eigen::matrix<double, 6, 6> information = eigen::matrix<double,6,6>::identity ();
    int id = 0;
    Add x0,x1,x2,x3//x0=0 EIGEN::ANGLEAXISD rotz0 (0, Eigen::vector3d::unitz ());
    Eigen::matrix3d rot0 = Rotz0.torotationmatrix ();
    Eigen::isometry3d t0;
    T0 = rot0;
    T0.translation () = Eigen::vector3d (0, 0, 0);
    vertexse3* v0 = new VertexSE3;
    V0->setid (id++);
    V0->setestimate (t0);
    Vertices.push_back (V0); X1=0
    EIGEN::ANGLEAXISD rotz1 (0, Eigen::vector3d::unitz ());
    Eigen::matrix3d rot1 = Rotz1.torotationmatrix ();
    Eigen::isometry3d T1;
    T1 = rot1;
    T1.translation () = Eigen::vector3d (0, 0, 0);
    vertexse3* v1 = new VertexSE3;
    V1->setid (id++);
    V0->setestimate (t1);
    Vertices.push_back (v1);
    X2=1 EIGEN::ANGLEAXISD rotz2 (0, Eigen::vector3d::unitz ());
    Eigen::matrix3d Rot2 = Rotz2.torotationmatrix ();
    Eigen::isometry3d T2;
    t2 = Rot2;
    T2.translation () = Eigen::vector3d (0, 0, 1);
    vertexse3* v2 = new VertexSE3;
    V2->setid (id++);
    V2->setestimate (T2);
    Vertices.push_back (v2);
    x3=0.2 EIGEN::ANGLEAXISD rotz3 (0, Eigen::vector3d::unitz ());
    Eigen::matrix3d rot3 = Rotz3.torotationmatrix ();
    Eigen::isometry3d T3;
    t3 = rot3;
    T3.translation () = Eigen::vector3d (0, 0, 0.2);
    vertexse3* v3 = new VertexSE3;
    V3->setid (id++);
    V3->setestimate (T3);
    Vertices.push_back (v3); E01=0 VertexSE3* prev01 = vertices[0];
    vertexse3* Cur01 = vertices[1];
    EIGEN::ANGLEAXISD R01 (0, Eigen::vector3d::unitz ());
    Eigen::matrix3d r01m = R01.torotationmatrix ();
    Eigen::isometry3d t01;
    t01 = r01m;
    T01.translation () = Eigen::vector3d (0, 0, 0);
    edgese3* e01 = new EdgeSE3;
    E01->setvertex (0, PREV01);
    E01->setvertex (1, Cur01);
    E01->setmeasurement (T01);
    E01->setinformation (information);

    Edges.push_back (E01);
    E12=1 vertexse3* prev12 = vertices[1];
    vertexse3* cur12 = vertices[2];
    EIGEN::ANGLEAXISD R12 (0, Eigen::vector3d::unitz ());
    Eigen::matrix3d r12m = R12.torotationmatrix ();
    Eigen::isometry3d T12;
    T12 = r12m;
    T12.translation () = Eigen::vector3d (0, 0, 1);
    edgese3* E12 = new EdgeSE3;
    E12->setvertex (0, PREV12);
    E12->setvertex (1, cur12);
    E12->setmeasurement (T12);
    E12->setinformation (information);

    Edges.push_back (E12);
  e23=-0.8 vertexse3* prev23= vertices[2];  vertexse3* cur23 = vertices[3];
    EIGEN::ANGLEAXISD r23 (0, Eigen::vector3d::unitz ());
    Eigen::matrix3d r23m = R23.torotationmatrix ();
    Eigen::isometry3d T23;
    T23 = r23m;
    T23.translation () = Eigen::vector3d (0, 0,-0.8);
    edgese3* E23 = new EdgeSE3;
    E23->setvertex (0, prev23);
    E23->setvertex (1, cur23);
    E23->setmeasurement (T23);
    E23->setinformation (information);

    Edges.push_back (E23);
    E31=0 vertexse3* prev31= vertices[3];
    vertexse3* cur31 = vertices[1];
    EIGEN::ANGLEAXISD r31 (0, Eigen::vector3d::unitz ());
    Eigen::matrix3d r31m = R31.torotationmatrix ();
    Eigen::isometry3d T31;
    T31 = r31m;
    T31.translation () = Eigen::vector3d (0, 0,0);
    edgese3* e31 = new EdgeSE3;
    E31->setvertex (0, prev31);
    E31->setvertex (1, cur31);
    E31->setmeasurement (T31);
    E31->setinformation (information);
     Edges.push_back (E31);
    Write output ofstream FileOutputStream; String Outfilename= "./ORi.g2o ";
    cout<<outfilename<<endl;
    Fileoutputstream.open (Outfilename.c_str ());
    Commandargs Arg;
    Arg.param ("O", Outfilename, "-", "Output filename");
    String vertextag = Factory::instance ()->tag (vertices[0]);

    String edgetag = Factory::instance ()->tag (edges[0]); ostream& Fout = outfilename! = "./out.g2o"?
    Fileoutputstream:cout;
    ostream& Fout=fileoutputstream;
    for (size_t i = 0; i < vertices.size (); ++i) {vertexse3* v = vertices[i];
    Fout << vertextag << "<< v->id () <<" ";
    V->write (Fout);
    Fout << Endl;
    } for (size_t i = 0; i < edges.size (); ++i) {edgese3* e = edges[i];
    vertexse3* from = static_cast<vertexse3*> (E->vertex (0));
    vertexse3* to = static_cast<vertexse3*> (E->vertex (1));
    Fout << edgetag << "<< from->id () <<" << to->id () << "";
    E->write (Fout); Fout &Lt;< Endl;
} return 0; }

Compile run, discover and then generate under current folder
ORI.G2O file
The contents are as follows

Vertex_se3:quat 0 0 0 0 0 0 0 1 
vertex_se3:quat 1 0 0 0 0 0 0 1 
vertex_se3:quat 2 0 0 1 0 0 0 1 
Vertex_se3:qua T 3 0 0 0.2 0 0 0 1 
edge_se3:quat 0 1 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1 
edge_se3:quat 1 2 0  0 1 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1 
edge_se3:quat 2 3 0 0-0.8 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1 

2. Display the generated topology map

Copy the above-generated files to
Run the G2O installation folder under the Bin folder
CD to Bin Run G2o_viewer

./g2o_viewer ORI.G2O

As shown in figure

Project code please download on my github
Https://github.com/QianFeifanhnu/topologicalMap/tree/master