slambook2（ch7）—— Ubuntu18.04 + 例程演示

一、orb_cv

 ./orb_cv ../1.png ../2.png

结果：

extract ORB cost = 0.0535694 seconds. 
match ORB cost = 0.0113605 seconds. 
-- Max dist : 95.000000 
-- Min dist : 4.000000 

for (int i = 0; i < descriptors_1.rows; i++)//遍历描述子
   {
    if (matches[i].distance <= max(2 * min_dist, 30.0)) //不同的结果可以在这里设置
    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误 30.0为经验值
    {
      good_matches.push_back(matches[i]);
    }
  }

二、orb_self

// global variables
string first_file = "../1.png";
string second_file = "../2.png";

./orb_self ../1.png ../2.png

结果：

bad/total: 43/638
bad/total: 8/595
extract ORB cost = 0.00490673 seconds. 
match ORB cost = 0.00141762 seconds. 
matches: 41

三、pose_estimation_2d2d

 ./pose_estimation_2d2d ../1.png ../2.png

结果：

-- Max dist : 95.000000 
-- Min dist : 4.000000 
一共找到了79组匹配点
fundamental_matrix is 
[4.844484382466111e-06, 0.0001222601840188731, -0.01786737827487386;
 -0.0001174326832719333, 2.122888800459598e-05, -0.01775877156212593;
 0.01799658210895528, 0.008143605989020664, 1]
essential_matrix is 
[-0.02036185505234771, -0.4007110038118444, -0.033240742498241;
 0.3939270778216368, -0.03506401846698084, 0.5857110303721015;
 -0.006788487241438231, -0.5815434272915687, -0.01438258684486259]
homography_matrix is 
[0.9497129583105288, -0.143556453147626, 31.20121878625771;
 0.04154536627445031, 0.9715568969832015, 5.306887618807696;
 -2.81813676978796e-05, 4.353702039810921e-05, 1]
R is 
[0.9985961798781877, -0.05169917220143662, 0.01152671359827862;
 0.05139607508976053, 0.9983603445075083, 0.02520051547522452;
 -0.01281065954813537, -0.02457271064688494, 0.9996159607036126]
t is 
[-0.8220841067933339;
 -0.0326974270640541;
 0.5684264241053518]
t^R=
[-0.02879601157010514, -0.5666909361828475, -0.0470095088643642;
 0.5570970160413605, -0.04958801046730488, 0.8283204827837457;
 -0.009600370724838811, -0.8224266019846685, -0.02034004937801358]
