转载自点击打开链接
关于graph-slam的学习详细参照教材<<Probabilistic Robotics>> 2005版 第11章
图优化实际上是解一种非线性最小二乘问题，主要用于离线的slam优化，也有用在在线的方式的。 最小二乘解决的就是偏差全局最小的问题，再在原基础量上
叠加最小偏差量即为最优量。 图优化将问题全部抽象成 node(点)与edge（边）的问题。。
点： 每个t时刻的机器人位置robot_pos 2d(x,y,theta),与t时刻观察到的landmark_pos(rou,theta)<极坐标的样式 > 作为点；
边： 不同时刻的robot_pos 之间距离作为边 （t 时刻量与t-1时刻运动预测量偏差），t 时刻 robot_pos 与 landmark_pos 间的距离作为边（我称为虚拟边，它在后面的矩阵稀疏化过程中会简化转成 跨时刻的 robot_pos 相应连接关系）
通过预测与观测的比较 ： 使得当前时刻 取得的机器人位置与landmark位置偏差全局最小。
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） 初始化起始点。运动预测量（运动模型）
2) 转化为信息矩阵的形式（具体还没弄清为什么转到 information？，IF有更稳定），建立整个关联矩阵。
3） 矩阵稀疏化。去landmark点，建立Xi与Xj间的联系。
4） 系统状态更新（机器人状态与landmark 状态）。
说明：详细说明参见书本介绍与相应的推导。
基于优化方法的机器人同步定位与地图创建(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)
论文： g2o: A General Framework for Graph Optimization;(2011)
开源代码 g2ohttp://www.openslam.org/g2o.html
g2o的框架结构如下图所示：
论文内容： 主要围绕最小二乘与非线性最小二乘的求解进行介绍 ，最后给出一些常用graph-slam的方法结果比较。。
进github ,下载代码， cd ;mkdir build ; cd build ; cmake .. ;make -j4; 正常通过。 cd bin ; ./tutorial_slam2d 测试执行;
sudo make install 安装相应的lib 与 头文件库 ，执行文件;
说明： 1）留意 cmake_modules 文件夹里。里面的××××.cmake后面会用到。 和cmakelists中的如下语句。
[html] view plain copy set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake-modules/") message(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})
2） g2o/examples/里面有相应的tutorial例子
g2o/examples/tutorial_slam2d 例子是用模拟生成的数据，利用g2o的OptimizationAlgorithmGaussNewton方法进行更新得到相应的优化。
1）初始化起点
[html] view plain copy // add the parameter representing the sensor offset ParameterSE2Offset* sensorOffset = new ParameterSE2Offset; sensorOffset->setOffset(sensorOffsetTransf); sensorOffset->setId(0); optimizer.addParameter(sensorOffset); 2） 添加机器人位置 [html] view plain copy 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） 添加里程边 [html] view plain copy // 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位置 [html] view plain copy // 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 边位置 [html] view plain copy 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）迭代优化 [html] view plain copy // 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");主要讲解移植过程中出现的问题。主要参照的是RGBSLAMV2 与 g2o 两个开源cmakelists的编写。。
我的工程结构如下： 以子文件夹的形式在ar中编译tutorial_slam2d
主要在两个点
1） 出现C++11不兼容问题的解决
参照：How to activate C++ 11 in CMake
[html] view plain copy 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文件夹复制到新工程
[html] view plain copy 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文件：
[html] view plain copy 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
g2o: A General Framework for Graph Optimization
说明：这些都是个人简单理解，方便大家入门。如有问题，欢迎留言指正，不甚感激。。
