slam 代码运行结果ch7

ch7

参考链接
在代码中，依旧使用opencv提取FAST关键点，计算描述子、匹配特征点的原理与前面章节所述相同

需要注意的是，源码ComputeORB函数中对出界特征点的筛选有瑕疵，修改后正确代码应该是下面这样的（看了好几篇文章，大家都把源码CV到自己的博客，加上注释，却没有修改代码错误的地方，属实是坑。。。）

if(kp.pt.x <= half_boundary || 
	kp.pt.y <= half_boundary || 
	kp.pt.x >= img.cols - half_boundary || 
	kp.pt.y >= img.rows - half_boundary)	//出界

为了简洁，源码的 ORB_pattern 表被单独移动到 ORB_pattern.cpp 中

_mm_popcnt_u32函数用于计算32位变量中1的位数，为了使用这个函数，需要包含头文件nmmintrin.h，并且在CMakeLists.txt中添加下面一行，以增加一个编译选项，支持SSE指令集（当然我们也可以不用这个函数，自己计算）

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

2d-2d

在已知 2D 的像素坐标的前提下，根据两幅图像间多组2D像素点对来估计相机的运动

参考1
参考2
参考3

queryIdx是查询图像的描述子和特征点的下标，trainIdx是训练图像的描述子和特征点的下标。

三角测量

已知：两幅图片中的特征点keypoint_1、keypoint_2以及他们的配对matches、转换R、t
未知：特征点的具体位置（在第一个相机坐标中的坐标
1.三角测量的概念
三角测量的过程及代码实现；
三角测量有哪些不确定性
如何提高三角测量的精度

pnp：VecVector3d pts_3d_eigen
solvePnP里有三种解法：P3P， EPnP，迭代法（默认）；opencv2里参数分别为CV_P3P，CV_EPNP，CV_ITERATIVE （opencv3里多了DLS和UPnP解法）
pnp/p3p可以通过三对点求解

1.orb_cv

sqq@sqq-X442URR:~/slambook2/ch7/build$ ./orb_cv 1.png 2.png
extract ORB cost = 0.0270362 seconds. 
match ORB cost = 0.00220661 seconds. 
-- Max dist : 94.000000 
-- Min dist : 4.000000 

运行结果

2.orb_self

sqq@sqq-X442URR:~/slambook2/ch7/build$ ./orb_self
bad/total: 43/638
bad/total: 8/595
extract ORB cost = 0.00528548 seconds. 
match ORB cost = 0.00119627 seconds. 
matches: 41

运行结果

3.pose_estimation_2d2d

sqq@sqq-X442URR:~/slambook2/ch7/build$ ./pose_estimation_2d2d 1.png 2.png
-- Max dist : 94.000000 
-- Min dist : 4.000000 
一共找到了79组匹配点
fundamental_matrix is 
[4.544437503937326e-06, 0.0001333855576988952, -0.01798499246457619;
 -0.0001275657012959839, 2.266794804637672e-05, -0.01416678429258694;
 0.01814994639952877, 0.004146055871509035, 1]
essential_matrix is 
[0.01097677480088526, 0.2483720528258777, 0.03167429207291153;
 -0.2088833206039177, 0.02908423961781584, -0.6744658838357441;
 0.008286777636447118, 0.6614041624098427, 0.01676523772725936]
homography_matrix is 
[0.9261214281175537, -0.1445322024509824, 33.26921085699328;
 0.04535424464910424, 0.9386696693816731, 8.570979966717406;
 -1.006197561051869e-05, -3.008140280167741e-05, 0.9999999999999999]
R is 
[0.9969387384754708, -0.05155574188737422, 0.05878058527591362;
 0.05000441581290405, 0.998368531736214, 0.02756507279306545;
 -0.06010582439453526, -0.02454140006844053, 0.9978902793175882]
t is 
[-0.9350802885437915;
 -0.03514646275858852;
 0.3526890700495534]
t^R=
[-0.01552350379276751, -0.3512511256212342, -0.04479421342842761;
 0.2954056249512841, -0.04113132611923882, 0.9538388002772655;
 -0.0117192733223717, -0.9353667366911764, -0.02370962656974232]