epipolar constraint = [0.002528128704106514]
epipolar constraint = [-0.001663727901710814]
epipolar constraint = [-0.0008009088410885212]
epipolar constraint = [0.0001705869410470254]
epipolar constraint = [-0.0003338015008984979]
epipolar constraint = [0.0003385525272308065]
epipolar constraint = [0.0001729349818584552]
epipolar constraint = [-9.552408320477601e-06]
epipolar constraint = [-0.0008834408754688165]
epipolar constraint = [-0.000444586092781381]
epipolar constraint = [-0.000156067923593961]
epipolar constraint = [0.001512967129777068]
epipolar constraint = [-0.0002644334828964742]
epipolar constraint = [-3.514414351252562e-06]
epipolar constraint = [-0.0004170632811044614]
epipolar constraint = [-0.0007749589896892117]
epipolar constraint = [0.002091463454860276]
epipolar constraint = [-0.001016195254389909]
epipolar constraint = [0.0005870797511206075]
epipolar constraint = [0.0002701337927295891]
epipolar constraint = [-0.0008153290073634545]
epipolar constraint = [0.005595329855570208]
epipolar constraint = [0.004456949384590653]
epipolar constraint = [-0.00109225844630996]
epipolar constraint = [-0.00122027527435923]
epipolar constraint = [0.0001206258081121597]
epipolar constraint = [-0.0007167735646266809]
epipolar constraint = [0.002034045481378033]
epipolar constraint = [-0.001256283161205782]
epipolar constraint = [-2.841028832301085e-06]
epipolar constraint = [-0.0009452266349239957]
epipolar constraint = [-0.0003143393086872948]
epipolar constraint = [-0.003421410506807192]
epipolar constraint = [0.0006511227496003788]
epipolar constraint = [-0.001310996225762057]
epipolar constraint = [-0.0001590978941137114]
epipolar constraint = [-0.002209031974010213]
epipolar constraint = [-0.0007840664639768846]
epipolar constraint = [-0.00219092181574767]
epipolar constraint = [0.002922765516590181]
epipolar constraint = [-0.0002852488690396338]
epipolar constraint = [0.001288650990044271]
epipolar constraint = [0.002981122529430141]
epipolar constraint = [0.001104024767925333]
epipolar constraint = [0.0005839797440089639]
epipolar constraint = [-0.002811645087152688]
epipolar constraint = [-0.001723388366795087]
epipolar constraint = [0.0001541481260837613]
epipolar constraint = [0.0006004071491191379]
epipolar constraint = [-0.001728591166312573]
epipolar constraint = [-0.0007782250239872224]
epipolar constraint = [-0.001075058873840032]
epipolar constraint = [0.004654782908027483]
epipolar constraint = [-0.00145774661621554]
epipolar constraint = [0.0003259585394422768]
epipolar constraint = [-9.139514634399354e-06]
epipolar constraint = [2.094089762112034e-06]
epipolar constraint = [-0.00122725042555329]
epipolar constraint = [-0.0008551935807612487]
epipolar constraint = [0.001650773210968584]
epipolar constraint = [0.00116044245491314]
epipolar constraint = [0.001879717958470126]
epipolar constraint = [-5.97742462742773e-06]
epipolar constraint = [-0.0003369336336238871]
epipolar constraint = [0.004360922097753794]
epipolar constraint = [-0.005310637569393865]
epipolar constraint = [-0.0006060103678098214]
epipolar constraint = [1.216121374464363e-06]
epipolar constraint = [-0.003401336870289186]
epipolar constraint = [0.002238878760289525]
epipolar constraint = [0.001475291883444502]
epipolar constraint = [-0.003206338609952966]
epipolar constraint = [-0.001462525388296471]
epipolar constraint = [-0.0007503932332671159]
epipolar constraint = [0.00384724837206624]
epipolar constraint = [6.646617176919722e-07]
epipolar constraint = [0.0007123827789497824]
epipolar constraint = [-0.0005911111586385312]
epipolar constraint = [-0.004921591124588801]

四、triangulation

 ./triangulation ../1.png ../2.png

结果：

-- Max dist : 95.000000 
-- Min dist : 4.000000 
一共找到了79组匹配点
depth: 45.3724
depth: 16.6993
depth: 14.046
depth: 13.267
depth: 13.6788
depth: 30.0102
depth: 14.6864
depth: 13.8394
depth: 14.408
depth: 15.118
depth: 17.1833
depth: 16.2955
depth: 14.4425
depth: 15.5032
depth: 16.6199
depth: 29.6163
depth: 29.8479
depth: 14.2034
depth: 17.2911
depth: 13.952
depth: 14.7138
depth: 51.8393
depth: 50.3779
depth: 15.2634
depth: 15.1423
depth: 13.5731
depth: 15.2204
depth: 17.2002
depth: 15.1018
depth: 13.1856
depth: 28.1319
depth: 27.8687
depth: 14.8212
depth: 16.2624
depth: 13.3843
depth: 14.8062
depth: 14.6493
depth: 16.8406
depth: 14.6166
depth: 44.9113
depth: 28.6374
depth: 28.7027
depth: 46.6309
depth: 15.6839
depth: 14.9472
depth: 14.5156
depth: 14.0974
depth: 13.5684
depth: 19.3896
depth: 17.501
depth: 29.7291
depth: 15.5625
depth: 53.3157
depth: 16.3022
depth: 12.9899
depth: 17.7342
depth: 18.2738
depth: 17.8923
depth: 16.5829
depth: 15.7578
depth: 17.9717
depth: 18.049
depth: 14.3796
depth: 10.4467
depth: 11.2243
depth: 13.9109
depth: 16.0181
depth: 22.9477
depth: 15.9597
depth: 10.2454
depth: 9.95815
depth: 16.3941
depth: 14.9895
depth: 18.6442
depth: 18.0098
depth: 11.627
depth: 16.2583
depth: 13.4797
depth: 17.2989

