A general tool for Pose Graph.
Authors: Xinliang Zhong. ([email protected])
1.Description
This is an offline tool for Pose-Graph. You will get an trajectory file (usually with x,y,z) after you have run the SLAM and VO algorithm. If the datasets contains a loop, This tool can optimize the pose and eliminate the cumulative error.
Note: This repo contains some other test files. All the useful file is POSE_GRAPH_CERES_PLUS folder.
Videos: Youtube, Local Demo
Gif:
2. Prerequisites
We have tested the library in 16.04, but it should be easy to compile in other platforms.
OpenCV
We use OpenCV to manipulate images and features. Dowload and install instructions can be found at: http://opencv.org. Required at leat 2.4.3. Tested with OpenCV 2.4.11 and OpenCV 3.2.
Eigen3
Download and install instructions can be found at: http://eigen.tuxfamily.org. Required at least 3.1.0.
Ceres
Download and install instructions can be found at: http://www.ceres-solver.org/installation.html.
3. How to build
mkdir build
cd build
cmake ..
make4. How to run
All the executable files are in bin/ folder.
Then go the bin folder and run:
generate_edges_from_trajectory_origion.cpp
Useage: ./motionestimate currentFrame.id lastFrame.id (eg:)
input: config/config.yaml
output: config/Edge_Candidates_index.txt
Edge_Candidates_index.txt Format
currentFrame.id lastFrame.id loopFrameCandidatesIndex.id... (eg:) 4477 4476 24 25 26 27 28
motionestimate.cpp
Useage: ./motionestimate currentFrame.id lastFrame.id (eg:) KITTI sequence 00
./motionestimate 4477 28 you will see: 蓝色点为上一帧的keypoints 绿色点为当前帧的keypoints。 红色点为groundtruth投影到当前帧的点，红色连线是用groundtruth将上一帧2D点投影到当前帧与当前帧KeyPoints的连线，蓝色线为pnp或者ceres估计的位姿的连线。
Matches
Reprojection-ceres
Reprojection-pnp
motionestimate_wholesequence.cpp
对整个序列做两帧之间的motionestimate
pose_graph_ceres_plus.cpp
回环帧之间的T计算方式改变，首先pnp获得inliers，然后用本质矩阵E从inliers里边恢复R，然后固定R和3D点，只优化t.
pose_graph_ceres_plus_final.cpp
从config/Edge_Candidates_index.txt中读取Edge，但务必记得先生成该文件，生成该文件的方式见generate_edges_from_trajectory_origion.cpp
After you run the pose_graph_ceres*.cpp.
You can get result in the result/ folder. And you can run ./.sh or ./.py to see some results! Please see result/PlotShow/README
then you will get result in result/ folder.
5. Show the results
get into the result/PlotShow folder run:
./plot*.sh
then you will get:
and trajectory
