分享

graph slam学习:g2o

 谢兴l4nztpvbdk 2017-07-18

 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./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中的如下语句。

  1. set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake-modules/")  
  2. message(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})  
  3.     


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

  3.2  简单介绍tutorial_slam2d

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

     1)初始化起点

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

     2) 添加机器人位置

  1. cerr << "Optimization: Adding robot poses ... ";  
  2. for (size_t i = 0; i < simulator.poses().size(); ++i) {  
  3.   const Simulator::GridPose& p = simulator.poses()[i];  
  4.   const SE2& t = p.simulatorPose;   
  5.   VertexSE2* robot =  new VertexSE2;  
  6.   robot->setId(p.id);  
  7.   robot->setEstimate(t);  
  8.   optimizer.addVertex(robot);  
  9. }  
    3) 添加里程边

  1. // second add the odometry constraints  
  2. cerr << "Optimization: Adding odometry measurements ... ";  
  3. for (size_t i = 0; i < simulator.odometry().size(); ++i) {  
  4.   const Simulator::GridEdge& simEdge = simulator.odometry()[i];  
  5.   
  6.   EdgeSE2* odometry = new EdgeSE2;  
  7.   odometry->vertices()[0] = optimizer.vertex(simEdge.from);  
  8.   odometry->vertices()[1] = optimizer.vertex(simEdge.to);  
  9.   odometry->setMeasurement(simEdge.simulatorTransf);  
  10.   odometry->setInformation(simEdge.information);  
  11.   optimizer.addEdge(odometry);  
  12. }  

     4) 添加landmark位置

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

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

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

6)迭代优化

  1. // prepare and run the optimization  
  2. // fix the first robot pose to account for gauge freedom  
  3. VertexSE2* firstRobotPose = dynamic_cast<VertexSE2*>(optimizer.vertex(0));  
  4. firstRobotPose->setFixed(true);  
  5. optimizer.setVerbose(true);  
  6.   
  7. cerr << "Optimizing" << endl;  
  8. optimizer.initializeOptimization();  
  9. optimizer.optimize(10);  
  10. cerr << "done." << endl;  
  11.   
  12. optimizer.save("tutorial_after.g2o");  

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

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

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



主要在两个点

1) 出现C++11不兼容问题的解决

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

  1.  include(CheckCXXCompilerFlag)  
  2. CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)  
  3. CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)  
  4. if(COMPILER_SUPPORTS_CXX11)  
  5.     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")  
  6. elseif(COMPILER_SUPPORTS_CXX0X)  
  7.     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")  
  8. else()  
  9.         message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")  
  10. endif()  
  11.   
  12. if(COMMAND cmake_policy)  
  13.   cmake_policy(SET CMP0003 NEW)  
  14. endif(COMMAND cmake_policy)  

2)findpackage找include与lib --->xx.cmake 文件

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

  1. set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake-modules/")  
  2. message(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})  
  3. # g2o #######################  
  4. find_package(G2O REQUIRED)  
  5. include_directories(${G2O_INCLUDE_DIR})  
  6. message("G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR}")  
  7. link_directories(${G2O_LIBRARY_DIRS})  
  8. link_libraries(${G2O_LIBRARIES})  
  9. message("G2o-libraries ${G2O_LIBRARIES}")  
  10.   
  11. <pre name="code" class="html">         # CSparse #######################  
  12.   find_package(CSparse REQUIRED)  
  13.   include_directories(${CSPARSE_INCLUDE_DIR})  
  14.   message("CSPARSE_INCLUDE_DIRS"${CSPARSE_INCLUDE_DIR})  
  15.   link_directories(${CSparse_LIBRARY_DIRS})  
  16.   link_libraries(${CSPARSE_LIBRARY})  
  17.   message("CSparse-libraries ${CSPARSE_LIBRARY}")  



以下是我的cmakelists文件:

  1. cmake_minimum_required(VERSION 2.8)  
  2. project(tutorial_slam2d)  
  3.   
  4.        include(CheckCXXCompilerFlag)  
  5.       CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)  
  6.       CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)  
  7.       if(COMPILER_SUPPORTS_CXX11)  
  8.           set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")  
  9.       elseif(COMPILER_SUPPORTS_CXX0X)  
  10.           set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")  
  11.       else()  
  12.               message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")  
  13.       endif()  
  14.   
  15.       if(COMMAND cmake_policy)  
  16.         cmake_policy(SET CMP0003 NEW)  
  17.       endif(COMMAND cmake_policy)  
  18.   
  19. set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake-modules/")  
  20. message(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})  
  21. # g2o #######################  
  22. find_package(G2O REQUIRED)  
  23. include_directories(${G2O_INCLUDE_DIR})  
  24. message("G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR}")  
  25. link_directories(${G2O_LIBRARY_DIRS})  
  26. link_libraries(${G2O_LIBRARIES})  
  27. message("G2o-libraries ${G2O_LIBRARIES}")  
  28.   
  29.          # CSparse #######################  
  30.   find_package(CSparse REQUIRED)  
  31.   include_directories(${CSPARSE_INCLUDE_DIR})  
  32.   message("CSPARSE_INCLUDE_DIRS"${CSPARSE_INCLUDE_DIR})  
  33.   link_directories(${CSparse_LIBRARY_DIRS})  
  34.   link_libraries(${CSPARSE_LIBRARY})  
  35.   message("CSparse-libraries ${CSPARSE_LIBRARY}")  
  36.   
  37.   
  38. INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR})  
  39.   
  40. file(GLOB_RECURSE HEADERS_AR  
  41.         ${PROJECT_SOURCE_DIR}/class/*.h  
  42.         ${PROJECT_SOURCE_DIR}/class/*.cpp  
  43. )  
  44. message("HEADERS_AR = ${HEADERS_AR}")  
  45. ADD_LIBRARY(tutorial_slam2d_library   SHARED  ${HEADERS_AR})  
  46.   
  47. SET_TARGET_PROPERTIES(tutorial_slam2d_library PROPERTIES OUTPUT_NAME ${LIB_PREFIX}tutorial_slam2d)  
  48.   
  49. TARGET_LINK_LIBRARIES(tutorial_slam2d_library  ${G2O_LIBRARIES} )  
  50.   
  51. ADD_EXECUTABLE(tutorial_slam2d  
  52.   tutorial_slam2d.cpp  
  53. )  
  54.   
  55. SET_TARGET_PROPERTIES(tutorial_slam2d PROPERTIES OUTPUT_NAME tutorial_slam2d${EXE_POSTFIX})  
  56.   
  57. TARGET_LINK_LIBRARIES(tutorial_slam2d tutorial_slam2d_library)  

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

g2o: A General Framework for Graph Optimization

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

 


    本站是提供个人知识管理的网络存储空间,所有内容均由用户发布,不代表本站观点。请注意甄别内容中的联系方式、诱导购买等信息,谨防诈骗。如发现有害或侵权内容,请点击一键举报。
    转藏 分享 献花(0

    0条评论

    发表

    请遵守用户 评论公约

    类似文章 更多