五、pose_estimation_3d2d

1.报错：In function `fmt

/home/sfann/sf_slam_temp_code/ch7/build> make -j2 pose_estimation_3d2d
[ 50%] Linking CXX executable pose_estimation_3d2d
CMakeFiles/pose_estimation_3d2d.dir/pose_estimation_3d2d.cpp.o: In function `fmt::v8::appender fmt::v8::detail::write<char, fmt::v8::appender>(fmt::v8::appender, fmt::v8::basic_string_view<fmt::v8::type_identity<char>::type>, fmt::v8::basic_format_specs<char> const&, fmt::v8::detail::locale_ref) [clone .isra.1473]':
pose_estimation_3d2d.cpp:(.text+0x496c): undefined reference to `fmt::v8::detail::throw_format_error(char const*)'
CMakeFiles/pose_estimation_3d2d.dir/pose_estimation_3d2d.cpp.o: In function `unsigned long long fmt::v8::detail::width_checker<fmt::v8::detail::error_handler>::operator()<float, 0>(float) [clone .isra.378]':
pose_estimation_3d2d.cpp:(.text._ZN3fmt2v86detail5valueINS0_20basic_format_contextINS0_8appenderEcEEE17format_custom_argIN5Eigen9TransposeIKNS8_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEENS1_18fallback_formatterISD_cvEEEEvPvRNS0_26basic_format_parse_contextIcNS1_13error_handlerEEERS5_[_ZN3fmt2v86detail5valueINS0_20basic_format_contextINS0_8appenderEcEEE17format_custom_argIN5Eigen9TransposeIKNS8_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEENS1_18fallback_formatterISD_cvEEEEvPvRNS0_26basic_format_parse_contextIcNS1_13error_handlerEEERS5_]+0x1fb): undefined reference to `std::locale fmt::v8::detail::locale_ref::get<std::locale>() const'
pose_estimation_3d2d.cpp:(.text._ZN3fmt2v86detail5valueINS0_20basic_format_contextINS0_8appenderEcEEE17format_custom_argIN5Eigen9TransposeIKNS8_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEENS1_18fallback_formatterISD_cvEEEEvPvRNS0_26basic_format_parse_contextIcNS1_13error_handlerEEERS5_[_ZN3fmt2v86detail5valueINS0_20basic_format_contextINS0_8appenderEcEEE17format_custom_argIN5Eigen9TransposeIKNS8_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEENS1_18fallback_formatterISD_cvEEEEvPvRNS0_26basic_format_parse_contextIcNS1_13error_handlerEEERS5_]+0xdef): undefined reference to `fmt::v8::detail::throw_format_error(char const*)'
CMakeFiles/pose_estimation_3d2d.dir/pose_estimation_3d2d.cpp.o: In function `void fmt::v8::detail::value<fmt::v8::basic_format_context<fmt::v8::appender, char> >::format_custom_arg<Eigen::Transpose<Eigen::Matrix<double, 4, 1, 0, 4, 1> >, fmt::v8::detail::fallback_formatter<Eigen::Transpose<Eigen::Matrix<double, 4, 1, 0, 4, 1> >, char, void> >(void*, fmt::v8::basic_format_parse_context<char, fmt::v8::detail::error_handler>&, fmt::v8::basic_format_context<fmt::v8::appender, char>&)':
pose_estimation_3d2d.cpp:(.text._ZN3fmt2v86detail5valueINS0_20basic_format_contextINS0_8appenderEcEEE17format_custom_argIN5Eigen9TransposeINS8_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEEENS1_18fallback_formatterISC_cvEEEEvPvRNS0_26basic_format_parse_contextIcNS1_13error_handlerEEERS5_[_ZN3fmt2v86detail5valueINS0_20basic_format_contextINS0_8appenderEcEEE17format_custom_argIN5Eigen9TransposeINS8_6MatrixIdLi4ELi1ELi0ELi4ELi1EEEEENS1_18fallback_formatterISC_cvEEEEvPvRNS0_26basic_format_parse_contextIcNS1_13error_handlerEEERS5_]+0xdef): undefined reference to `fmt::v8::detail::throw_format_error(char const*)'
collect2: error: ld returned 1 exit status
CMakeFiles/pose_estimation_3d2d.dir/build.make:119: recipe for target 'pose_estimation_3d2d' failed
CMakeFiles/Makefile2:212: recipe for target 'CMakeFiles/pose_estimation_3d2d.dir/all' failed
make[3]: *** [pose_estimation_3d2d] Error 1
make[2]: *** [CMakeFiles/pose_estimation_3d2d.dir/all] Error 2
CMakeFiles/Makefile2:219: recipe for target 'CMakeFiles/pose_estimation_3d2d.dir/rule' failed
make[1]: *** [CMakeFiles/pose_estimation_3d2d.dir/rule] Error 2
Makefile:189: recipe for target 'pose_estimation_3d2d' failed
make: *** [pose_estimation_3d2d] Error 2
*** 失败：退出代码 2 ***

