[slam14讲] 第七讲视觉里程计

理论

特征点提取与匹配

对极约束

三角测距

PnP

ICP

---

实践

特征点提取

特征点匹配

对极约束(2d-2d)

三角化测距

PnP(3d-2d)

EPnP求解位姿

BA优化

➜  build git:(master) ✗ ./pose_estimation_3d2d  ../1.png ../2.png ../1_depth.png 
[ INFO:0] Initialize OpenCL runtime...
-- Max dist : 95.000000 
-- Min dist : 7.000000 
一共找到了81组匹配点
3d-2d pairs: 77
R=
[0.9979193252225089, -0.05138618904650331, 0.03894200717386666;
 0.05033852907733834, 0.9983556574295412, 0.02742286944793203;
 -0.04028712992734059, -0.02540552801469367, 0.9988651091656532]
t=
[-0.1255867099750398;
 -0.007363525258777434;
 0.06099926588678889]
calling bundle adjustment
iteration= 0	 chi2= 0.001292	 time= 9.5219e-05	 cumTime= 9.5219e-05	 edges= 77	 schur= 1	 lambda= 80.003723	 levenbergIter= 1
iteration= 1	 chi2= 0.000000	 time= 2.5681e-05	 cumTime= 0.0001209	 edges= 77	 schur= 1	 lambda= 53.335815	 levenbergIter= 1
iteration= 2	 chi2= 0.000000	 time= 2.4944e-05	 cumTime= 0.000145844	 edges= 77	 schur= 1	 lambda= 35.557210	 levenbergIter= 1
iteration= 3	 chi2= 0.000000	 time= 2.3343e-05	 cumTime= 0.000169187	 edges= 77	 schur= 1	 lambda= 23.704807	 levenbergIter= 1
iteration= 4	 chi2= 0.000000	 time= 2.3144e-05	 cumTime= 0.000192331	 edges= 77	 schur= 1	 lambda= 15.803205	 levenbergIter= 1
iteration= 5	 chi2= 0.000000	 time= 2.3012e-05	 cumTime= 0.000215343	 edges= 77	 schur= 1	 lambda= 10.535470	 levenbergIter= 1
iteration= 6	 chi2= 0.000000	 time= 7.1043e-05	 cumTime= 0.000286386	 edges= 77	 schur= 1	 lambda= 230150.846755	 levenbergIter= 6
iteration= 7	 chi2= 0.000000	 time= 5.1523e-05	 cumTime= 0.000337909	 edges= 77	 schur= 1	 lambda= 235674467.076671	 levenbergIter= 4
optimization costs time: 0.00060238 seconds.

after optimization:
T=
   0.997841  -0.0518393   0.0403291   -0.127516
  0.0507013      0.9983    0.028745 -0.00947167
 -0.0417507  -0.0266382    0.998773   0.0595037
          0           0           0           1

ICP(3d-3d)

SVD求解位姿

非线性优化

➜  build git:(master) ✗ ./pose_estimation_3d3d ../1.png ../2.png ../1_depth.png ../2_depth.png
[ INFO:0] Initialize OpenCL runtime...
-- Max dist : 95.000000 
-- Min dist : 7.000000 
一共找到了81组匹配点
3d-3d pairs: 75
W=  11.8688 -0.717698   1.89486
 -1.88065   3.83391  -5.78219
  3.25846  -5.82734   9.65267
U=  0.592295  -0.805658 -0.0101195
 -0.418171  -0.318113   0.850845
  0.688709   0.499719   0.525319
V=   0.64736  -0.761401 -0.0345329
 -0.388765  -0.368829   0.844291
  0.655581   0.533135   0.534772
ICP via SVD results: 
R = [0.9972065647956201, 0.05834273442898391, -0.04663895869192625;
 -0.05787745545449197, 0.998260122172866, 0.01126626067193237;
 0.04721511705620757, -0.008535444848613793, 0.9988482762174666]
t = [0.1379879629890433;
 -0.06551699470729988;
 -0.02981649388290575]
R_inv = [0.9972065647956201, -0.05787745545449197, 0.04721511705620757;
 0.05834273442898391, 0.998260122172866, -0.008535444848613793;
 -0.04663895869192625, 0.01126626067193237, 0.9988482762174666]
t_inv = [-0.1399866702492459;
 0.05709791102272541;
 0.03695589996443214]
calling bundle adjustment
iteration= 0	 chi2= 18157.747747	 time= 3.6268e-05	 cumTime= 3.6268e-05	 edges= 75	 schur= 0
iteration= 1	 chi2= 18151.933202	 time= 1.0828e-05	 cumTime= 4.7096e-05	 edges= 75	 schur= 0
iteration= 2	 chi2= 18151.932131	 time= 9.943e-06	 cumTime= 5.7039e-05	 edges= 75	 schur= 0
iteration= 3	 chi2= 18151.932130	 time= 1.0684e-05	 cumTime= 6.7723e-05	 edges= 75	 schur= 0
iteration= 4	 chi2= 18151.932130	 time= 9.21e-06	 cumTime= 7.6933e-05	 edges= 75	 schur= 0
iteration= 5	 chi2= 18151.932130	 time= 9.216e-06	 cumTime= 8.6149e-05	 edges= 75	 schur= 0
iteration= 6	 chi2= 18151.932130	 time= 9.213e-06	 cumTime= 9.5362e-05	 edges= 75	 schur= 0
iteration= 7	 chi2= 18151.932130	 time= 9.273e-06	 cumTime= 0.000104635	 edges= 75	 schur= 0
iteration= 8	 chi2= 18151.932130	 time= 9.206e-06	 cumTime= 0.000113841	 edges= 75	 schur= 0
iteration= 9	 chi2= 18151.932130	 time= 9.251e-06	 cumTime= 0.000123092	 edges= 75	 schur= 0
optimization costs time: 0.000322155 seconds.

after optimization:
T=
   0.997207   0.0583427   -0.046639    0.137988
 -0.0578775     0.99826   0.0112663   -0.065517
  0.0472151 -0.00853546    0.998848  -0.0298169
          0           0           0           1
p1 = [-0.0374123, -0.830816, 2.7448]
p2 = [-0.0111479, -0.746763, 2.7652]
(R*p2+t) = [-0.04566300488482969;
 -0.7791822151698653;
 2.738046267661636]

p1 = [-0.243698, -0.117719, 1.5848]
p2 = [-0.299118, -0.0975683, 1.6558]
(R*p2+t) = [-0.243212054430598;
 -0.1269486029625337;
 1.610786345002579]

p1 = [-0.627753, 0.160186, 1.3396]
p2 = [-0.709645, 0.159033, 1.4212]
(R*p2+t) = [-0.6266796777024644;
 0.1503229238263245;
 1.354883323538178]

p1 = [-0.323443, 0.104873, 1.4266]
p2 = [-0.399079, 0.12047, 1.4838]
(R*p2+t) = [-0.3221508525590339;
 0.09455772952719482;
 1.432403794814274]

p1 = [-0.627221, 0.101454, 1.3116]
p2 = [-0.709709, 0.100216, 1.3998]
(R*p2+t) = [-0.6291763602679332;
 0.09137127679150184;
 1.334006907588644]

参考

pose_estimation_3d3d