【转】 slam 优化包g2o 学习

转载 2015年10月07日 10:07:20

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

[html] view plaincopy
  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)初始化起点

[html] view plaincopy
  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) 添加机器人位置

[html] view plaincopy
  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) 添加里程边

[html] view plaincopy
  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位置

[html] view plaincopy
  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 边位置

[html] view plaincopy
  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)迭代优化

[html] view plaincopy
  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 

[html] view plaincopy
  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文件夹复制到新工程

[html] view plaincopy
  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文件:

[html] view plaincopy
  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  

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

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


相关文章推荐

视觉SLAM漫谈(二):图优化理论与g2o的使用

1    前言以及回顾     各位朋友,自从上一篇《视觉SLAM漫谈》写成以来已经有一段时间了。我收到几位热心读者的邮件。有的希望我介绍一下当前视觉SLAM程序的实用程度,更多的人希望了解一下前...
  • BBZZ2
  • BBZZ2
  • 2016-03-01 13:42
  • 3157

SLAM代码(优化及常用库)

在SLAM的后端优化中,比较常用的一种方法是g2o,该图优化方法在优化时现成可以选用的有2种优化方法。一是Gauss-Newton,另外是LM方法。这里首先介绍Gauss-Newton方法,其次介绍g...
  • wendox
  • wendox
  • 2016-09-11 23:21
  • 1349

SLAM学习--非线性优化

slam中经常遇到非线性优化问题:许多个误差项平方和组成的最小二乘问题,两个最常见的梯度下降方法-非线性优化方案:高斯牛顿法、裂纹伯格-马夸尔特方法 两个c++的优化库:来自谷歌的ceres库和基于...

图优化理论与g2o的使用(2)

转载自 半闲居士  在这里,感谢博主的无私分享。 前言   本节我们将深入介绍视觉slam中的主流优化方法——图优化(graph-based optimization)。下一节中,介绍一下非常...

orbslam2-基础理论(六)-g2o(常用的优化算法)

2. 几个数学概念 1) 梯度(一阶导数) 考虑一座在 (x1, x2) 点高度是 f(x1, x2) 的山。那么,某一点的梯度方向是在该点坡度最陡的方向,而梯度的大小告诉我们坡度到底有多陡。...

图优化理论与g2o的使用(3)

转载自 半闲居士  在这里,感谢博主的无私分享。 内容提要   讲完了优化的基本知识,我们来看一下g2o的结构。本篇将讨论g2o的代码结构,并带着大家一起写一个简单的双视图bund...

深入理解图优化与g2o:g2o篇

内容提要   讲完了优化的基本知识,我们来看一下g2o的结构。本篇将讨论g2o的代码结构,并带着大家一起写一个简单的双视图bundle adjustment:从两张图像中估计相机运动和特征点位置...

g2o:一种图优化的C++框架

转载自:http://blog.csdn.net/u012700322/article/details/52857244 声明:转载自 Taylor Guo ,仅供个人学习 g2o: A ...

ORB-SLAM2:基于可识别特征的自主导航与地图构建

目录(?)[+] ORB-SLAM2 基于可识别特征的自主导航与地图构建 简介 地图 A 地图特征点或3D ORB B 关键帧 C 可视化图像 位置识别 A 图像识别数据库  B...
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:深度学习:神经网络中的前向传播和反向传播算法推导
举报原因:
原因补充:

(最多只允许输入30个字)