1 Ceres BA的书写
1.1 源文件和头文件
各文件联系如下，
1.2 CMakeLists.txt文件
内容如下，
set(CMAKE_BUILD_TYPE "Release") #能加快编译
find_package(Ceres REQUIRED)
include_directories(${CERES_DIRECTORIES})
include_directories("/usr/include/eigen3")
add_library(bal_common common.cpp) #生成静态库
add_executable(main bundle_adjustment_ceres.cpp)
target_link_libraries(main ${CERES_LIBRARIES} bal_common)
1.3 执行结果
屏幕输出为，
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 5.40e-02 2.11e-01
1 1.449093e+06 1.70e+07 1.75e+06 2.16e+03 1.84e+00 3.00e+04 1 1.08e-01 3.19e-01
2 5.848543e+04 1.39e+06 1.30e+06 1.55e+03 1.87e+00 9.00e+04 1 9.83e-02 4.17e-01
3 1.581483e+04 4.27e+04 4.98e+05 4.98e+02 1.29e+00 2.70e+05 1 9.79e-02 5.15e-01
4 1.251823e+04 3.30e+03 4.64e+04 9.96e+01 1.11e+00 8.10e+05 1 9.72e-02 6.12e-01
5 1.240936e+04 1.09e+02 9.78e+03 1.33e+01 1.42e+00 2.43e+06 1 9.76e-02 7.10e-01
6 1.237699e+04 3.24e+01 3.91e+03 5.04e+00 1.70e+00 7.29e+06 1 9.72e-02 8.07e-01
7 1.236187e+04 1.51e+01 1.96e+03 3.40e+00 1.75e+00 2.19e+07 1 9.74e-02 9.04e-01
8 1.235405e+04 7.82e+00 1.03e+03 2.40e+00 1.76e+00 6.56e+07 1 9.76e-02 1.00e+00
9 1.234934e+04 4.71e+00 5.04e+02 1.67e+00 1.87e+00 1.97e+08 1 9.75e-02 1.10e+00
10 1.234610e+04 3.24e+00 4.31e+02 1.15e+00 1.88e+00 5.90e+08 1 9.73e-02 1.20e+00
11 1.234386e+04 2.24e+00 3.27e+02 8.44e-01 1.90e+00 1.77e+09 1 9.73e-02 1.29e+00
12 1.234232e+04 1.54e+00 3.44e+02 6.69e-01 1.82e+00 5.31e+09 1 9.75e-02 1.39e+00
13 1.234126e+04 1.07e+00 2.21e+02 5.45e-01 1.91e+00 1.59e+10 1 9.75e-02 1.49e+00
14 1.234047e+04 7.90e-01 1.12e+02 4.84e-01 1.87e+00 4.78e+10 1 9.71e-02 1.59e+00
15 1.233986e+04 6.07e-01 1.02e+02 4.22e-01 1.95e+00 1.43e+11 1 9.76e-02 1.68e+00
16 1.233934e+04 5.22e-01 1.03e+02 3.82e-01 1.97e+00 4.30e+11 1 9.74e-02 1.78e+00
17 1.233891e+04 4.25e-01 1.07e+02 3.46e-01 1.93e+00 1.29e+12 1 9.72e-02 1.88e+00
18 1.233855e+04 3.59e-01 1.04e+02 3.15e-01 1.96e+00 3.87e+12 1 9.75e-02 1.98e+00
19 1.233825e+04 3.06e-01 9.27e+01 2.88e-01 1.98e+00 1.16e+13 1 9.77e-02 2.07e+00
20 1.233799e+04 2.61e-01 1.17e+02 2.16e-01 1.97e+00 3.49e+13 1 9.74e-02 2.17e+00
21 1.233777e+04 2.18e-01 1.22e+02 1.15e-01 1.97e+00 1.05e+14 1 9.79e-02 2.27e+00
22 1.233760e+04 1.73e-01 1.10e+02 9.46e-02 1.89e+00 3.14e+14 1 9.74e-02 2.37e+00
23 1.233746e+04 1.37e-01 1.14e+02 9.12e-02 1.98e+00 9.41e+14 1 9.78e-02 2.46e+00
24 1.233735e+04 1.13e-01 1.16e+02 5.27e-01 1.96e+00 2.82e+15 1 9.72e-02 2.56e+00
WARNING: Logging before InitGoogleLogging() is written to STDERR
W0527 17:28:06.777132 11379 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.16e+02 0.00e+00 0.00e+00 1.41e+15 1 3.20e-02 2.59e+00
26 1.233725e+04 9.50e-02 1.20e+02 5.20e-01 1.99e+00 4.24e+15 1 9.65e-02 2.69e+00
27 1.233718e+04 6.92e-02 5.66e+01 5.83e-01 1.70e+00 1.00e+16 1 9.79e-02 2.79e+00
W0527 17:28:07.003507 11379 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
28 1.233718e+04 0.00e+00 5.66e+01 0.00e+00 0.00e+00 5.00e+15 1 3.19e-02 2.82e+00
29 1.233718e+04 0.00e+00 5.66e+01 0.00e+00 0.00e+00 1.25e+15 1 3.06e-02 2.85e+00
W0527 17:28:07.034121 11379 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
30 1.233714e+04 3.65e-02 5.87e+01 2.88e-01 1.93e+00 3.75e+15 1 9.68e-02 2.95e+00
31 1.233714e+04 0.00e+00 5.87e+01 0.00e+00 0.00e+00 1.88e+15 1 3.23e-02 2.98e+00
W0527 17:28:07.163192 11379 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
32 1.233711e+04 3.32e-02 6.14e+01 5.34e-01 2.00e+00 5.62e+15 1 9.60e-02 3.08e+00
33 1.233711e+04 0.00e+00 6.14e+01 0.00e+00 0.00e+00 2.81e+15 1 3.20e-02 3.11e+00
W0527 17:28:07.291311 11379 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
34 1.233711e+04 0.00e+00 6.14e+01 0.00e+00 0.00e+00 7.03e+14 1 3.07e-02 3.14e+00
W0527 17:28:07.322024 11379 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
35 1.233708e+04 3.14e-02 6.12e+01 2.00e-01 2.00e+00 2.11e+15 1 9.71e-02 3.24e+00
36 1.233705e+04 2.50e-02 2.05e+01 9.85e-01 1.68e+00 6.33e+15 1 9.81e-02 3.33e+00
W0527 17:28:07.549093 11379 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
37 1.233705e+04 0.00e+00 2.05e+01 0.00e+00 0.00e+00 3.16e+15 1 3.18e-02 3.37e+00
38 1.233704e+04 1.58e-02 2.40e+01 4.00e+00 1.95e+00 9.49e+15 1 9.65e-02 3.46e+00
39 1.233704e+04 0.00e+00 2.40e+01 0.00e+00 0.00e+00 4.75e+15 1 3.20e-02 3.49e+00
W0527 17:28:07.677624 11379 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.40e+01 0.00e+00 0.00e+00 1.19e+15 1 3.08e-02 3.53e+00
W0527 17:28:07.708451 11379 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
41 1.233702e+04 1.51e-02 2.06e+01 2.59e-01 1.99e+00 3.56e+15 1 9.74e-02 3.62e+00
42 1.233702e+04 0.00e+00 2.06e+01 0.00e+00 0.00e+00 1.78e+15 1 3.18e-02 3.65e+00
W0527 17:28:07.837718 11379 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
43 1.233701e+04 1.48e-02 2.06e+01 5.78e-01 1.99e+00 5.34e+15 1 9.60e-02 3.75e+00
44 1.233701e+04 0.00e+00 2.06e+01 0.00e+00 0.00e+00 2.67e+15 1 3.23e-02 3.78e+00
W0527 17:28:07.966040 11379 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
45 1.233701e+04 0.00e+00 2.06e+01 0.00e+00 0.00e+00 6.67e+14 1 3.07e-02 3.81e+00
W0527 17:28:07.996728 11379 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
46 1.233700e+04 1.42e-02 2.08e+01 1.63e-01 1.99e+00 2.00e+15 1 9.67e-02 3.91e+00
47 1.233698e+04 1.39e-02 3.80e+01 2.20e+00 2.00e+00 6.01e+15 1 9.77e-02 4.01e+00
48 1.233698e+04 0.00e+00 3.80e+01 0.00e+00 0.00e+00 3.00e+15 1 3.18e-02 4.04e+00
W0527 17:28:08.222977 11379 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
W0527 17:28:08.253832 11379 levenberg_marquardt_strategy.cc:115] Linear solver failure. Failed to compute a step: CHOLMOD warning: Matrix not positive definite.
49 1.233698e+04 0.00e+00 3.80e+01 0.00e+00 0.00e+00 7.51e+14 1 3.08e-02 4.07e+00
50 1.233697e+04 1.36e-02 2.12e+01 1.67e-01 2.00e+00 2.25e+15 1 9.59e-02 4.17e+00
Solver Summary (v 2.0.0-eigen-(3.3.7)-lapack-suitesparse-(5.7.1)-cxsparse-(3.2.0)-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.156824
Residual only evaluation 0.459660 (36)
Jacobian & residual evaluation 1.663201 (37)
Linear solver 1.607496 (50)
Minimizer 4.010450
Postprocessor 0.003175
Total 4.170453
Termination: NO_CONVERGENCE (Maximum number of iterations reached. Number of iterations: 50.)
虽然结果是NO_CONVERGENCE，但cost_change已接近
1
×
1
0
−
2
1 \times 10^{-2}
1×10−2了。
initial.ply展示结果如下，
final.ply展示结果如下，
其中绿色的点表示相机观测点。
利用ceres进行BA优化前后的点云演示视频见BA优化前点云和BA优化后点云。
2 g2o求解BA
源代码和头文件关系如下，
CMakeLists.txt内容如下，
cmake_minimum_required(VERSION 2.8)
project(bundle_adjustment)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_STANDARD 11)
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) #本项目中的cmake文件夹
Find_Package(G2O REQUIRED)
Find_Package(Eigen3 REQUIRED)
Find_Package(Sophus REQUIRED)
Find_Package(CSparse REQUIRED)
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 main.cpp)
target_link_libraries(bundle_adjustment_g2o ${G2O_LIBS} bal_common)
注意本项目中的cmake文件夹提供了相应库的查找链接！
程序运行结果如下，
Header: 16 22106 83718
iteration= 0 chi2= 8894423.022949 time= 0.35444 cumTime= 0.35444 edges= 83718 schur= 1 lambda= 227.832660 levenbergIter= 1
iteration= 1 chi2= 1772145.050517 time= 0.322224 cumTime= 0.676664 edges= 83718 schur= 1 lambda= 75.944220 levenbergIter= 1
iteration= 2 chi2= 752585.293391 time= 0.321201 cumTime= 0.997865 edges= 83718 schur= 1 lambda= 25.314740 levenbergIter= 1
iteration= 3 chi2= 402814.243627 time= 0.321596 cumTime= 1.31946 edges= 83718 schur= 1 lambda= 8.438247 levenbergIter= 1
iteration= 4 chi2= 284879.378894 time= 0.33075 cumTime= 1.65021 edges= 83718 schur= 1 lambda= 2.812749 levenbergIter= 1
iteration= 5 chi2= 238356.214415 time= 0.326522 cumTime= 1.97673 edges= 83718 schur= 1 lambda= 0.937583 levenbergIter= 1
iteration= 6 chi2= 193550.755079 time= 0.32524 cumTime= 2.30197 edges= 83718 schur= 1 lambda= 0.312528 levenbergIter= 1
iteration= 7 chi2= 146859.909574 time= 0.323884 cumTime= 2.62586 edges= 83718 schur= 1 lambda= 0.104176 levenbergIter= 1
iteration= 8 chi2= 122887.700218 time= 0.326389 cumTime= 2.95225 edges= 83718 schur= 1 lambda= 0.069451 levenbergIter= 1
iteration= 9 chi2= 97810.139925 time= 0.316248 cumTime= 3.26849 edges= 83718 schur= 1 lambda= 0.046300 levenbergIter= 1
iteration= 10 chi2= 80329.940265 time= 0.267348 cumTime= 3.53584 edges= 83718 schur= 1 lambda= 0.030867 levenbergIter= 1
iteration= 11 chi2= 65663.994405 time= 0.233712 cumTime= 3.76955 edges= 83718 schur= 1 lambda= 0.020578 levenbergIter= 1
iteration= 12 chi2= 55960.726637 time= 0.213886 cumTime= 3.98344 edges= 83718 schur= 1 lambda= 0.013719 levenbergIter= 1
iteration= 13 chi2= 53275.547797 time= 0.200156 cumTime= 4.1836 edges= 83718 schur= 1 lambda= 0.009146 levenbergIter= 1
iteration= 14 chi2= 35983.312124 time= 0.241608 cumTime= 4.4252 edges= 83718 schur= 1 lambda= 0.006097 levenbergIter= 2
iteration= 15 chi2= 32091.891518 time= 0.280408 cumTime= 4.70561 edges= 83718 schur= 1 lambda= 0.016259 levenbergIter= 3
iteration= 16 chi2= 31156.262647 time= 0.240244 cumTime= 4.94586 edges= 83718 schur= 1 lambda= 0.021679 levenbergIter= 2
iteration= 17 chi2= 30773.139623 time= 0.198548 cumTime= 5.1444 edges= 83718 schur= 1 lambda= 0.014453 levenbergIter= 1
iteration= 18 chi2= 29079.563460 time= 0.240078 cumTime= 5.38448 edges= 83718 schur= 1 lambda= 0.012488 levenbergIter= 2
iteration= 19 chi2= 28484.154313 time= 0.239262 cumTime= 5.62374 edges= 83718 schur= 1 lambda= 0.016651 levenbergIter= 2
iteration= 20 chi2= 28445.405201 time= 0.198838 cumTime= 5.82258 edges= 83718 schur= 1 lambda= 0.011101 levenbergIter= 1
iteration= 21 chi2= 27170.592543 time= 0.24015 cumTime= 6.06273 edges= 83718 schur= 1 lambda= 0.011118 levenbergIter= 2
iteration= 22 chi2= 26748.191194 time= 0.240306 cumTime= 6.30304 edges= 83718 schur= 1 lambda= 0.014824 levenbergIter= 2
iteration= 23 chi2= 26675.118188 time= 0.1987 cumTime= 6.50174 edges= 83718 schur= 1 lambda= 0.009883 levenbergIter= 1
iteration= 24 chi2= 26087.985781 time= 0.240532 cumTime= 6.74227 edges= 83718 schur= 1 lambda= 0.010281 levenbergIter= 2
iteration= 25 chi2= 25875.818536 time= 0.239127 cumTime= 6.9814 edges= 83718 schur= 1 lambda= 0.013708 levenbergIter= 2
iteration= 26 chi2= 25831.564925 time= 0.198495 cumTime= 7.17989 edges= 83718 schur= 1 lambda= 0.009139 levenbergIter= 1
iteration= 27 chi2= 25568.344873 time= 0.240224 cumTime= 7.42011 edges= 83718 schur= 1 lambda= 0.011118 levenbergIter= 2
iteration= 28 chi2= 25455.865005 time= 0.238966 cumTime= 7.65908 edges= 83718 schur= 1 lambda= 0.011781 levenbergIter= 2
iteration= 29 chi2= 25454.942053 time= 0.198693 cumTime= 7.85777 edges= 83718 schur= 1 lambda= 0.007854 levenbergIter= 1
iteration= 30 chi2= 25260.709796 time= 0.240853 cumTime= 8.09863 edges= 83718 schur= 1 lambda= 0.009148 levenbergIter= 2
iteration= 31 chi2= 25171.392636 time= 0.23984 cumTime= 8.33847 edges= 83718 schur= 1 lambda= 0.009425 levenbergIter= 2
iteration= 32 chi2= 25104.160294 time= 0.238824 cumTime= 8.57729 edges= 83718 schur= 1 lambda= 0.008637 levenbergIter= 2
iteration= 33 chi2= 25042.986799 time= 0.238589 cumTime= 8.81588 edges= 83718 schur= 1 lambda= 0.008765 levenbergIter= 2
iteration= 34 chi2= 24984.677998 time= 0.23827 cumTime= 9.05415 edges= 83718 schur= 1 lambda= 0.005949 levenbergIter= 2
iteration= 35 chi2= 24943.879912 time= 0.238347 cumTime= 9.2925 edges= 83718 schur= 1 lambda= 0.007933 levenbergIter= 2
iteration= 36 chi2= 24886.075504 time= 0.238136 cumTime= 9.53063 edges= 83718 schur= 1 lambda= 0.005674 levenbergIter= 2
iteration= 37 chi2= 24868.088225 time= 0.238319 cumTime= 9.76895 edges= 83718 schur= 1 lambda= 0.007565 levenbergIter= 2
iteration= 38 chi2= 24833.053138 time= 0.238212 cumTime= 10.0072 edges= 83718 schur= 1 lambda= 0.008448 levenbergIter= 2
iteration= 39 chi2= 24815.047826 time= 0.238156 cumTime= 10.2453 edges= 83718 schur= 1 lambda= 0.009766 levenbergIter= 2
利用g2o库进行BA优化前点云，
利用g2o库进行BA优化后点云，
注：绿色的点表示相机观测点。
相应的展示视频见BA优化前点云和BA优化后点云。
扫一扫
1296
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
