SLAM第九讲实践：【后端1】 基于slambook2 的ceres的BA优化以及g2o的BA优化
2022 10.17 bionic
一切必要的库文件例如 ceres fmt 等等安装完成之后再继续往下看
具体步骤
-
- 1 安装MeshLab
- 2 修改CMakeLists.txt文件
- 3 实现结果（ceres）
- 4 使用meshlab
- 5 基于g2o的BA优化
1 安装MeshLab
sudo apt install meshlab
2 修改CMakeLists.txt文件
cmake_minimum_required(VERSION 2.8)
project(bundle_adjustment)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-O3 -std=c++14")
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(fmt REQUIRED)
Find_Package(G2O REQUIRED)
Find_Package(Eigen3 REQUIRED)
Find_Package(Ceres REQUIRED)
Find_Package(Sophus REQUIRED)
Find_Package(CSparse REQUIRED)
set(FMT_LIBRARIES fmt::fmt)
SET(G2O_LIBS g2o_csparse_extension g2o_stuff g2o_core cxsparse)
include_directories(${PROJECT_SOURCE_DIR} ${EIGEN3_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR})
add_library(bal_common common.cpp)
add_executable(bundle_adjustment_g2o bundle_adjustment_g2o.cpp)
add_executable(bundle_adjustment_ceres bundle_adjustment_ceres.cpp)
target_link_libraries(bundle_adjustment_ceres ${CERES_LIBRARIES} bal_common)
target_link_libraries(bundle_adjustment_g2o ${G2O_LIBS} bal_common)
target_link_libraries(bundle_adjustment_g2o ${G2O_LIBS} ${FMT_LIBRARIES} bal_common)
3 实现结果（ceres）
./bundle_adjustment_ceres ../problem-16-22106-pre.txt
$ ./bundle_adjustment_ceres ../problem-16-22106-pre.txt
Header: 16 22106 83718bal problem file loaded...
bal problem have 16 cameras and 22106 points.
Forming 83718 observations.
Solving ceres BA ...
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.842900e+07 0.00e+00 2.04e+06 0.00e+00 0.00e+00 1.00e+04 0 6.58e-02 2.36e-01
1 1.449093e+06 1.70e+07 1.75e+06 2.16e+03 1.84e+00 3.00e+04 1 1.68e-01 4.04e-01
2 5.848543e+04 1.39e+06 1.30e+06 1.55e+03 1.87e+00 9.00e+04 1 1.27e-01 5.31e-01
3 1.581483e+04 4.27e+04 4.98e+05 4.98e+02 1.29e+00 2.70e+05 1 1.23e-01 6.54e-01
4 1.251823e+04 3.30e+03 4.64e+04 9.96e+01 1.11e+00 8.10e+05 1 1.21e-01 7.75e-01
5 1.240936e+04 1.09e+02 9.78e+03 1.33e+01 1.42e+00 2.43e+06 1 1.24e-01 8.99e-01
6 1.237699e+04 3.24e+01 3.91e+03 5.04e+00 1.70e+00 7.29e+06 1 1.22e-01 1.02e+00
7 1.236187e+04 1.51e+01 1.96e+03 3.40e+00 1.75e+00 2.19e+07 1 1.20e-01 1.14e+00
8 1.235405e+04 7.82e+00 1.03e+03 2.40e+00 1.76e+00 6.56e+07 1 1.22e-01 1.26e+00
9 1.234934e+04 4.71e+00 5.04e+02 1.67e+00 1.87e+00 1.97e+08 1 1.22e-01 1.39e+00
10 1.234610e+04 3.24e+00 4.31e+02 1.15e+00 1.88e+00 5.90e+08 1 1.22e-01 1.51e+00
11 1.234386e+04 2.24e+00 3.27e+02 8.44e-01 1.90e+00 1.77e+09 1 1.23e-01 1.63e+00
12 1.234232e+04 1.54e+00 3.44e+02 6.69e-01 1.82e+00 5.31e+09 1 1.21e-01 1.75e+00
13 1.234126e+04 1.07e+00 2.21e+02 5.45e-01 1.91e+00 1.59e+10 1 1.21e-01 1.87e+00
14 1.234047e+04 7.90e-01 1.12e+02 4.84e-01 1.87e+00 4.78e+10 1 1.22e-01 2.00e+00
15 1.233986e+04 6.07e-01 1.02e+02 4.22e-01 1.95e+00 1.43e+11 1 1.21e-01 2.12e+00
16 1.233934e+04 5.22e-01 1.03e+02 3.82e-01 1.97e+00 4.30e+11 1 1.22e-01 2.24e+00
17 1.233891e+04 4.25e-01 1.07e+02 3.46e-01 1.93e+00 1.29e+12 1 1.23e-01 2.36e+00
18 1.233855e+04 3.59e-01 1.04e+02 3.15e-01 1.96e+00 3.87e+12 1 1.23e-01 2.48e+00
19 1.233825e+04 3.06e-01 9.27e+01 2.88e-01 1.98e+00 1.16e+13 1 1.23e-01 2.61e+00
20 1.233799e+04 2.61e-01 1.17e+02 2.16e-01 1.97e+00 3.49e+13 1 1.22e-01 2.73e+00
21 1.233777e+04 2.18e-01 1.22e+02 1.15e-01 1.97e+00 1.05e+14 1 1.27e-01 2.86e+00
22 1.233760e+04 1.73e-01 1.10e+02 9.67e-02 1.89e+00 3.14e+14 1 1.24e-01 2.98e+00
23 1.233746e+04 1.37e-01 1.14e+02 1.11e-01 1.98e+00 9.41e+14 1 1.21e-01 3.10e+00
24 1.233735e+04 1.13e-01 1.17e+02 1.96e-01 1.96e+00 2.82e+15 1 1.20e-01 3.22e+00
WARNING: Logging before InitGoogleLogging() is written to STDERR
W1224 16:08:32.052875 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
25 1.233735e+04 0.00e+00 1.17e+02 0.00e+00 0.00e+00 1.41e+15 1 4.76e-02 3.27e+00
26 1.233725e+04 9.50e-02 1.20e+02 3.98e-01 1.99e+00 4.24e+15 1 1.19e-01 3.39e+00
W1224 16:08:32.218746 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
27 1.233725e+04 0.00e+00 1.20e+02 0.00e+00 0.00e+00 2.12e+15 1 4.67e-02 3.43e+00
W1224 16:08:32.262396 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
28 1.233725e+04 0.00e+00 1.20e+02 0.00e+00 0.00e+00 5.30e+14 1 4.36e-02 3.48e+00
29 1.233718e+04 6.92e-02 5.76e+01 1.59e-01 1.70e+00 1.59e+15 1 1.18e-01 3.60e+00
30 1.233714e+04 3.65e-02 5.80e+01 6.25e-01 1.93e+00 4.77e+15 1 1.23e-01 3.72e+00
W1224 16:08:32.550280 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
31 1.233714e+04 0.00e+00 5.80e+01 0.00e+00 0.00e+00 2.38e+15 1 4.75e-02 3.77e+00
W1224 16:08:32.593500 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
32 1.233714e+04 0.00e+00 5.80e+01 0.00e+00 0.00e+00 5.96e+14 1 4.32e-02 3.81e+00
33 1.233711e+04 3.32e-02 5.99e+01 2.01e-01 2.00e+00 1.79e+15 1 1.22e-01 3.93e+00
34 1.233708e+04 3.14e-02 6.02e+01 3.64e-01 2.00e+00 5.36e+15 1 1.24e-01 4.06e+00
W1224 16:08:32.885481 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
35 1.233708e+04 0.00e+00 6.02e+01 0.00e+00 0.00e+00 2.68e+15 1 4.57e-02 4.10e+00
W1224 16:08:32.928736 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
36 1.233708e+04 0.00e+00 6.02e+01 0.00e+00 0.00e+00 6.70e+14 1 4.32e-02 4.14e+00
37 1.233705e+04 2.50e-02 2.04e+01 1.37e-01 1.68e+00 2.01e+15 1 1.20e-01 4.26e+00
38 1.233704e+04 1.58e-02 2.01e+01 4.77e-01 1.95e+00 6.03e+15 1 1.23e-01 4.39e+00
W1224 16:08:33.218616 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
39 1.233704e+04 0.00e+00 2.01e+01 0.00e+00 0.00e+00 3.02e+15 1 4.69e-02 4.43e+00
W1224 16:08:33.262526 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
40 1.233704e+04 0.00e+00 2.01e+01 0.00e+00 0.00e+00 7.54e+14 1 4.39e-02 4.48e+00
41 1.233702e+04 1.51e-02 2.07e+01 3.02e-01 2.00e+00 2.26e+15 1 1.21e-01 4.60e+00
42 1.233701e+04 1.48e-02 2.00e+01 1.14e+00 1.99e+00 6.79e+15 1 1.26e-01 4.73e+00
W1224 16:08:33.556674 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
43 1.233701e+04 0.00e+00 2.00e+01 0.00e+00 0.00e+00 3.39e+15 1 4.72e-02 4.77e+00
W1224 16:08:33.600188 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
44 1.233701e+04 0.00e+00 2.00e+01 0.00e+00 0.00e+00 8.48e+14 1 4.35e-02 4.82e+00
45 1.233700e+04 1.42e-02 2.09e+01 2.20e-01 1.99e+00 2.54e+15 1 1.23e-01 4.94e+00
46 1.233698e+04 1.39e-02 1.88e+01 1.65e+00 2.00e+00 7.63e+15 1 1.24e-01 5.06e+00
W1224 16:08:33.894613 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
47 1.233698e+04 0.00e+00 1.88e+01 0.00e+00 0.00e+00 3.82e+15 1 4.74e-02 5.11e+00
W1224 16:08:33.938587 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
48 1.233698e+04 0.00e+00 1.88e+01 0.00e+00 0.00e+00 9.54e+14 1 4.40e-02 5.15e+00
49 1.233697e+04 1.36e-02 2.14e+01 3.81e-01 2.00e+00 2.86e+15 1 1.23e-01 5.28e+00
W1224 16:08:34.107534 18868 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
50 1.233697e+04 0.00e+00 2.14e+01 0.00e+00 0.00e+00 1.43e+15 1 4.63e-02 5.32e+00
Solver Summary (v 2.0.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)
Original Reduced
Parameter blocks 22122 22122
Parameters 66462 66462
Residual blocks 83718 83718
Residuals 167436 167436
Minimizer TRUST_REGION
Sparse linear algebra library SUITE_SPARSE
Trust region strategy LEVENBERG_MARQUARDT
Given Used
Linear solver SPARSE_SCHUR SPARSE_SCHUR
Threads 1 1
Linear solver ordering AUTOMATIC 22106,16
Schur structure 2,3,9 2,3,9
Cost:
Initial 1.842900e+07
Final 1.233697e+04
Change 1.841667e+07
Minimizer iterations 51
Successful steps 37
Unsuccessful steps 14
Time (in seconds):
Preprocessor 0.169962
Residual only evaluation 0.507175 (36)
Jacobian & residual evaluation 1.776603 (37)
Linear solver 2.374681 (50)
Minimizer 5.156110
Postprocessor 0.005225
Total 5.331297
Termination: NO_CONVERGENCE (Maximum number of iterations reached. Number of iterations: 50.)
4 使用meshlab
meshlab initial.ply
meshlab final.ply
5 基于g2o的BA优化
CMake 不用改直接沿用上面的
./bundle_adjustment_g2o ../problem-16-22106-pre.txt
zhangyuanbo@zhangyuanbo-virtual-machine:~/Slam14_2/ch9/build$ ./bundle_adjustment_g2o ../problem-16-22106-pre.txt
Header: 16 22106 83718iteration= 0 chi2= 8894422.962194 time= 0.422137 cumTime= 0.422137 edges= 83718 schur= 1 lambda= 227.832660 levenbergIter= 1
iteration= 1 chi2= 1772145.543625 time= 0.307068 cumTime= 0.729205 edges= 83718 schur= 1 lambda= 75.944220 levenbergIter= 1
iteration= 2 chi2= 752585.321418 time= 0.299007 cumTime= 1.02821 edges= 83718 schur= 1 lambda= 25.314740 levenbergIter= 1
iteration= 3 chi2= 402814.285609 time= 0.335285 cumTime= 1.3635 edges= 83718 schur= 1 lambda= 8.438247 levenbergIter= 1
iteration= 4 chi2= 284879.389455 time= 0.330836 cumTime= 1.69433 edges= 83718 schur= 1 lambda= 2.812749 levenbergIter= 1
iteration= 5 chi2= 238356.210033 time= 0.324749 cumTime= 2.01908 edges= 83718 schur= 1 lambda= 0.937583 levenbergIter= 1
iteration= 6 chi2= 193550.729802 time= 0.340701 cumTime= 2.35978 edges= 83718 schur= 1 lambda= 0.312528 levenbergIter= 1
iteration= 7 chi2= 146861.192839 time= 0.326223 cumTime= 2.68601 edges= 83718 schur= 1 lambda= 0.104176 levenbergIter= 1
iteration= 8 chi2= 122873.392728 time= 0.297094 cumTime= 2.9831 edges= 83718 schur= 1 lambda= 0.069451 levenbergIter= 1
iteration= 9 chi2= 97812.478436 time= 0.300793 cumTime= 3.28389 edges= 83718 schur= 1 lambda= 0.046300 levenbergIter= 1
iteration= 10 chi2= 80336.316621 time= 0.323352 cumTime= 3.60724 edges= 83718 schur= 1 lambda= 0.030867 levenbergIter= 1
iteration= 11 chi2= 65654.850651 time= 0.302573 cumTime= 3.90982 edges= 83718 schur= 1 lambda= 0.020578 levenbergIter= 1
iteration= 12 chi2= 55967.141021 time= 0.315573 cumTime= 4.22539 edges= 83718 schur= 1 lambda= 0.013719 levenbergIter= 1
iteration= 13 chi2= 53270.115686 time= 0.319433 cumTime= 4.54482 edges= 83718 schur= 1 lambda= 0.009146 levenbergIter= 1
iteration= 14 chi2= 35981.369897 time= 0.382385 cumTime= 4.92721 edges= 83718 schur= 1 lambda= 0.006097 levenbergIter= 2
iteration= 15 chi2= 32092.173309 time= 0.44166 cumTime= 5.36887 edges= 83718 schur= 1 lambda= 0.016259 levenbergIter= 3
iteration= 16 chi2= 31154.877381 time= 0.379767 cumTime= 5.74864 edges= 83718 schur= 1 lambda= 0.021679 levenbergIter= 2
iteration= 17 chi2= 30773.690800 time= 0.3253 cumTime= 6.07394 edges= 83718 schur= 1 lambda= 0.014453 levenbergIter= 1
iteration= 18 chi2= 29079.971263 time= 0.404443 cumTime= 6.47838 edges= 83718 schur= 1 lambda= 0.012508 levenbergIter= 2
iteration= 19 chi2= 28481.944292 time= 0.430352 cumTime= 6.90873 edges= 83718 schur= 1 lambda= 0.016678 levenbergIter= 2
iteration= 20 chi2= 28439.938323 time= 0.312436 cumTime= 7.22117 edges= 83718 schur= 1 lambda= 0.011118 levenbergIter= 1
iteration= 21 chi2= 27171.835892 time= 0.383616 cumTime= 7.60478 edges= 83718 schur= 1 lambda= 0.011153 levenbergIter= 2
iteration= 22 chi2= 26749.623597 time= 0.374734 cumTime= 7.97952 edges= 83718 schur= 1 lambda= 0.014871 levenbergIter= 2
iteration= 23 chi2= 26674.555645 time= 0.330881 cumTime= 8.3104 edges= 83718 schur= 1 lambda= 0.009914 levenbergIter= 1
iteration= 24 chi2= 26089.998120 time= 0.417419 cumTime= 8.72782 edges= 83718 schur= 1 lambda= 0.010288 levenbergIter= 2
iteration= 25 chi2= 25877.861699 time= 0.396127 cumTime= 9.12395 edges= 83718 schur= 1 lambda= 0.013717 levenbergIter= 2
iteration= 26 chi2= 25834.638622 time= 0.300915 cumTime= 9.42486 edges= 83718 schur= 1 lambda= 0.009145 levenbergIter= 1
iteration= 27 chi2= 25570.298632 time= 0.396889 cumTime= 9.82175 edges= 83718 schur= 1 lambda= 0.011127 levenbergIter= 2
iteration= 28 chi2= 25457.520755 time= 0.376847 cumTime= 10.1986 edges= 83718 schur= 1 lambda= 0.011716 levenbergIter= 2
iteration= 29 chi2= 25380.650160 time= 0.384412 cumTime= 10.583 edges= 83718 schur= 1 lambda= 0.012090 levenbergIter= 2
iteration= 30 chi2= 25362.215118 time= 0.30289 cumTime= 10.8859 edges= 83718 schur= 1 lambda= 0.008060 levenbergIter= 1
iteration= 31 chi2= 25202.452901 time= 0.41973 cumTime= 11.3056 edges= 83718 schur= 1 lambda= 0.008800 levenbergIter= 2
iteration= 32 chi2= 25122.596797 time= 0.407066 cumTime= 11.7127 edges= 83718 schur= 1 lambda= 0.009613 levenbergIter= 2
iteration= 33 chi2= 25115.107638 time= 0.330403 cumTime= 12.0431 edges= 83718 schur= 1 lambda= 0.006408 levenbergIter= 1
iteration= 34 chi2= 24967.189622 time= 0.381439 cumTime= 12.4245 edges= 83718 schur= 1 lambda= 0.006268 levenbergIter= 2
iteration= 35 chi2= 24910.138884 time= 0.374837 cumTime= 12.7994 edges= 83718 schur= 1 lambda= 0.008358 levenbergIter= 2
iteration= 36 chi2= 24867.273980 time= 0.445144 cumTime= 13.2445 edges= 83718 schur= 1 lambda= 0.006277 levenbergIter= 2
iteration= 37 chi2= 24848.840518 time= 0.392381 cumTime= 13.6369 edges= 83718 schur= 1 lambda= 0.008370 levenbergIter= 2
iteration= 38 chi2= 24847.135479 time= 0.365546 cumTime= 14.0024 edges= 83718 schur= 1 lambda= 0.005580 levenbergIter= 1
iteration= 39 chi2= 24798.075555 time= 0.381402 cumTime= 14.3838 edges= 83718 schur= 1 lambda= 0.007249 levenbergIter= 2
meshlab initial.ply
meshlab final.ply