2.修改CMakeLists.txt

需要用Sophus和g2o的
在每个target_link_libraries后面都加一个fmt

add_executable(pose_estimation_3d3d pose_estimation_3d3d.cpp)
target_link_libraries(pose_estimation_3d3d
          g2o_core g2o_stuff
          ${OpenCV_LIBS} ${Sophus_LIBS} fmt)

g20要用C++14 compiler
所以
CMakeLists.txt

#set(CMAKE_CXX_FLAGS "-std=c++11 -O2 ${SSE_FLAGS} -msse4")
set(CMAKE_CXX_FLAGS "-std=c++14 -O2 ${SSE_FLAGS} -msse4") 

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(vo1)
 
set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
#set(CMAKE_CXX_FLAGS "-std=c++11 -O2 ${SSE_FLAGS} -msse4")
set(CMAKE_CXX_FLAGS "-std=c++14 -O2 ${SSE_FLAGS} -msse4") 
 
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
 
find_package(OpenCV 3 REQUIRED)
find_package(G2O REQUIRED)
find_package(Sophus REQUIRED)
 
include_directories(
        ${OpenCV_INCLUDE_DIRS}
        ${G2O_INCLUDE_DIRS}
        ${Sophus_INCLUDE_DIRS}
        "/usr/include/eigen3/"
       
)
 
add_executable(orb_cv orb_cv.cpp)
target_link_libraries(orb_cv ${OpenCV_LIBS} fmt)
 
add_executable(orb_self orb_self.cpp)
target_link_libraries(orb_self ${OpenCV_LIBS} fmt)
 
# add_executable( pose_estimation_2d2d pose_estimation_2d2d.cpp extra.cpp ) # use this if in OpenCV2 
add_executable(pose_estimation_2d2d pose_estimation_2d2d.cpp)
target_link_libraries(pose_estimation_2d2d ${OpenCV_LIBS} fmt)
 
# # add_executable( triangulation triangulation.cpp extra.cpp) # use this if in opencv2
add_executable(triangulation triangulation.cpp)
target_link_libraries(triangulation ${OpenCV_LIBS} fmt)
 
add_executable(pose_estimation_3d2d pose_estimation_3d2d.cpp)
target_link_libraries(pose_estimation_3d2d
        g2o_core g2o_stuff
        ${OpenCV_LIBS} fmt)
 
