PythonRobotics机器人算法库-迭代最近点(ICP)匹配

说明：

• 介绍如何计算点到点之间的旋转矩阵和平移向量

• 这是具有奇异值分解的 2D ICP 匹配示例

• 参考代码

步骤：

• 进入目录PythonRobotics/SLAM/iterative_closest_point

• 执行文件

python3 iterative_closest_point.py

• 结果如下

iterative_closest_point.py start!!
Residual: 3796.431392683221
Residual: 3661.5073048852523
Residual: 3537.149820442701
Residual: 3409.175476274257
Residual: 3290.238875556988
Residual: 3180.855196296639
Residual: 3085.821589721196
Residual: 3012.091807136007
Residual: 2947.59335116456
Residual: 2879.6241130397584
Residual: 2811.3354433941986
Residual: 2749.7686452899893
Residual: 2698.0427257209026
Residual: 2639.2197660571705
Residual: 2559.9314999871985
Residual: 2476.7246600035865
Residual: 2406.8510306938238
Residual: 2351.3613000010278
Residual: 2283.2843055680587
Residual: 2213.131545626712
Residual: 2130.6681391192387
Residual: 2055.266508824357
Residual: 1957.3055490416539
Residual: 1860.0127137621964
Residual: 1738.6849226154015
Residual: 1622.7684751159795
Residual: 1492.2976539864183
Residual: 1361.3376741363975
Residual: 1232.860645900511
Residual: 1084.0878491281085
Residual: 919.6615830086902
Residual: 721.1557717366784
Residual: 480.85119664003236
Residual: 198.6820953619024
Residual: 15.709500630014057
Residual: 3.9570652730095064e-11
Residual: 3.9475617084547015e-11
Converge 3.9475617084547015e-11 9.50356455480496e-14 37
R: [[ 0.98480775 -0.17364818]
 [ 0.17364818  0.98480775]]
T: [-0.13155574 -2.03590682]
Residual: 3824.3075284732317
Residual: 3688.103732933912
Residual: 3541.2468428312454
Residual: 3411.789577424408
Residual: 3291.095094245945
Residual: 3195.6843062201597
Residual: 3099.3474018613397
Residual: 3029.9160673887804
Residual: 2967.043809511793
Residual: 2909.3069468298277
Residual: 2845.2808882932804
Residual: 2783.8397838951364
Residual: 2707.853888703722
Residual: 2628.4355396944097
Residual: 2540.635577637876
Residual: 2445.5824238412747
Residual: 2340.2278238045405
Residual: 2221.8291506359483
Residual: 2123.3736656658507
Residual: 2022.4947412236736
Residual: 1943.7435594476249
Residual: 1874.2588536814965
Residual: 1818.3577720468115
Residual: 1767.4279503778296
Residual: 1726.086652752472
Residual: 1676.4360624518888
Residual: 1616.934336566566
Residual: 1546.3100176500352
Residual: 1465.9178166405127
Residual: 1374.910456441421
Residual: 1284.463960799184
Residual: 1195.7274635019965
Residual: 1056.3354889047414
Residual: 887.3772342230761
Residual: 648.3227383992139
Residual: 352.68104263561725
Residual: 86.04589795751752
Residual: 1.6299257959011682
Residual: 2.765317883742113e-11
Residual: 2.4048736444124342e-11
Converge 2.4048736444124342e-11 3.604442393296788e-12 40
R: [[ 0.98480775 -0.17364818]
 [ 0.17364818  0.98480775]]
T: [-0.25244799 -2.17257865]
Residual: 3803.365827551035
Residual: 3701.8677662567757
Residual: 3592.207751990907
Residual: 3499.9449032620073
Residual: 3421.9848483905307
Residual: 3338.5296287812284
Residual: 3258.1577687255262
Residual: 3148.3616482682633
Residual: 3058.9798655662803
Residual: 2983.7769765540784
Residual: 2901.174765448263
Residual: 2826.75902022557
Residual: 2762.497678090809
Residual: 2678.86604490383
Residual: 2608.8204776784437
Residual: 2544.1284810685943
Residual: 2465.7324715356135
Residual: 2395.9228289603475
Residual: 2340.896316100333
Residual: 2280.1907520807044
Residual: 2211.707825717245
Residual: 2119.982214293351
Residual: 2044.8439205791901
Residual: 1965.4192165727152
Residual: 1882.6754264326596
Residual: 1799.2334609378906
Residual: 1732.78914343378
Residual: 1654.9403268267458
Residual: 1549.322561361585
Residual: 1411.3064762211065
Residual: 1282.0578138628214
Residual: 1137.4533855844652
Residual: 954.6087055221706
Residual: 747.8682873514982
Residual: 478.94868008808106
Residual: 183.34475135665969
Residual: 15.214735335435758
Residual: 0.0033766359022544628
Residual: 1.1597580926588071e-11
Residual: 1.3699932321121167e-11
Not Converge... 1.1597580926588071e-11 -2.1023513945330953e-12 40
R: [[ 0.98480775 -0.17364818]
 [ 0.17364818  0.98480775]]
T: [-0.18477227 -2.05577711]
iterative_closest_point.py start!!
Residual: 6164.47425687834
Residual: 5454.707437916643
Residual: 4802.937169160898
Residual: 4174.86479211435
Residual: 3534.252427539319
Residual: 2868.9696562299036
Residual: 1943.1317345929867
Residual: 830.0935793021998
Residual: 41.847355722980545
Residual: 2.1249501348206016e-11
Residual: 2.2644983295690392e-11
Not Converge... 2.1249501348206016e-11 -1.3954819474843765e-12 11
R: [[ 0.98485903 -0.0031611  -0.17332832]
 [ 0.00252963  0.99998934 -0.00386399]
 [ 0.17333869  0.00336703  0.98485652]]
T: [-1.16003844 -1.92961226  4.90936273]
Residual: 6150.976669756038
Residual: 5516.419244185019
Residual: 4913.524665814191
Residual: 4301.354188296855
Residual: 3670.230778281458
Residual: 2954.6586339978767
Residual: 2051.974372087842
Residual: 1024.6997555105545
Residual: 120.49552541509236
Residual: 1.0582956789399554e-11
Residual: 1.4327000583505554e-11
Not Converge... 1.0582956789399554e-11 -3.744043794106e-12 11
R: [[ 9.84786883e-01 -1.20719340e-03 -1.73762301e-01]
 [-4.18440849e-04  9.99956494e-01 -9.31856872e-03]
 [ 1.73765990e-01  9.24951349e-03  9.84743534e-01]]
T: [-1.06798807 -2.05883331  4.85195382]
Residual: 6200.011970479581
Residual: 5478.255330678545
Residual: 4802.9014817155885
Residual: 4203.499449812523
Residual: 3664.0824807187023
Residual: 2976.6209497500317
Residual: 2064.481584314095
Residual: 932.9547934471335
Residual: 77.39996793925772
Residual: 1.9904496809368614e-11
Residual: 1.9930937600739775e-11
Not Converge... 1.9904496809368614e-11 -2.644079137116133e-14 11
R: [[ 0.98482223  0.00317279 -0.17353703]
 [-0.00409984  0.99997918 -0.00498391]
 [ 0.1735176   0.00561974  0.98481473]]
T: [-1.12509652 -2.14321974  4.81299524]