[置顶] graph slam学习:g2o

1. graph_slam 学习

      关于graph-slam的学习详细参照教材<<Probabilistic Robotics>> 2005版     第11章  

     图优化实际上是解一种非线性最小二乘问题，主要用于离线的slam优化，也有用在在线的方式的。 最小二乘解决的就是偏差全局最小的问题，再在原基础量上

叠加最小偏差量即为最优量。 图优化将问题全部抽象成 node(点)与edge（边）的问题。。

1.1 组成：   图优化的图形简化见下图

     点：    每个t时刻的机器人位置robot_pos  2d(x,y,theta),与t时刻观察到的landmark_pos(rou,theta)<极坐标的样式 > 作为点；

     边：   不同时刻的robot_pos 之间距离作为边 （t 时刻量与t-1时刻运动预测量偏差），t 时刻 robot_pos 与 landmark_pos 间的距离作为边（我称为虚拟边，它在后面的矩阵稀疏化过程中会简化转成 跨时刻的  robot_pos 相应连接关系）

 1.2 目的：

                 通过预测与观测的比较 ： 使得当前时刻 取得的机器人位置与landmark位置偏差全局最小。

 1.3  说明：

         1）  需要初始的位置，迭代最优。

         2） 稀疏矩阵的应用： 如图所示，每个格子占据表示的是robot_pos 与landmark_pos的关联关系。m3在t=2.3.4的时刻都可以观察到。robot_pos本来就有t=2,3间的

关系，假如我把m3简化调，必须引进robot_pos在t=2，4见的联系（x2->x4），如下右图。

1.4  图优化的过程伪代码：       算法的总体框架如下：

     1） 初始化起始点。运动预测量（运动模型）

    2)     转化为信息矩阵的形式（具体还没弄清为什么转到 information？，IF有更稳定），建立整个关联矩阵。

    3） 矩阵稀疏化。去landmark点，建立Xi与Xj间的联系。

  4） 系统状态更新（机器人状态与landmark 状态）。

说明：详细说明参见书本介绍与相应的推导。

1.5 图优化相应的资源

基于优化方法的机器人同步定位与地图创建(SLAM)后端(Back-end)设计技术收集

RGBD-SLAM 

论文：

Hierarchical Optimization on Manifolds for Online 2D and 3D Mapping;(2010)

g2o: A General Framework for Graph Optimization;(2011)

Experimental Analysis of Dynamic Covariance Scaling for Robust Map Optimization Under Bad Initial Estimates;(2014)

2.  g2o简单turorial:

   论文： g2o: A General Framework for Graph Optimization;(2011)

   开源代码  g2ohttp://www.openslam.org/g2o.html

      g2o的框架结构如下图所示：

     论文内容： 主要围绕最小二乘与非线性最小二乘的求解进行介绍  ，最后给出一些常用graph-slam的方法结果比较。。

3.  g2o 移植使用说明

   3.1  g2o 库的编译与安装

    进github ,下载代码， cd   ;mkdir build ;   cd build ; cmake .. ;make -j4;  正常通过。 cd bin ;  ./tutorial_slam2d  测试执行; 

    sudo make install  安装相应的lib 与 头文件库 ，执行文件; 

说明：  1）留意 cmake_modules 文件夹里。里面的××××.cmake后面会用到。  和cmakelists中的如下语句。

set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake-modules/")
message(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})
  

             2）  g2o/examples/里面有相应的tutorial例子

  3.2  简单介绍tutorial_slam2d

             g2o/examples/tutorial_slam2d  例子是用模拟生成的数据，利用g2o的OptimizationAlgorithmGaussNewton方法进行更新得到相应的优化。

     1）初始化起点

  // add the parameter representing the sensor offset
  ParameterSE2Offset* sensorOffset = new ParameterSE2Offset;
  sensorOffset->setOffset(sensorOffsetTransf);
  sensorOffset->setId(0);
  optimizer.addParameter(sensorOffset);

     2） 添加机器人位置

  cerr << "Optimization: Adding robot poses ... ";
  for (size_t i = 0; i < simulator.poses().size(); ++i) {
    const Simulator::GridPose& p = simulator.poses()[i];
    const SE2& t = p.simulatorPose; 
    VertexSE2* robot =  new VertexSE2;
    robot->setId(p.id);
    robot->setEstimate(t);
    optimizer.addVertex(robot);
  }

    3） 添加里程边

  // second add the odometry constraints
  cerr << "Optimization: Adding odometry measurements ... ";
  for (size_t i = 0; i < simulator.odometry().size(); ++i) {
    const Simulator::GridEdge& simEdge = simulator.odometry()[i];

    EdgeSE2* odometry = new EdgeSE2;
    odometry->vertices()[0] = optimizer.vertex(simEdge.from);
    odometry->vertices()[1] = optimizer.vertex(simEdge.to);
    odometry->setMeasurement(simEdge.simulatorTransf);
    odometry->setInformation(simEdge.information);
    optimizer.addEdge(odometry);
  }

     4） 添加landmark位置

  // add the landmark observations
  cerr << "Optimization: add landmark vertices ... ";
  for (size_t i = 0; i < simulator.landmarks().size(); ++i) {
    const Simulator::Landmark& l = simulator.landmarks()[i];
    VertexPointXY* landmark = new VertexPointXY;
    landmark->setId(l.id);
    landmark->setEstimate(l.simulatedPose);
    optimizer.addVertex(landmark);
  }
  cerr << "done." << endl;

   5） 添加机器人与landmark 边位置

  cerr << "Optimization: add landmark observations ... ";
  for (size_t i = 0; i < simulator.landmarkObservations().size(); ++i) {
    const Simulator::LandmarkEdge& simEdge = simulator.landmarkObservations()[i];
    EdgeSE2PointXY* landmarkObservation =  new EdgeSE2PointXY;
    landmarkObservation->vertices()[0] = optimizer.vertex(simEdge.from);
    landmarkObservation->vertices()[1] = optimizer.vertex(simEdge.to);
    landmarkObservation->setMeasurement(simEdge.simulatorMeas);
    landmarkObservation->setInformation(simEdge.information);
    landmarkObservation->setParameterId(0, sensorOffset->id());
    optimizer.addEdge(landmarkObservation);
  }
  cerr << "done." << endl;

