graph slam學(xué)習(xí):g2o

謝興l4nztpvbdk 2017-07-18

展開(kāi)全文

1. graph_slam 學(xué)習(xí)

關(guān)于graph-slam的學(xué)習(xí)詳細(xì)參照教材<<Probabilistic Robotics>> 2005版第11章

圖優(yōu)化實(shí)際上是解一種非線(xiàn)性最小二乘問(wèn)題,，主要用于離線(xiàn)的slam優(yōu)化,，也有用在在線(xiàn)的方式的,。最小二乘解決的就是偏差全局最小的問(wèn)題,，再在原基礎(chǔ)量上

疊加最小偏差量即為最優(yōu)量,。圖優(yōu)化將問(wèn)題全部抽象成 node(點(diǎn))與edge（邊）的問(wèn)題。,。

1.1 組成：圖優(yōu)化的圖形簡(jiǎn)化見(jiàn)下圖

點(diǎn)：每個(gè)t時(shí)刻的機(jī)器人位置robot_pos 2d(x,y,theta),與t時(shí)刻觀(guān)察到的landmark_pos(rou,theta)<極坐標(biāo)的樣式 > 作為點(diǎn),；

邊：不同時(shí)刻的robot_pos 之間距離作為邊（t 時(shí)刻量與t-1時(shí)刻運(yùn)動(dòng)預(yù)測(cè)量偏差），t 時(shí)刻 robot_pos 與 landmark_pos 間的距離作為邊（我稱(chēng)為虛擬邊,，它在后面的矩陣稀疏化過(guò)程中會(huì)簡(jiǎn)化轉(zhuǎn)成跨時(shí)刻的 robot_pos 相應(yīng)連接關(guān)系）

1.2 目的：

通過(guò)預(yù)測(cè)與觀(guān)測(cè)的比較：使得當(dāng)前時(shí)刻取得的機(jī)器人位置與landmark位置偏差全局最小,。

1.3 說(shuō)明：

1）需要初始的位置，迭代最優(yōu),。

2）稀疏矩陣的應(yīng)用：如圖所示,，每個(gè)格子占據(jù)表示的是robot_pos 與landmark_pos的關(guān)聯(lián)關(guān)系。m3在t=2.3.4的時(shí)刻都可以觀(guān)察到,。robot_pos本來(lái)就有t=2,3間的

關(guān)系,，假如我把m3簡(jiǎn)化調(diào)，必須引進(jìn)robot_pos在t=2,，4見(jiàn)的聯(lián)系（x2->x4）,，如下右圖。

1.4 圖優(yōu)化的過(guò)程偽代碼：算法的總體框架如下：

1）初始化起始點(diǎn),。運(yùn)動(dòng)預(yù)測(cè)量（運(yùn)動(dòng)模型）

2) 轉(zhuǎn)化為信息矩陣的形式（具體還沒(méi)弄清為什么轉(zhuǎn)到 information,？，IF有更穩(wěn)定）,，建立整個(gè)關(guān)聯(lián)矩陣,。

3）矩陣稀疏化。去landmark點(diǎn),，建立Xi與Xj間的聯(lián)系,。

4）系統(tǒng)狀態(tài)更新（機(jī)器人狀態(tài)與landmark 狀態(tài)）,。

說(shuō)明：詳細(xì)說(shuō)明參見(jiàn)書(shū)本介紹與相應(yīng)的推導(dǎo)。

1.5 圖優(yōu)化相應(yīng)的資源

基于優(yōu)化方法的機(jī)器人同步定位與地圖創(chuàng)建(SLAM)后端(Back-end)設(shè)計(jì)技術(shù)收集

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簡(jiǎn)單turorial:

論文： g2o: A General Framework for Graph Optimization;(2011)

開(kāi)源代碼 g2ohttp://www./g2o.html

g2o的框架結(jié)構(gòu)如下圖所示：

論文內(nèi)容：主要圍繞最小二乘與非線(xiàn)性最小二乘的求解進(jìn)行介紹 ,，最后給出一些常用graph-slam的方法結(jié)果比較,。。

3. g2o 移植使用說(shuō)明

3.1 g2o 庫(kù)的編譯與安裝

進(jìn)github ,下載代碼,， cd ;mkdir build ; cd build ; cmake .. ;make -j4; 正常通過(guò),。 cd bin ; ./tutorial_slam2d 測(cè)試執(zhí)行;

sudo make install 安裝相應(yīng)的lib 與頭文件庫(kù) ，執(zhí)行文件;

說(shuō)明： 1）留意 cmake_modules 文件夾里,。里面的××××.cmake后面會(huì)用到,。和cmakelists中的如下語(yǔ)句。

[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/里面有相應(yīng)的tutorial例子

3.2 簡(jiǎn)單介紹tutorial_slam2d

g2o/examples/tutorial_slam2d 例子是用模擬生成的數(shù)據(jù),，利用g2o的OptimizationAlgorithmGaussNewton方法進(jìn)行更新得到相應(yīng)的優(yōu)化,。

1）初始化起點(diǎn)

[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）添加機(jī)器人位置

[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）添加機(jī)器人與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）迭代優(yōu)化

[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庫(kù)進(jìn)行相應(yīng)的開(kāi)發(fā)

主要講解移植過(guò)程中出現(xiàn)的問(wèn)題,。主要參照的是RGBSLAMV2 與 g2o 兩個(gè)開(kāi)源cmakelists的編寫(xiě),。。

我的工程結(jié)構(gòu)如下：以子文件夾的形式在ar中編譯tutorial_slam2d

主要在兩個(gè)點(diǎn)

1）出現(xiàn)C++11不兼容問(wèn)題的解決

參照：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文件夾復(fù)制到新工程

[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)

g2o: A General Framework for Graph Optimization

說(shuō)明：這些都是個(gè)人簡(jiǎn)單理解,，方便大家入門(mén),。如有問(wèn)題，歡迎留言指正,，不甚感激,。。

本站是提供個(gè)人知識(shí)管理的網(wǎng)絡(luò)存儲(chǔ)空間,，所有內(nèi)容均由用戶(hù)發(fā)布,，不代表本站觀(guān)點(diǎn)。請(qǐng)注意甄別內(nèi)容中的聯(lián)系方式,、誘導(dǎo)購(gòu)買(mǎi)等信息,，謹(jǐn)防詐騙。如發(fā)現(xiàn)有害或侵權(quán)內(nèi)容,，請(qǐng)點(diǎn)擊一鍵舉報(bào),。

轉(zhuǎn)藏 分享

QQ空間 QQ好友新浪微博微信

獻(xiàn)花（0） +1

來(lái)自：謝興l4nztpvbdk > 《待分類(lèi)》

舉報(bào)/認(rèn)領(lǐng)