【转】 slam 优化包g2o 学习

1. graph_slam 学习

      关于graph-slam的学习详细参照教材<> 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  

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

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