6）迭代优化

  // prepare and run the optimization
  // fix the first robot pose to account for gauge freedom
  VertexSE2* firstRobotPose = dynamic_cast<VertexSE2*>(optimizer.vertex(0));
  firstRobotPose->setFixed(true);
  optimizer.setVerbose(true);

  cerr << "Optimizing" << endl;
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  cerr << "done." << endl;

  optimizer.save("tutorial_after.g2o");

 3.3  利用g2o库进行相应的开发

    主要讲解移植过程中出现的问题。主要参照的是RGBSLAMV2 与 g2o 两个开源cmakelists的编写。。

   我的工程结构如下： 以子文件夹的形式在ar中编译tutorial_slam2d

主要在两个点

1） 出现c++11不兼容问题的解决

参照：How to activate C++ 11 in CMake 

       include(CheckCXXCompilerFlag)
      CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
      CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
      if(COMPILER_SUPPORTS_CXX11)
          set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
      elseif(COMPILER_SUPPORTS_CXX0X)
          set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
      else()
              message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
      endif()

      if(COMMAND cmake_policy)
        cmake_policy(SET CMP0003 NEW)
      endif(COMMAND cmake_policy)

２）findpackage找include与lib --->xx.cmake 文件

3）  将 g2o/cmake_modules文件夹复制到新工程

set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake-modules/")
message(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})
# g2o #######################
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIR})
message("G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR}")
link_directories(${G2O_LIBRARY_DIRS})
link_libraries(${G2O_LIBRARIES})
message("G2o-libraries ${G2O_LIBRARIES}")

<pre name="code" class="html">         # CSparse #######################
  find_package(CSparse REQUIRED)
  include_directories(${CSPARSE_INCLUDE_DIR})
  message("CSPARSE_INCLUDE_DIRS"${CSPARSE_INCLUDE_DIR})
  link_directories(${CSparse_LIBRARY_DIRS})
  link_libraries(${CSPARSE_LIBRARY})
  message("CSparse-libraries ${CSPARSE_LIBRARY}")

以下是我的cmakelists文件：

cmake_minimum_required(VERSION 2.8)
project(tutorial_slam2d)

       include(CheckCXXCompilerFlag)
      CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
      CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
      if(COMPILER_SUPPORTS_CXX11)
          set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
      elseif(COMPILER_SUPPORTS_CXX0X)
          set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
      else()
              message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
      endif()

      if(COMMAND cmake_policy)
        cmake_policy(SET CMP0003 NEW)
      endif(COMMAND cmake_policy)

set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake-modules/")
message(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})
# g2o #######################
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIR})
message("G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR}")
link_directories(${G2O_LIBRARY_DIRS})
link_libraries(${G2O_LIBRARIES})
message("G2o-libraries ${G2O_LIBRARIES}")

         # CSparse #######################
  find_package(CSparse REQUIRED)
  include_directories(${CSPARSE_INCLUDE_DIR})
  message("CSPARSE_INCLUDE_DIRS"${CSPARSE_INCLUDE_DIR})
  link_directories(${CSparse_LIBRARY_DIRS})
  link_libraries(${CSPARSE_LIBRARY})
  message("CSparse-libraries ${CSPARSE_LIBRARY}")


INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR})

file(GLOB_RECURSE HEADERS_AR
        ${PROJECT_SOURCE_DIR}/class/*.h
        ${PROJECT_SOURCE_DIR}/class/*.cpp
)
message("HEADERS_AR = ${HEADERS_AR}")
ADD_LIBRARY(tutorial_slam2d_library   SHARED  ${HEADERS_AR})

SET_TARGET_PROPERTIES(tutorial_slam2d_library PROPERTIES OUTPUT_NAME ${LIB_PREFIX}tutorial_slam2d)

TARGET_LINK_LIBRARIES(tutorial_slam2d_library  ${G2O_LIBRARIES} )

ADD_EXECUTABLE(tutorial_slam2d
  tutorial_slam2d.cpp
)

SET_TARGET_PROPERTIES(tutorial_slam2d PROPERTIES OUTPUT_NAME tutorial_slam2d${EXE_POSTFIX})

TARGET_LINK_LIBRARIES(tutorial_slam2d tutorial_slam2d_library)

4）相关资源 ：  g 2 o: A general Framework for (Hyper) Graph Optimization 

说明：这些都是个人简单理解，方便大家入门。如有问题，欢迎留言指正，不甚感激。。 

后面做做结果再补个人实验结构图。找到为什么用信息向量与信息矩阵的原因。。