add_executable(pose_estimation_3d3d pose_estimation_3d3d.cpp)
target_link_libraries(pose_estimation_3d3d
        g2o_core g2o_stuff
        ${OpenCV_LIBS} fmt)

2.报错：error while loading shared libraries

./pose_estimation_3d2d: error while loading shared libraries: libg2o_core.so: cannot open shared object file: No such file or directory

在build文件夹目录环境下输入：

sudo ldconfig

然后编译就可以了。
因为g2o刚装,没生效。

3.编译运行

 ./pose_estimation_3d2d ../1.png ../2.png ../1_depth.png ../2_depth.png 

结果：

-- Max dist : 95.000000 
-- Min dist : 4.000000 
一共找到了79组匹配点
3d-2d pairs: 76
solve pnp in opencv cost time: 0.013353 seconds.
R=
[0.9978662025826269, -0.05167241613316376, 0.03991244360207524;
 0.0505958915956335, 0.998339762771668, 0.02752769192381471;
 -0.04126860182960625, -0.025449547736074, 0.998823919929363]
t=
[-0.1272259656955879;
 -0.007507297652615337;
 0.06138584177157709]
calling bundle adjustment by gauss newton
iteration 0 cost=45538.1857253
iteration 1 cost=413.221881688
iteration 2 cost=301.36705717
iteration 3 cost=301.365779441
pose by g-n: 
    0.99786620258  -0.0516724160901   0.0399124437155   -0.127225965886
   0.050595891549    0.998339762774     0.02752769194 -0.00750729768072
 -0.0412686019426  -0.0254495477483    0.998823919924   0.0613858418151
                0                 0                 0                 1
solve pnp by gauss newton cost time: 0.00029197 seconds.
calling bundle adjustment by g2o
iteration= 0	 chi2= 413.221882	 time= 2.0887e-05	 cumTime= 2.0887e-05	 edges= 76	 schur= 0
iteration= 1	 chi2= 301.367057	 time= 9.591e-06	 cumTime= 3.0478e-05	 edges= 76	 schur= 0
iteration= 2	 chi2= 301.365779	 time= 8.691e-06	 cumTime= 3.9169e-05	 edges= 76	 schur= 0
iteration= 3	 chi2= 301.365779	 time= 8.47e-06	 cumTime= 4.7639e-05	 edges= 76	 schur= 0
iteration= 4	 chi2= 301.365779	 time= 8.385e-06	 cumTime= 5.6024e-05	 edges= 76	 schur= 0
iteration= 5	 chi2= 301.365779	 time= 8.251e-06	 cumTime= 6.4275e-05	 edges= 76	 schur= 0
iteration= 6	 chi2= 301.365779	 time= 8.317e-06	 cumTime= 7.2592e-05	 edges= 76	 schur= 0
iteration= 7	 chi2= 301.365779	 time= 8.27e-06	 cumTime= 8.0862e-05	 edges= 76	 schur= 0
iteration= 8	 chi2= 301.365779	 time= 8.243e-06	 cumTime= 8.9105e-05	 edges= 76	 schur= 0
iteration= 9	 chi2= 301.365779	 time= 8.209e-06	 cumTime= 9.7314e-05	 edges= 76	 schur= 0
optimization costs time: 0.000770925 seconds.
pose estimated by g2o =
   0.997866202583  -0.0516724161336   0.0399124436024   -0.127225965696
   0.050595891596    0.998339762772   0.0275276919261 -0.00750729765631
   -0.04126860183  -0.0254495477384    0.998823919929   0.0613858417711
                0                 0                 0                 1
solve pnp by g2o cost time: 0.004325422 seconds.

六、pose_estimation_3d3d

./pose_estimation_3d3d ../1.png ../2.png ../1_depth.png ../2_depth.png 

结果：

-- Max dist : 95.000000 
-- Min dist : 4.000000 
一共找到了79组匹配点
3d-3d pairs: 74
W=  11.9404 -0.567258   1.64182
 -1.79283   4.31299  -6.57615
  3.12791  -6.55815   10.8576
