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

[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例子

3.2 简单介绍tutorial_slam2d

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");

3.3 利用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)