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

SLAM第九讲实践:【后端1】 基于slambook2 的ceres的BA优化以及g2o的BA优化_第1张图片

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

SLAM第九讲实践:【后端1】 基于slambook2 的ceres的BA优化以及g2o的BA优化_第2张图片

meshlab final.ply

SLAM第九讲实践:【后端1】 基于slambook2 的ceres的BA优化以及g2o的BA优化_第3张图片

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

SLAM第九讲实践:【后端1】 基于slambook2 的ceres的BA优化以及g2o的BA优化_第4张图片
meshlab final.ply

SLAM第九讲实践:【后端1】 基于slambook2 的ceres的BA优化以及g2o的BA优化_第5张图片

你可能感兴趣的:(SLAM14讲,c++,开发语言,ubuntu,自动驾驶)