Intersection extraction test

[TypingCat]

This test is a extension of issue #6.

Previous test shows the performance of the topology extraction computation. On the other hand, the concern of this issue is whether to extract intersections well. Following is the dataset to be used for testing. My robot was moving through the hall for a minute to collect the topology. There are 5 intersections.

[TypingCat]

Here is the result of intersection position mean-shift clustering. There are 10 clusters and it took 1 second. Clustering bandwidth is 1m which is the largest number that can divide clusters 1 and 2. The cluster number is in descending order of the number of samples. Major intersections(=cluster 0~4) have more than 60 samples.

Time elapsed: 0.969153881072998
Cluster 0
Number of Samples: 118
Mean: [12.162206171569066, -6.985476384762426]
Components: [0.08788653899477596, 0.9961304915840694], [-0.9961304915840694, 0.08788653899477596]
Std.: 0.49539496507154274, 0.3013853502975086
Cluster 1
Number of Samples: 112
Mean: [12.105148123882584, -2.504276032765694]
Components: [-0.017863528018902692, -0.9998404344527769], [0.9998404344527769, -0.017863528018902692]
Std.: 0.4298490766200242, 0.1411558187405476
Cluster 2
Number of Samples: 96
Mean: [11.913007303095158, -0.7884197346327589]
Components: [-0.18487514311396774, -0.9827620166950848], [0.9827620166950848, -0.18487514311396774]
Std.: 0.2819592361817285, 0.0894635422792863
Cluster 3
Number of Samples: 68
Mean: [3.837165986246329, -1.5040044973646463]
Components: [0.9852925500558137, 0.17087595151018645], [0.17087595151018645, -0.9852925500558137]
Std.: 0.26953984833392863, 0.07810006195569245
Cluster 4
Number of Samples: 62
Mean: [13.179703771382588, -9.581389470763307]
Components: [-0.43253737359423383, 0.9016160049844956], [-0.9016160049844956, -0.43253737359423383]
Std.: 0.2121673209983509, 0.02948882486403218
Cluster 5
Number of Samples: 13
Mean: [13.296898908608638, -11.20788390672979]
Components: [-0.023890947775154218, 0.9997145705722232], [0.9997145705722232, 0.023890947775154218]
Std.: 0.2405439217671509, 0.05205092076615887
Cluster 6
Number of Samples: 6
Mean: [10.807487528563756, -0.5451837343606661]
Components: [0.9992960935868507, -0.03751422852811913], [0.03751422852811913, 0.9992960935868507]
Std.: 0.3193383281168898, 0.014868637519609417
Cluster 7
Number of Samples: 9
Mean: [6.134500952695734, -1.0894938827001246]
Components: [-0.9999949296423528, -0.00318444494158222], [0.00318444494158222, -0.9999949296423528]
Std.: 0.3828290385561136, 0.10647975815402723
Cluster 8
Number of Samples: 6
Mean: [9.318208722346839, -0.7054591825059205]
Components: [0.9939561931027033, 0.10977743936156228], [-0.10977743936156228, 0.9939561931027033]
Std.: 0.5286330364572214, 0.026444277001977663
Cluster 9
Number of Samples: 2
Mean: [14.541147064965411, -3.0883532250298558]
Components: [-0.3666422917354909, 0.9303619886425645], [0.9303619886425645, 0.3666422917354909]
Std.: 0.16724267084639383, 1.1702823029974678e-16

[TypingCat]

Here is the result of merging resampled local topology. See node (12.7, -1.6) below. It should be connected with node (12.2, -0.6) rather than node (13.0, -0.4). I think geometric distance is not enough to decide the closest node. If this assumption is correct, node (11.7, -6.1) has the same problem. One comforting thing is that this operation is fast(0.005s for last iteration).