epipolar constraint = [-0.0005415187881110395]
epipolar constraint = [-0.002158321995511289]
epipolar constraint = [0.0003344241044139079]
epipolar constraint = [-8.762782289159499e-06]
epipolar constraint = [0.000212132191399178]
epipolar constraint = [0.0001088766997727197]
epipolar constraint = [0.0004246621771667111]
epipolar constraint = [-0.003173128212208817]
epipolar constraint = [-3.716645801081497e-05]
epipolar constraint = [-0.001451851842241985]
epipolar constraint = [-0.0009607717979376318]
epipolar constraint = [-0.0008051993498427168]
epipolar constraint = [-0.001424813546545535]
epipolar constraint = [-0.0004339424745821371]
epipolar constraint = [-0.000478610965853582]
epipolar constraint = [-0.000196569283305803]
epipolar constraint = [0.001542309777196771]
epipolar constraint = [0.003107154828888181]
epipolar constraint = [0.0006774648890596965]
epipolar constraint = [-0.001176167495933316]
epipolar constraint = [-0.003987986032032258]
epipolar constraint = [-0.001255263863164498]
epipolar constraint = [-0.001553941958847024]
epipolar constraint = [0.002009914869739976]
epipolar constraint = [-0.0006068096307232096]
epipolar constraint = [-2.769881004420494e-05]
epipolar constraint = [0.001274573011402158]
epipolar constraint = [-0.004169986054360234]
epipolar constraint = [-0.001108104733689524]
epipolar constraint = [-0.0005154295520383712]
epipolar constraint = [-0.001776993208794389]
epipolar constraint = [6.677362292956124e-07]
epipolar constraint = [-0.001853977732928294]
epipolar constraint = [-0.0004823070333038748]
epipolar constraint = [0.0004740904480800279]
epipolar constraint = [-0.002961041748859255]
epipolar constraint = [-0.003347655248297339]
epipolar constraint = [-0.0001975680967504778]
epipolar constraint = [-0.002824643387150695]
epipolar constraint = [-0.0004311798170555103]
epipolar constraint = [0.001181203684013504]
epipolar constraint = [1.756266610475343e-07]
epipolar constraint = [-0.002137829859245668]
epipolar constraint = [0.001153415527396465]
epipolar constraint = [-0.002120357728713398]
epipolar constraint = [2.741529055910741e-06]
epipolar constraint = [0.000904458201820401]
epipolar constraint = [-0.002100484435898768]
epipolar constraint = [-0.0003517789219357401]
epipolar constraint = [-2.72046484559238e-05]
epipolar constraint = [-0.003784823353263345]
epipolar constraint = [-0.001588562608841583]
epipolar constraint = [-0.002928516011805465]
epipolar constraint = [-0.001016592266471106]
epipolar constraint = [0.0001241874577868896]
epipolar constraint = [-0.0009797104629406181]
epipolar constraint = [0.001457875224685545]
epipolar constraint = [-1.153738078288336e-05]
epipolar constraint = [-0.00273324742758773]
epipolar constraint = [0.00141572125781092]
epipolar constraint = [0.001790255871328722]
epipolar constraint = [-0.002547929671675515]
epipolar constraint = [-0.006257078861794559]
epipolar constraint = [-0.001874877415315751]
epipolar constraint = [-0.002104542912833518]
epipolar constraint = [1.39253152928176e-06]
epipolar constraint = [-0.004013502582090489]
epipolar constraint = [-0.004726241071896793]
epipolar constraint = [-0.001328682673648302]
epipolar constraint = [3.995925173527759e-07]
epipolar constraint = [-0.005080511723986339]
epipolar constraint = [-0.001977141850700581]
epipolar constraint = [-0.00166133426468739]
epipolar constraint = [0.004285762870626424]
epipolar constraint = [0.004087325194305855]
epipolar constraint = [0.0001482534285787533]
epipolar constraint = [-0.000962530113462208]
epipolar constraint = [-0.002341076011427135]
epipolar constraint = [-0.006138005035457875]

4.triangulation

