（一）SLAM拓扑地图（地图的生成和显示）

首先，SLAM中的拓扑地图是什么？
拓扑地图由节点和边组成。
如下图：

那么如何生成这种拓扑地图呢？
本文主要目的是生成一个简单的拓扑地图，并在g2o_viewer 中显示。

1.拓扑地图的生成

1.1安装g2o_viewer

参考我的上一篇博客
http://blog.csdn.net/ktigerhero3/article/details/75457432

1.2生成拓扑地图

本文参考g2o包中的create_sphere.cpp来生成拓扑地图的节点和边。
并参考下面博文中的示例
http://blog.csdn.net/heyijia0327/article/details/47686523#reply
生成的拓扑地图结构如下

具体实现如下：
（1）使用cmake加载g2o库函数
新建createmap工程
CmakeLists.txt如下
注意将g2o安装包中的cmake_modules文件夹拷贝到当前工程目录中。

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 found:"${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）使用代码生成节点和边（保存为*.g2o文件）

#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 << endl;
    }
    return 0;
}

编译运行，发现再当前文件夹下生成
ori.g2o文件
内容如下

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:QUAT 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 
EDGE_SE3:QUAT 3 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 

２.显示生成的拓扑地图

将以上生成的文件拷贝到
运行g2o安装文件夹下bin文件夹
cd到bin运行g2o_viewer

 ./g2o_viewer ori.g2o

如图

工程代码请到我的github上下载
https://github.com/QianFeifanhnu/topologicalMap/tree/master