U=  0.474144  -0.880373 -0.0114952
 -0.460275  -0.258979   0.849163
  0.750556   0.397334   0.528006
V=  0.535211  -0.844064 -0.0332488
 -0.434767  -0.309001    0.84587
  0.724242   0.438263   0.532352
ICP via SVD results: 
R = [0.9972395977366735, 0.05617039856770108, -0.04855997354553421;
 -0.05598345194682008, 0.9984181427731503, 0.005202431117422968;
 0.04877538122983249, -0.002469515369266706, 0.9988067198811419]
t = [0.1417248739257467;
 -0.05551033302525168;
 -0.03119093188273836]
R_inv = [0.9972395977366735, -0.05598345194682008, 0.04877538122983249;
 0.05617039856770108, 0.9984181427731503, -0.002469515369266706;
 -0.04855997354553421, 0.005202431117422968, 0.9988067198811419]
t_inv = [-0.1429199667309692;
 0.04738475446275831;
 0.03832465717628154]
calling bundle adjustment
iteration= 0	 chi2= 1.811537	 time= 2.6815e-05	 cumTime= 2.6815e-05	 edges= 74	 schur= 0	 lambda= 0.000795	 levenbergIter= 1
iteration= 1	 chi2= 1.811051	 time= 1.174e-05	 cumTime= 3.8555e-05	 edges= 74	 schur= 0	 lambda= 0.000530	 levenbergIter= 1
iteration= 2	 chi2= 1.811050	 time= 1.0705e-05	 cumTime= 4.926e-05	 edges= 74	 schur= 0	 lambda= 0.000353	 levenbergIter= 1
iteration= 3	 chi2= 1.811050	 time= 1.0713e-05	 cumTime= 5.9973e-05	 edges= 74	 schur= 0	 lambda= 0.000236	 levenbergIter= 1
iteration= 4	 chi2= 1.811050	 time= 1.0348e-05	 cumTime= 7.0321e-05	 edges= 74	 schur= 0	 lambda= 0.000157	 levenbergIter= 1
iteration= 5	 chi2= 1.811050	 time= 1.0477e-05	 cumTime= 8.0798e-05	 edges= 74	 schur= 0	 lambda= 0.000105	 levenbergIter= 1
iteration= 6	 chi2= 1.811050	 time= 1.5339e-05	 cumTime= 9.6137e-05	 edges= 74	 schur= 0	 lambda= 0.006703	 levenbergIter= 3
optimization costs time: 0.000467389 seconds.

after optimization:
T=
   0.99724  0.0561704   -0.04856   0.141725
-0.0559834   0.998418 0.00520242 -0.0555103
 0.0487754 -0.0024695   0.998807 -0.0311913
         0          0          0          1
p1 = [-0.0374123, -0.830816, 2.7448]
p2 = [-0.0111479, -0.746763, 2.7652]
(R*p2+t) = [-0.0456162060096475;
 -0.7860824176937432;
 2.732009294040371]

p1 = [-0.243698, -0.117719, 1.5848]
p2 = [-0.299118, -0.0975683, 1.6558]
(R*p2+t) = [-0.2424538642667685;
 -0.127564412420742;
 1.608284141645051]

p1 = [-0.627753, 0.160186, 1.3396]
p2 = [-0.709645, 0.159033, 1.4212]
(R*p2+t) = [-0.6260418020833747;
 0.1503926994308992;
 1.353306864239974]

p1 = [-0.323443, 0.104873, 1.4266]
p2 = [-0.399079, 0.12047, 1.4838]
(R*p2+t) = [-0.3215392021872832;
 0.09483002837549004;
 1.43107537937968]

p1 = [-0.627221, 0.101454, 1.3116]
p2 = [-0.709709, 0.100216, 1.3998]
(R*p2+t) = [-0.6283696073388314;
 0.09156140480846489;
 1.33207446092358]