sqq@sqq-X442URR:~/slambook2/ch7/build$ ./triangulation 1.png 2.png
-- Max dist : 94.000000 
-- Min dist : 4.000000 
一共找到了79组匹配点
depth: 14.4036
depth: 9.63635
depth: 7.88393
depth: 8.35771
depth: 7.75093
depth: 12.5495
depth: 7.71705
depth: 7.93482
depth: 7.57168
depth: 8.46969
depth: 9.78479
depth: 8.88268
depth: 9.02949
depth: 8.87554
depth: 9.66984
depth: 12.2194
depth: 14.8655
depth: 15.0627
depth: 7.7117
depth: 8.78825
depth: 9.10896
depth: 9.62574
depth: 8.78611
depth: 12.5336
depth: 7.88369
depth: 8.45255
depth: 9.65039
depth: 8.64867
depth: 12.2082
depth: 12.1836
depth: 9.18492
depth: 9.52295
depth: 8.39902
depth: 8.78013
depth: 7.80552
depth: 8.57948
depth: 8.53905
depth: 14.3688
depth: 9.03321
depth: 12.3013
depth: 12.3386
depth: 14.5313
depth: 7.71837
depth: 7.95137
depth: 9.54909
depth: 8.44807
depth: 9.13752
depth: 9.51844
depth: 9.60796
depth: 9.20528
depth: 8.86045
depth: 9.33464
depth: 9.40554
depth: 12.4849
depth: 8.26873
depth: 9.41741
depth: 9.40039
depth: 9.31937
depth: 9.22272
depth: 10.0239
depth: 7.69828
depth: 9.09606
depth: 8.77566
depth: 9.33225
depth: 10.8048
depth: 11.7904
depth: 7.37231
depth: 9.43498
depth: 7.26135
depth: 7.231
depth: 9.42683
depth: 9.14959
depth: 10.1792
depth: 9.09463
depth: 9.35573
depth: 9.51033
depth: 8.4322
depth: 7.89044
depth: 9.86828

img1

img2

5.pose_estimation_3d2d

sqq@sqq-X442URR:~/slambook2/ch7/build$ ./pose_estimation_3d2d 1.png 2.png 1_depth.png 2_depth.png
-- Max dist : 94.000000 
-- Min dist : 4.000000 
一共找到了79组匹配点
3d-2d pairs: 75
solve pnp in opencv cost time: 0.00139201 seconds.
R=
[0.9979059095508309, -0.05091940095965415, 0.03988747033140448;
 0.04981866260950624, 0.998362315737503, 0.02812094185923628;
 -0.04125404876286552, -0.02607491363668024, 0.998808391203994]
t=
[-0.1267821387649159;
 -0.008439496980302531;
 0.06034935738136282]
calling bundle adjustment by gauss newton
iteration 0 cost=40517.7576706
iteration 1 cost=410.547029116
iteration 2 cost=299.76468142
iteration 3 cost=299.763574327
pose by g-n: 
   0.997905909549  -0.0509194008562   0.0398874705187   -0.126782139096
   0.049818662505    0.998362315745   0.0281209417649 -0.00843949683874
 -0.0412540489424  -0.0260749135374    0.998808391199   0.0603493575229
                0                 0                 0                 1
solve pnp by gauss newton cost time: 9.9675e-05 seconds.
calling bundle adjustment by g2o
iteration= 0	 chi2= 410.547029	 time= 2.1195e-05	 cumTime= 2.1195e-05	 edges= 75	 schur= 0
iteration= 1	 chi2= 299.764681	 time= 1.0056e-05	 cumTime= 3.1251e-05	 edges= 75	 schur= 0
iteration= 2	 chi2= 299.763574	 time= 8.734e-06	 cumTime= 3.9985e-05	 edges= 75	 schur= 0
iteration= 3	 chi2= 299.763574	 time= 8.652e-06	 cumTime= 4.8637e-05	 edges= 75	 schur= 0
iteration= 4	 chi2= 299.763574	 time= 8.514e-06	 cumTime= 5.7151e-05	 edges= 75	 schur= 0
iteration= 5	 chi2= 299.763574	 time= 8.603e-06	 cumTime= 6.5754e-05	 edges= 75	 schur= 0
iteration= 6	 chi2= 299.763574	 time= 8.464e-06	 cumTime= 7.4218e-05	 edges= 75	 schur= 0
iteration= 7	 chi2= 299.763574	 time= 8.483e-06	 cumTime= 8.2701e-05	 edges= 75	 schur= 0
iteration= 8	 chi2= 299.763574	 time= 8.511e-06	 cumTime= 9.1212e-05	 edges= 75	 schur= 0
iteration= 9	 chi2= 299.763574	 time= 8.565e-06	 cumTime= 9.9777e-05	 edges= 75	 schur= 0
optimization costs time: 0.000355799 seconds.
pose estimated by g2o =
    0.99790590955  -0.0509194008911   0.0398874704367   -0.126782138956
  0.0498186625425    0.998362315744   0.0281209417542 -0.00843949681823
 -0.0412540488609  -0.0260749135293    0.998808391203   0.0603493574888
                0                 0                 0                 1
solve pnp by g2o cost time: 0.000477456 seconds.

6.pose_estimation_3d3d

sqq@sqq-X442URR:~/slambook2/ch7/build$ ./pose_estimation_3d3d 1.png 2.png 1_depth.png 2_depth.png
-- Max dist : 94.000000 
-- Min dist : 4.000000 
一共找到了79组匹配点
3d-3d pairs: 72
W=  10.871 -1.01948  2.54771
-2.16033  3.85307 -5.77742
 3.94738 -5.79979  9.62203
U=  0.558087  -0.829399 -0.0252034
 -0.428009  -0.313755   0.847565
  0.710878   0.462228   0.530093
V=  0.617887  -0.784771 -0.0484806
 -0.399894  -0.366747   0.839989
  0.676979   0.499631   0.540434
ICP via SVD results: 
R = [0.9969452349468715, 0.05983347698056557, -0.05020113095482046;
 -0.05932607657705309, 0.9981719679735133, 0.01153858699565957;
 0.05079975545906246, -0.008525103184062521, 0.9986724725659557]
t = [0.144159841091821;
 -0.06667849443812729;
 -0.03009747273569774]
R_inv = [0.9969452349468715, -0.05932607657705309, 0.05079975545906246;
 0.05983347698056557, 0.9981719679735133, -0.008525103184062521;
 -0.05020113095482046, 0.01153858699565957, 0.9986724725659557]
t_inv = [-0.1461462958593589;
 0.05767443542067568;
 0.03806388018483625]
calling bundle adjustment
iteration= 0	 chi2= 1.816112	 time= 3.1288e-05	 cumTime= 3.1288e-05	 edges= 72	 schur= 0	 lambda= 0.000758	 levenbergIter= 1
iteration= 1	 chi2= 1.815514	 time= 1.3137e-05	 cumTime= 4.4425e-05	 edges= 72	 schur= 0	 lambda= 0.000505	 levenbergIter= 1
iteration= 2	 chi2= 1.815514	 time= 1.1835e-05	 cumTime= 5.626e-05	 edges= 72	 schur= 0	 lambda= 0.000337	 levenbergIter= 1
iteration= 3	 chi2= 1.815514	 time= 1.1569e-05	 cumTime= 6.7829e-05	 edges= 72	 schur= 0	 lambda= 0.000225	 levenbergIter= 1
iteration= 4	 chi2= 1.815514	 time= 1.1585e-05	 cumTime= 7.9414e-05	 edges= 72	 schur= 0	 lambda= 0.000150	 levenbergIter= 1
iteration= 5	 chi2= 1.815514	 time= 1.167e-05	 cumTime= 9.1084e-05	 edges= 72	 schur= 0	 lambda= 0.000100	 levenbergIter= 1
iteration= 6	 chi2= 1.815514	 time= 1.1605e-05	 cumTime= 0.000102689	 edges= 72	 schur= 0	 lambda= 0.000067	 levenbergIter= 1
iteration= 7	 chi2= 1.815514	 time= 3.0473e-05	 cumTime= 0.000133162	 edges= 72	 schur= 0	 lambda= 4571263.861810	 levenbergIter= 8
optimization costs time: 0.000376194 seconds.

after optimization:
T=
  0.996945  0.0598335 -0.0502011    0.14416
-0.0593261   0.998172  0.0115386 -0.0666785
 0.0507998 -0.0085251   0.998672 -0.0300979
         0          0          0          1
p1 = [-0.0374123, -0.830816, 2.7448]
p2 = [-0.0111479, -0.746763, 2.7652]
(R*p2+t) = [-0.05045153727143989;
 -0.7795087351232992;
 2.737231009770682]

p1 = [-0.243698, -0.117719, 1.5848]
p2 = [-0.297211, -0.0956614, 1.6558]
(R*p2+t) = [-0.2409901494624399;
 -0.125427050612953;
 1.609221205035477]

p1 = [-0.627753, 0.160186, 1.3396]
p2 = [-0.709645, 0.159033, 1.4212]
(R*p2+t) = [-0.6251478077232009;
 0.1505624178119533;
 1.351809862712792]

p1 = [-0.323443, 0.104873, 1.4266]
p2 = [-0.399079, 0.12047, 1.4838]
(R*p2+t) = [-0.3209806217783837;
 0.09436777718271659;
 1.430432194554881]

p1 = [-0.627221, 0.101454, 1.3116]
p2 = [-0.709709, 0.100216, 1.3998]
(R*p2+t) = [-0.6276559234155774;
 0.09161021993899245;
 1.330936372911853]

ch8

##optical_flow

qq@sqq-X442URR:~/slambook2/ch8/build$ ./optical_flow 
build pyramid time: 0.000236402
track pyr 3 cost time: 0.000693532
track pyr 2 cost time: 0.000558222
track pyr 1 cost time: 0.000541002
track pyr 0 cost time: 0.000530893
optical flow by gauss-newton: 0.00320991
optical flow by opencv: 0.00288701