ntd_mapping log

method_type: 2
use_odom: 0
use_imu: 0
imu_upside_down: 0
imu_topic: imu_raw
incremental_voxel_update: 0
(tf_x,tf_y,tf_z,tf_roll,tf_pitch,tf_yaw): (0, 0, 0, 0, 0, 0)
-----------------------------------------------------------------
Sequence number: 4930
Number of scan points: 6294 points.
Number of filtered scan points: 722 points.
transformed_scan_ptr: 6294 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.2114
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.00378692, 0.0164724, -0.000223981, -0.00047122, -0.00104701, -0.00415465)
Transformation Matrix:
    0.999991   0.00415513  -0.00104504  -0.00378692
 -0.00415463     0.999991  0.000475569    0.0164724
  0.00104701 -0.000471219     0.999999 -0.000223981
           0            0            0            1
shift: 0.016902
-----------------------------------------------------------------
(Processed/Input): (1 / 6)
-----------------------------------------------------------------
Sequence number: 4931
Number of scan points: 6294 points.
Number of filtered scan points: 721 points.
transformed_scan_ptr: 6294 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.2235
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.00761295, 0.0219133, 0.000314394, -0.000697163, -0.00104286, -0.00424208)
Transformation Matrix:
     0.99999   0.00424279   -0.0010399  -0.00761295
 -0.00424207     0.999991  0.000701576    0.0219133
  0.00104286 -0.000697163     0.999999  0.000314394
           0            0            0            1
shift: 0.023198
-----------------------------------------------------------------
(Processed/Input): (2 / 6)
-----------------------------------------------------------------
Sequence number: 4932
Number of scan points: 6294 points.
Number of filtered scan points: 721 points.
transformed_scan_ptr: 6294 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.2187
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0141849, 0.012398, -0.00130411, -0.000308826, -0.000888515, -0.00465285)
Transformation Matrix:
    0.999989   0.00465311 -0.000887068   -0.0141849
 -0.00465283     0.999989  0.000312956     0.012398
 0.000888515 -0.000308826            1  -0.00130411
           0            0            0            1
shift: 0.0188393
-----------------------------------------------------------------
(Processed/Input): (3 / 6)
-----------------------------------------------------------------
Sequence number: 4933
Number of scan points: 6294 points.
Number of filtered scan points: 720 points.
transformed_scan_ptr: 6294 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.2173
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0157019, 0.0194076, -0.000536648, -0.000326968, -0.00110621, -0.00371196)
Transformation Matrix:
    0.999992   0.00371232  -0.00110498   -0.0157019
 -0.00371195     0.999993   0.00033108    0.0194076
  0.00110621 -0.000326968     0.999999 -0.000536648
           0            0            0            1
shift: 0.0249641
-----------------------------------------------------------------
(Processed/Input): (4 / 6)
-----------------------------------------------------------------
Sequence number: 4934
Number of scan points: 6294 points.
Number of filtered scan points: 720 points.
transformed_scan_ptr: 6294 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.1739
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.00804324, 0.0235166, 0.000151243, -0.000252284, -0.0011184, -0.00308572)
Transformation Matrix:
    0.999995     0.003086  -0.00111762  -0.00804324
 -0.00308571     0.999995  0.000255737    0.0235166
   0.0011184 -0.000252284     0.999999  0.000151243
           0            0            0            1
shift: 0.0248541
-----------------------------------------------------------------
(Processed/Input): (5 / 6)
-----------------------------------------------------------------
Sequence number: 4935
Number of scan points: 6294 points.
Number of filtered scan points: 720 points.
transformed_scan_ptr: 6294 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.2136
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0200421, 0.0201578, -0.000512306, -0.000326471, -0.00110614, -0.00351432)
Transformation Matrix:
    0.999993   0.00351467  -0.00110498   -0.0200421
 -0.00351431     0.999994  0.000330364    0.0201578
  0.00110614 -0.000326471     0.999999 -0.000512306
           0            0            0            1
shift: 0.0284258
-----------------------------------------------------------------
(Processed/Input): (6 / 6)
-----------------------------------------------------------------
Sequence number: 4936
Number of scan points: 6278 points.
Number of filtered scan points: 719 points.
transformed_scan_ptr: 6278 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.2679
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0148786, 0.015239, -0.000179254, -0.000286315, -0.00212471, -0.00333835)
Transformation Matrix:
    0.999992   0.00333895  -0.00212374   -0.0148786
 -0.00333834     0.999994  0.000293404     0.015239
  0.00212471 -0.000286315     0.999998 -0.000179254
           0            0            0            1
shift: 0.0212979
-----------------------------------------------------------------
(Processed/Input): (7 / 7)
-----------------------------------------------------------------
Sequence number: 4937
Number of scan points: 6245 points.
Number of filtered scan points: 719 points.
transformed_scan_ptr: 6245 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.9591
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0211869, 0.0205668, -0.00108052, -0.000390523, -0.00368379, -0.00554739)
Transformation Matrix:
   0.999978   0.0055488 -0.00368156  -0.0211869
-0.00554733    0.999985 0.000410934   0.0205668
 0.00368379 -0.00039052    0.999993 -0.00108052
          0           0           0           1
shift: 0.0295276
-----------------------------------------------------------------
(Processed/Input): (8 / 8)
-----------------------------------------------------------------
Sequence number: 4938
Number of scan points: 6220 points.
Number of filtered scan points: 733 points.
transformed_scan_ptr: 6220 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.0271
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.00227749, 0.0160105, -0.00056129, -0.000546168, -0.0046666, -0.00280137)
Transformation Matrix:
    0.999985   0.00280392  -0.00466503  -0.00227749
 -0.00280134     0.999996  0.000559235    0.0160105
  0.00466658 -0.000546162     0.999989  -0.00056129
           0            0            0            1
shift: 0.0161717
-----------------------------------------------------------------
(Processed/Input): (9 / 9)
-----------------------------------------------------------------
Sequence number: 4939
Number of scan points: 6268 points.
Number of filtered scan points: 718 points.
transformed_scan_ptr: 6268 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.8963
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0302971, 0.0110519, -0.00141446, -0.000300501, -0.00428942, -0.00117926)
Transformation Matrix:
     0.99999   0.00118055  -0.00428905    0.0302971
 -0.00117925     0.999999  0.000305566    0.0110519
  0.00428941 -0.000300498     0.999991  -0.00141446
           0            0            0            1
shift: 0.0322499
-----------------------------------------------------------------
(Processed/Input): (10 / 10)
-----------------------------------------------------------------
Sequence number: 4940
Number of scan points: 6303 points.
Number of filtered scan points: 745 points.
transformed_scan_ptr: 6303 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.0083
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0806567, 0.0113927, 0.00196571, -0.00238782, -0.00496345, -0.00164691)
Transformation Matrix:
   0.999986  0.00165874 -0.00495948   0.0806567
-0.00164689    0.999996  0.00239598   0.0113927
 0.00496343 -0.00238779    0.999985  0.00196571
          0           0           0           1
shift: 0.0814573
-----------------------------------------------------------------
(Processed/Input): (11 / 11)
-----------------------------------------------------------------
Sequence number: 4941
Number of scan points: 6311 points.
Number of filtered scan points: 746 points.
transformed_scan_ptr: 6311 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.608
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0934048, 0.0243031, 0.00329029, -0.00253165, -0.0047979, -0.0019888)
Transformation Matrix:
   0.999987  0.00200093 -0.00479282   0.0934048
-0.00198878    0.999995  0.00254118   0.0243031
 0.00479788 -0.00253162    0.999985  0.00329029
          0           0           0           1
shift: 0.0965148
-----------------------------------------------------------------
(Processed/Input): (12 / 12)
-----------------------------------------------------------------
Sequence number: 4942
Number of scan points: 6345 points.
Number of filtered scan points: 738 points.
transformed_scan_ptr: 6345 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.8523
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0882098, 0.0211448, 0.00576862, -0.00363352, -0.00423749, -0.00233246)
Transformation Matrix:
   0.999988  0.00234783 -0.00422897   0.0882098
-0.00233244    0.999991  0.00364338   0.0211448
 0.00423748 -0.00363348    0.999984  0.00576862
          0           0           0           1
shift: 0.0907087
-----------------------------------------------------------------
(Processed/Input): (13 / 13)
-----------------------------------------------------------------
Sequence number: 4943
Number of scan points: 6396 points.
Number of filtered scan points: 733 points.
transformed_scan_ptr: 6396 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.9805
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0374861, 0.0299794, 0.00507022, -0.00416566, -0.00448529, -0.00353341)
Transformation Matrix:
   0.999984  0.00355208 -0.00447048   0.0374861
-0.00353337    0.999985  0.00418149   0.0299794
 0.00448528 -0.00416561    0.999981  0.00507022
          0           0           0           1
shift: 0.0479997
-----------------------------------------------------------------
(Processed/Input): (14 / 14)
-----------------------------------------------------------------
Sequence number: 4944
Number of scan points: 6420 points.
Number of filtered scan points: 747 points.
transformed_scan_ptr: 6420 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.5403
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0138763, 0.0166671, 0.00605405, -0.00416391, -0.00508014, -0.00153586)
Transformation Matrix:
   0.999986    0.001557 -0.00507368   0.0138763
-0.00153584     0.99999  0.00417169   0.0166671
 0.00508012 -0.00416384    0.999978  0.00605405
          0           0           0           1
shift: 0.0216874
-----------------------------------------------------------------
(Processed/Input): (15 / 15)
-----------------------------------------------------------------
Sequence number: 4945
Number of scan points: 6436 points.
Number of filtered scan points: 758 points.
transformed_scan_ptr: 6436 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.677
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.015063, 0.00615291, 0.00685984, -0.00288157, -0.00364399, -0.00415332)
Transformation Matrix:
   0.999985  0.00416379 -0.00363198    0.015063
-0.00415328    0.999987  0.00289667  0.00615291
 0.00364399 -0.00288155    0.999989  0.00685984
          0           0           0           1
shift: 0.0162712
-----------------------------------------------------------------
(Processed/Input): (16 / 16)
-----------------------------------------------------------------
Sequence number: 4946
Number of scan points: 6602 points.
Number of filtered scan points: 752 points.
transformed_scan_ptr: 6602 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 22.9959
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0276875, -0.00139905, 0.00806492, -0.00379383, -0.00146751, -0.00197656)
Transformation Matrix:
   0.999997  0.00198213 -0.00145998   0.0276875
-0.00197656    0.999991  0.00379672 -0.00139905
 0.00146751 -0.00379381    0.999992  0.00806492
          0           0           0           1
shift: 0.0277229
-----------------------------------------------------------------
(Processed/Input): (17 / 17)
-----------------------------------------------------------------
Sequence number: 4947
Number of scan points: 6602 points.
Number of filtered scan points: 759 points.
transformed_scan_ptr: 6602 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.4046
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0183337, 0.00405092, 0.00437204, -0.002592, -0.00151861, -0.00503991)
Transformation Matrix:
   0.999986  0.00504381 -0.00150552   0.0183337
-0.00503988    0.999984  0.00259962  0.00405092
 0.00151861   -0.002592    0.999995  0.00437204
          0           0           0           1
shift: 0.0187759
-----------------------------------------------------------------
(Processed/Input): (18 / 18)
-----------------------------------------------------------------
Sequence number: 4948
Number of scan points: 6666 points.
Number of filtered scan points: 775 points.
transformed_scan_ptr: 6666 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.678
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0155032, -0.012682, 0.000432467, -0.0017729, -0.00168608, -0.00205089)
Transformation Matrix:
   0.999996  0.00205389 -0.00168242   0.0155032
-0.00205089    0.999996  0.00177638   -0.012682
 0.00168608  -0.0017729    0.999997 0.000432467
          0           0           0           1
shift: 0.0200295
-----------------------------------------------------------------
(Processed/Input): (19 / 19)
-----------------------------------------------------------------
Sequence number: 4949
Number of scan points: 6694 points.
Number of filtered scan points: 765 points.
transformed_scan_ptr: 6694 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 22.9958
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0313035, -0.00436905, 0.0023816, -0.00351089, -0.00271923, -0.000512669)
Transformation Matrix:
    0.999996  0.000522232   -0.0027174    0.0313035
-0.000512667     0.999994   0.00351228  -0.00436905
  0.00271922  -0.00351087      0.99999    0.0023816
           0            0            0            1
shift: 0.031607
-----------------------------------------------------------------
(Processed/Input): (20 / 20)
-----------------------------------------------------------------
Sequence number: 4950
Number of scan points: 6747 points.
Number of filtered scan points: 788 points.
transformed_scan_ptr: 6747 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 22.6316
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0209151, 0.0128721, 0.00273712, -0.00476761, -0.00115091, 0.00107209)
Transformation Matrix:
   0.999999  -0.0010666   -0.001156   0.0209151
 0.00107209    0.999988  0.00476636   0.0128721
 0.00115091 -0.00476759    0.999988  0.00273712
          0           0           0           1
shift: 0.0245588
-----------------------------------------------------------------
(Processed/Input): (21 / 21)
-----------------------------------------------------------------
Sequence number: 4951
Number of scan points: 6775 points.
Number of filtered scan points: 780 points.
transformed_scan_ptr: 6775 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.2085
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.0507711, 0.00203508, 0.000321334, -0.00527809, -0.00114955, -0.0127606)
Transformation Matrix:
   0.999918   0.0127661  -0.0010821  -0.0507711
 -0.0127602    0.999905   0.0052923  0.00203508
 0.00114955 -0.00527806    0.999985 0.000321334
          0           0           0           1
shift: 0.0508118
-----------------------------------------------------------------
(Processed/Input): (22 / 22)
-----------------------------------------------------------------
Sequence number: 4952
Number of scan points: 6843 points.
Number of filtered scan points: 800 points.
transformed_scan_ptr: 6843 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.3348
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.0261301, -0.040477, 5.92529e-06, -0.00594404, 0.00074277, -0.0146938)
Transformation Matrix:
   0.999892   0.0146886 0.000830031  -0.0261301
 -0.0146933    0.999874  0.00593244   -0.040477
-0.00074277   -0.005944    0.999982 5.92529e-06
          0           0           0           1
shift: 0.0481785
-----------------------------------------------------------------
(Processed/Input): (23 / 23)
-----------------------------------------------------------------
Sequence number: 4953
Number of scan points: 6902 points.
Number of filtered scan points: 799 points.
transformed_scan_ptr: 6902 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.9076
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0086859, -0.0496417, -0.00339407, -0.00489111, 0.000360717, -0.012859)
Transformation Matrix:
    0.999917    0.0128568  0.000423589    0.0086859
  -0.0128587     0.999905   0.00488604   -0.0496417
-0.000360717  -0.00489109     0.999988  -0.00339407
           0            0            0            1
shift: 0.0503959
-----------------------------------------------------------------
(Processed/Input): (24 / 24)
-----------------------------------------------------------------
Sequence number: 4954
Number of scan points: 7013 points.
Number of filtered scan points: 822 points.
transformed_scan_ptr: 7013 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.9787
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0390956, -0.016512, -0.0034589, -0.0066735, -0.000852138, -0.00541789)
Transformation Matrix:
    0.999985   0.00542343 -0.000815961    0.0390956
 -0.00541786     0.999963   0.00667796    -0.016512
 0.000852138  -0.00667344     0.999977   -0.0034589
           0            0            0            1
shift: 0.0424395
-----------------------------------------------------------------
(Processed/Input): (25 / 25)
-----------------------------------------------------------------
Sequence number: 4955
Number of scan points: 6969 points.
Number of filtered scan points: 830 points.
transformed_scan_ptr: 6969 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.7828
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.012103, -0.019611, -0.00288442, -0.00805956, -0.00235118, -0.00525107)
Transformation Matrix:
   0.999983  0.00526981 -0.00230877    0.012103
-0.00525103    0.999954   0.0080717   -0.019611
 0.00235118 -0.00805945    0.999965 -0.00288442
          0           0           0           1
shift: 0.0230451
-----------------------------------------------------------------
(Processed/Input): (26 / 26)
-----------------------------------------------------------------
Sequence number: 4956
Number of scan points: 7057 points.
Number of filtered scan points: 838 points.
transformed_scan_ptr: 7057 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.2812
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.00937515, -0.00575047, -0.00709405, -0.0095277, -0.00286373, -0.00515607)
Transformation Matrix:
   0.999983  0.00518308 -0.00281445 -0.00937515
-0.00515602    0.999941  0.00954219 -0.00575047
 0.00286372 -0.00952752    0.999951 -0.00709405
          0           0           0           1
shift: 0.0109982
-----------------------------------------------------------------
(Processed/Input): (27 / 27)
-----------------------------------------------------------------
Sequence number: 4957
Number of scan points: 7137 points.
Number of filtered scan points: 846 points.
transformed_scan_ptr: 7137 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 22.803
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0336586, 0.00732984, -0.00154852, -0.0145293, -0.00607615, -0.0134495)
Transformation Matrix:
   0.999891    0.013536 -0.00587951  -0.0336586
 -0.0134489    0.999803   0.0146092  0.00732984
 0.00607612  -0.0145285    0.999876 -0.00154852
          0           0           0           1
shift: 0.0344474
-----------------------------------------------------------------
(Processed/Input): (28 / 28)
-----------------------------------------------------------------
Sequence number: 4958
Number of scan points: 7117 points.
Number of filtered scan points: 853 points.
transformed_scan_ptr: 7117 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.0311
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.036443, -0.00320715, -0.00286627, -0.0188436, -0.00683064, -0.00361707)
Transformation Matrix:
    0.99997  0.00374514 -0.00676117   -0.036443
-0.00361698    0.999815    0.018867 -0.00320715
 0.00683059   -0.018842    0.999799 -0.00286627
          0           0           0           1
shift: 0.0365839
-----------------------------------------------------------------
(Processed/Input): (29 / 29)
-----------------------------------------------------------------
Sequence number: 4959
Number of scan points: 7205 points.
Number of filtered scan points: 878 points.
transformed_scan_ptr: 7205 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.2081
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0130067, 0.0314802, -0.00391934, -0.0202981, -0.00680795, -0.00356997)
Transformation Matrix:
    0.99997  0.00370741 -0.00673399   0.0130067
-0.00356988    0.999787   0.0203208   0.0314802
  0.0068079  -0.0202962    0.999771 -0.00391934
          0           0           0           1
shift: 0.0340614
-----------------------------------------------------------------
(Processed/Input): (30 / 30)
-----------------------------------------------------------------
Sequence number: 4960
Number of scan points: 7235 points.
Number of filtered scan points: 911 points.
transformed_scan_ptr: 7235 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.3429
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.00680624, 0.0326962, -0.000544223, -0.0220987, -0.00723061, -0.00473637)
Transformation Matrix:
    0.999963   0.00489499  -0.00712403  -0.00680624
 -0.00473623     0.999744    0.0221309    0.0326962
  0.00723055   -0.0220963      0.99973 -0.000544223
           0            0            0            1
shift: 0.0333971
-----------------------------------------------------------------
(Processed/Input): (31 / 31)
-----------------------------------------------------------------
Sequence number: 4961
Number of scan points: 7248 points.
Number of filtered scan points: 930 points.
transformed_scan_ptr: 7248 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.0743
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.00217275, 0.0201667, 0.000349513, -0.0242102, -0.00711427, -0.0127205)
Transformation Matrix:
   0.999894   0.0128886  -0.0068036 -0.00217275
 -0.0127198    0.999624   0.0242963   0.0201667
 0.00711421  -0.0242072    0.999682 0.000349513
          0           0           0           1
shift: 0.0202834
-----------------------------------------------------------------
(Processed/Input): (32 / 32)
-----------------------------------------------------------------
Sequence number: 4962
Number of scan points: 7287 points.
Number of filtered scan points: 954 points.
transformed_scan_ptr: 7287 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.9756
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.00789677, 0.0312003, 0.00216979, -0.0260461, -0.00779711, -0.0116122)
Transformation Matrix:
   0.999902   0.0118111 -0.00749141  0.00789677
 -0.0116116    0.999591   0.0261319   0.0312003
 0.00779703  -0.0260423     0.99963  0.00216979
          0           0           0           1
shift: 0.0321841
-----------------------------------------------------------------
(Processed/Input): (33 / 33)
-----------------------------------------------------------------
Sequence number: 4963
Number of scan points: 7356 points.
Number of filtered scan points: 967 points.
transformed_scan_ptr: 7356 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.3343
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.000417699, 0.0371699, 0.00362906, -0.0235014, -0.00648531, -0.00803877)
Transformation Matrix:
   0.999947  0.00818886 -0.00629436 0.000417699
-0.00803852     0.99969   0.0235506   0.0371699
 0.00648526  -0.0234988    0.999703  0.00362906
          0           0           0           1
shift: 0.0371722
-----------------------------------------------------------------
(Processed/Input): (34 / 34)
-----------------------------------------------------------------
Sequence number: 4964
Number of scan points: 7394 points.
Number of filtered scan points: 950 points.
transformed_scan_ptr: 7394 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.9195
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0137697, 0.00832355, 0.00838804, -0.0167046, -0.0027272, -0.00808028)
Transformation Matrix:
   0.999964  0.00812462 -0.00259176  -0.0137697
-0.00808017    0.999827   0.0167253  0.00832355
  0.0027272  -0.0167038    0.999857  0.00838804
          0           0           0           1
shift: 0.0160899
-----------------------------------------------------------------
(Processed/Input): (35 / 35)
-----------------------------------------------------------------
Sequence number: 4965
Number of scan points: 7416 points.
Number of filtered scan points: 960 points.
transformed_scan_ptr: 7416 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.461
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0125231, -0.0193704, 0.0113391, -0.011091, 0.00161315, -0.00470325)
Transformation Matrix:
   0.999988  0.00468505  0.00166521   0.0125231
-0.00470323    0.999928    0.011083  -0.0193704
-0.00161315  -0.0110907    0.999937   0.0113391
          0           0           0           1
shift: 0.023066
-----------------------------------------------------------------
(Processed/Input): (36 / 36)
-----------------------------------------------------------------
Sequence number: 4966
Number of scan points: 7398 points.
Number of filtered scan points: 980 points.
transformed_scan_ptr: 7398 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.5543
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.00122374, -0.053501, 0.00902797, -0.00366143, 0.00365695, -0.00459523)
Transformation Matrix:
   0.999983   0.0045818  0.00367369 -0.00122374
-0.00459518    0.999983  0.00364459   -0.053501
-0.00365694  -0.0036614    0.999987  0.00902797
          0           0           0           1
shift: 0.053515
-----------------------------------------------------------------
(Processed/Input): (37 / 37)
-----------------------------------------------------------------
Sequence number: 4967
Number of scan points: 7388 points.
Number of filtered scan points: 976 points.
transformed_scan_ptr: 7388 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.1215
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.00793347, -0.0610355, 0.00102758, 0.0051402, 0.0035855, -0.00605548)
Transformation Matrix:
   0.999975  0.00607379  0.00355425  0.00793347
 -0.0060554    0.999968 -0.00516179  -0.0610355
-0.00358549  0.00514014     0.99998  0.00102758
          0           0           0           1
shift: 0.0615489
-----------------------------------------------------------------
(Processed/Input): (38 / 38)
-----------------------------------------------------------------
Sequence number: 4968
Number of scan points: 7353 points.
Number of filtered scan points: 980 points.
transformed_scan_ptr: 7353 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.0353
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0140941, -0.0624637, -0.00818659, 0.00948135, 0.0043787, -0.00528619)
Transformation Matrix:
   0.999976  0.00532744  0.00432832   0.0140941
-0.00528612    0.999941 -0.00950422  -0.0624637
-0.00437869  0.00948112    0.999945 -0.00818659
          0           0           0           1
shift: 0.0640341
-----------------------------------------------------------------
(Processed/Input): (39 / 39)
-----------------------------------------------------------------
Sequence number: 4969
Number of scan points: 7385 points.
Number of filtered scan points: 1005 points.
transformed_scan_ptr: 7385 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.0632
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.041579, -0.073511, -0.0255607, 0.00983519, 0.00638885, -0.00371003)
Transformation Matrix:
   0.999973  0.00377266  0.00635197    0.041579
-0.00370994    0.999945 -0.00985866   -0.073511
-0.00638881  0.00983483    0.999931  -0.0255607
          0           0           0           1
shift: 0.0844552
-----------------------------------------------------------------
(Processed/Input): (40 / 40)
-----------------------------------------------------------------
Sequence number: 4970
Number of scan points: 7282 points.
Number of filtered scan points: 1022 points.
transformed_scan_ptr: 7282 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.6699
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0544338, -0.0713245, -0.0376744, 0.00530921, 0.00738133, -0.00384677)
Transformation Matrix:
   0.999965   0.0038859  0.00736068   0.0544338
-0.00384665    0.999978 -0.00533755  -0.0713245
-0.00738126  0.00530904    0.999959  -0.0376744
          0           0           0           1
shift: 0.089723
-----------------------------------------------------------------
(Processed/Input): (41 / 41)
-----------------------------------------------------------------
Sequence number: 4971
Number of scan points: 7360 points.
Number of filtered scan points: 1058 points.
transformed_scan_ptr: 7360 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.6286
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.052128, 0.00234396, -0.0644927, -0.00818862, 0.0122714, -0.000857566)
Transformation Matrix:
    0.999924  0.000757061    0.0122777     0.052128
-0.000857502     0.999966     0.008178   0.00234396
   -0.012271  -0.00818791     0.999891   -0.0644927
           0            0            0            1
shift: 0.0521807
-----------------------------------------------------------------
(Processed/Input): (42 / 42)
-----------------------------------------------------------------
Sequence number: 4972
Number of scan points: 7193 points.
Number of filtered scan points: 1045 points.
transformed_scan_ptr: 7193 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.7074
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0203013, -0.0254789, -0.0810399, -0.0158213, 0.0117789, -0.00914004)
Transformation Matrix:
   0.999889  0.00895242   0.0119213   0.0203013
-0.00913928    0.999835   0.0157123  -0.0254789
 -0.0117786  -0.0158195    0.999806  -0.0810399
          0           0           0           1
shift: 0.0325779
-----------------------------------------------------------------
(Processed/Input): (43 / 43)
-----------------------------------------------------------------
Sequence number: 4973
Number of scan points: 7224 points.
Number of filtered scan points: 1051 points.
transformed_scan_ptr: 7224 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.6305
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.00823359, -0.0683582, -0.100731, -0.0196266, 0.0112093, -0.00841853)
Transformation Matrix:
   0.999902  0.00819684   0.0113718 -0.00823359
 -0.0084179    0.999774   0.0195303  -0.0683582
 -0.0112091  -0.0196241    0.999745   -0.100731
          0           0           0           1
shift: 0.0688523
-----------------------------------------------------------------
(Processed/Input): (44 / 44)
-----------------------------------------------------------------
Sequence number: 4974
Number of scan points: 7239 points.
Number of filtered scan points: 1052 points.
transformed_scan_ptr: 7239 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.0688
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.023126, 0.0156373, -0.120908, -0.0153606, 0.00984456, -0.00330062)
Transformation Matrix:
   0.999946  0.00314903  0.00989388    0.023126
-0.00330045    0.999877   0.0153274   0.0156373
 -0.0098444  -0.0153593    0.999834   -0.120908
          0           0           0           1
shift: 0.0279166
-----------------------------------------------------------------
(Processed/Input): (45 / 45)
-----------------------------------------------------------------
Sequence number: 4975
Number of scan points: 7266 points.
Number of filtered scan points: 1053 points.
transformed_scan_ptr: 7266 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.0534
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0253935, -0.0385426, -0.132632, -0.0101092, 0.00613759, -0.00167557)
Transformation Matrix:
    0.99998  0.00161343  0.00615417   0.0253935
-0.00167553    0.999948   0.0100987  -0.0385426
-0.00613756  -0.0101088     0.99993   -0.132632
          0           0           0           1
shift: 0.0461559
-----------------------------------------------------------------
(Processed/Input): (46 / 46)
-----------------------------------------------------------------
Sequence number: 4976
Number of scan points: 7135 points.
Number of filtered scan points: 1062 points.
transformed_scan_ptr: 7135 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.3247
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0218474, -0.0244984, -0.137857, 0.0009402, 0.00164904, -0.0020096)
Transformation Matrix:
    0.999997   0.00201114   0.00164715    0.0218474
 -0.00200959     0.999998 -0.000943505   -0.0244984
 -0.00164904  0.000940198     0.999998    -0.137857
           0            0            0            1
shift: 0.032825
-----------------------------------------------------------------
(Processed/Input): (47 / 47)
(Processed/Input): (48 / 48)
-----------------------------------------------------------------
Sequence number: 4977
Number of scan points: 7105 points.
Number of filtered scan points: 1073 points.
transformed_scan_ptr: 7105 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5665
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.00572269, -0.0892745, -0.142488, 0.016169, -0.00219547, -0.00784294)
Transformation Matrix:
   0.999967  0.00780634 -0.00232191 -0.00572269
-0.00784284    0.999839  -0.0161506  -0.0892745
 0.00219547   0.0161682    0.999867   -0.142488
          0           0           0           1
shift: 0.0894577
-----------------------------------------------------------------
-----------------------------------------------------------------
Sequence number: 4978
Number of scan points: 7098 points.
Number of filtered scan points: 1058 points.
transformed_scan_ptr: 7098 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.6573
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0460274, -0.096874, -0.140139, 0.0269287, -0.00359205, 0.000605998)
Transformation Matrix:
    0.999993 -0.000702504  -0.00357442    0.0460274
 0.000605994     0.999637   -0.0269276    -0.096874
  0.00359205    0.0269252     0.999631    -0.140139
           0            0            0            1
shift: 0.107252
-----------------------------------------------------------------
(Processed/Input): (49 / 49)
-----------------------------------------------------------------
Sequence number: 4979
Number of scan points: 6947 points.
Number of filtered scan points: 1055 points.
transformed_scan_ptr: 6947 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.1356
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0569511, -0.0801379, -0.138094, 0.0318658, -0.00575092, -0.00298255)
Transformation Matrix:
   0.999979   0.0027978 -0.00584297   0.0569511
 -0.0029825    0.999488  -0.0318432  -0.0801379
 0.00575089   0.0318599    0.999476   -0.138094
          0           0           0           1
shift: 0.0983133
-----------------------------------------------------------------
(Processed/Input): (50 / 50)
-----------------------------------------------------------------
Sequence number: 4980
Number of scan points: 6911 points.
Number of filtered scan points: 1091 points.
transformed_scan_ptr: 6911 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.787
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.00558316, -0.109254, -0.133365, 0.033642, -0.0077558, -0.00487464)
Transformation Matrix:
   0.999958  0.00461101  -0.0079152  0.00558316
-0.00487448    0.999424  -0.0335975   -0.109254
 0.00775572   0.0336347    0.999404   -0.133365
          0           0           0           1
shift: 0.109397
-----------------------------------------------------------------
(Processed/Input): (51 / 51)
-----------------------------------------------------------------
Sequence number: 4981
Number of scan points: 6973 points.
Number of filtered scan points: 1117 points.
transformed_scan_ptr: 6973 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0445
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0169734, -0.0958142, -0.124684, 0.0322864, -0.011018, -0.00956779)
Transformation Matrix:
   0.999894  0.00920701  -0.0113203  -0.0169734
-0.00956707    0.999436  -0.0321739  -0.0958142
  0.0110177   0.0322788    0.999418   -0.124684
          0           0           0           1
shift: 0.097306
-----------------------------------------------------------------
(Processed/Input): (52 / 52)
-----------------------------------------------------------------
Sequence number: 4982
Number of scan points: 6870 points.
Number of filtered scan points: 1080 points.
transformed_scan_ptr: 6870 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.2019
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0266886, -0.0781252, -0.111147, 0.032145, -0.00993899, -0.0071321)
Transformation Matrix:
   0.999925  0.00680894  -0.0101627  -0.0266886
-0.00713169     0.99946  -0.0320678  -0.0781252
 0.00993882   0.0321378    0.999434   -0.111147
          0           0           0           1
shift: 0.082558
-----------------------------------------------------------------
(Processed/Input): (53 / 53)
-----------------------------------------------------------------
Sequence number: 4983
Number of scan points: 6767 points.
Number of filtered scan points: 1094 points.
transformed_scan_ptr: 6767 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.643
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.000820437, -0.125381, -0.103871, 0.0331599, -0.00992735, -0.00280335)
Transformation Matrix:
    0.999947   0.00247267   -0.0100146 -0.000820437
 -0.00280321     0.999447   -0.0331259    -0.125381
  0.00992718    0.0331522     0.999401    -0.103871
           0            0            0            1
shift: 0.125383
-----------------------------------------------------------------
(Processed/Input): (54 / 54)
-----------------------------------------------------------------
Sequence number: 4984
Number of scan points: 7016 points.
Number of filtered scan points: 1132 points.
transformed_scan_ptr: 7016 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.2946
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.0115511, -0.136168, -0.094847, 0.0348202, -0.00717045, -0.00729742)
Transformation Matrix:
   0.999948  0.00704332 -0.00741989  -0.0115511
-0.00729716    0.999369  -0.0347599   -0.136168
 0.00717039   0.0348122    0.999368   -0.094847
          0           0           0           1
shift: 0.136657
-----------------------------------------------------------------
(Processed/Input): (55 / 55)
-----------------------------------------------------------------
Sequence number: 4985
Number of scan points: 7036 points.
Number of filtered scan points: 1156 points.
transformed_scan_ptr: 7036 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.112
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.039607, -0.0784213, -0.0833986, 0.0348008, -0.00588289, -0.000384491)
Transformation Matrix:
    0.999983  0.000179575  -0.00589268    -0.039607
-0.000384484     0.999395   -0.0347915   -0.0784213
  0.00588286    0.0347932     0.999377   -0.0833986
           0            0            0            1
shift: 0.0878556
-----------------------------------------------------------------
(Processed/Input): (56 / 56)
-----------------------------------------------------------------
Sequence number: 4986
Number of scan points: 7001 points.
Number of filtered scan points: 1160 points.
transformed_scan_ptr: 7001 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.7083
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0370514, -0.0821259, -0.0741678, 0.0361037, -0.00476915, -0.000735075)
Transformation Matrix:
    0.999988  0.000562455  -0.00479256   -0.0370514
-0.000735066     0.999348   -0.0360924   -0.0821259
  0.00476914    0.0360955     0.999337   -0.0741678
           0            0            0            1
shift: 0.090097
-----------------------------------------------------------------
(Processed/Input): (57 / 57)
-----------------------------------------------------------------
Sequence number: 4987
Number of scan points: 7120 points.
Number of filtered scan points: 1188 points.
transformed_scan_ptr: 7120 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.8046
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0327677, -0.087717, -0.0632614, 0.039393, -0.00338143, -0.00239841)
Transformation Matrix:
   0.999991  0.00226338 -0.00347325   0.0327677
-0.00239839    0.999222  -0.0393746   -0.087717
 0.00338143   0.0393826    0.999219  -0.0632614
          0           0           0           1
shift: 0.0936375
-----------------------------------------------------------------
(Processed/Input): (58 / 58)
-----------------------------------------------------------------
Sequence number: 4988
Number of scan points: 7019 points.
Number of filtered scan points: 1195 points.
transformed_scan_ptr: 7019 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.7081
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0431029, -0.146831, -0.0536982, 0.0461987, -0.00192691, -0.00380408)
Transformation Matrix:
   0.999991  0.00371102 -0.00210052   0.0431029
-0.00380406    0.998926  -0.0461746   -0.146831
 0.00192691   0.0461822    0.998931  -0.0536982
          0           0           0           1
shift: 0.153027
-----------------------------------------------------------------
(Processed/Input): (59 / 59)
-----------------------------------------------------------------
Sequence number: 4989
Number of scan points: 7145 points.
Number of filtered scan points: 1208 points.
transformed_scan_ptr: 7145 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.4996
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0185299, -0.197299, -0.0530774, 0.055996, 0.00339206, -0.00798309)
Transformation Matrix:
   0.999962  0.00816032  0.00293985   0.0185299
-0.00798296    0.998399   -0.055992   -0.197299
-0.00339205   0.0559664    0.998427  -0.0530774
          0           0           0           1
shift: 0.198168
-----------------------------------------------------------------
(Processed/Input): (60 / 60)
-----------------------------------------------------------------
Sequence number: 4990
Number of scan points: 7106 points.
Number of filtered scan points: 1206 points.
transformed_scan_ptr: 7106 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.3149
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(0.041356, -0.161631, -0.0470721, 0.065528, 0.00749936, -0.000430178)
Transformation Matrix:
    0.999972   0.00092032   0.00745502     0.041356
-0.000430166     0.997854   -0.0654844    -0.161631
 -0.00749929    0.0654793     0.997826   -0.0470721
           0            0            0            1
shift: 0.166838
-----------------------------------------------------------------
(Processed/Input): (61 / 61)
-----------------------------------------------------------------
Sequence number: 4991
Number of scan points: 6861 points.
Number of filtered scan points: 1184 points.
transformed_scan_ptr: 6861 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.7621
Number of iteration: 8
(x,y,z,roll,pitch,yaw):
(-0.00716093, -0.269613, -0.0316585, 0.0745969, 0.00630462, -0.0098581)
Transformation Matrix:
   0.999932   0.0103004  0.00555206 -0.00716093
-0.00985774    0.997166  -0.0745861   -0.269613
-0.00630458   0.0745263    0.997199  -0.0316585
          0           0           0           1
shift: 0.269708
-----------------------------------------------------------------
(Processed/Input): (62 / 62)
-----------------------------------------------------------------
Sequence number: 4992
Number of scan points: 6804 points.
Number of filtered scan points: 1197 points.
transformed_scan_ptr: 6804 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.3983
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(-0.0128719, -0.107761, 0.0119515, 0.0297058, -0.0295855, -0.00696975)
Transformation Matrix:
   0.999538  0.00608805  -0.0297744  -0.0128719
-0.00696665    0.999541  -0.0294947   -0.107761
  0.0295812   0.0296885    0.999121   0.0119515
          0           0           0           1
shift: 0.108527
-----------------------------------------------------------------
(Processed/Input): (63 / 63)
-----------------------------------------------------------------
Sequence number: 4993
Number of scan points: 7248 points.
Number of filtered scan points: 1267 points.
transformed_scan_ptr: 7248 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.0459
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(-0.167718, -0.176285, 0.0611739, 0.0438062, -0.0758281, -0.00743549)
Transformation Matrix:
   0.997099   0.0041109  -0.0760063   -0.167718
-0.00741406    0.999038  -0.0432283   -0.176285
  0.0757554   0.0436664     0.99617   0.0611739
          0           0           0           1
shift: 0.243323
-----------------------------------------------------------------
(Processed/Input): (64 / 64)
-----------------------------------------------------------------
Sequence number: 4994
Number of scan points: 6974 points.
Number of filtered scan points: 1256 points.
transformed_scan_ptr: 6974 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.3721
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.174979, -0.107272, 0.0643426, 0.0468502, -0.0762697, -0.00819251)
Transformation Matrix:
  0.997059 0.00461509 -0.0764933  -0.174979
-0.0081686   0.998898 -0.0462079  -0.107272
 0.0761958  0.0466969   0.995999  0.0643426
         0          0          0          1
shift: 0.205243
-----------------------------------------------------------------
(Processed/Input): (65 / 65)
-----------------------------------------------------------------
Sequence number: 4995
Number of scan points: 7123 points.
Number of filtered scan points: 1257 points.
transformed_scan_ptr: 7123 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.7281
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0335818, -0.0770933, 0.111906, 0.0159365, 0.00648489, -0.00609739)
Transformation Matrix:
    0.99996  0.00619991  0.00638675   0.0335818
-0.00609723    0.999854   -0.015975  -0.0770933
-0.00648485   0.0159354    0.999852    0.111906
          0           0           0           1
shift: 0.0840899
-----------------------------------------------------------------
(Processed/Input): (66 / 66)
-----------------------------------------------------------------
Sequence number: 4996
Number of scan points: 7007 points.
Number of filtered scan points: 1227 points.
transformed_scan_ptr: 7007 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.5502
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(0.125799, -0.101069, 0.118909, 0.0432194, 0.0439372, -0.00130048)
Transformation Matrix:
   0.999034  0.00319697   0.0438258    0.125799
-0.00129922    0.999063  -0.0432629   -0.101069
 -0.0439231   0.0431642    0.998102    0.118909
          0           0           0           1
shift: 0.16137
-----------------------------------------------------------------
(Processed/Input): (67 / 67)
-----------------------------------------------------------------
Sequence number: 4997
Number of scan points: 6943 points.
Number of filtered scan points: 1242 points.
transformed_scan_ptr: 6943 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.1259
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0915286, -0.246731, 0.0671197, 0.107555, 0.0349676, -0.0019473)
Transformation Matrix:
   0.999387  0.00568897   0.0345493   0.0915286
-0.00194611    0.994212   -0.107415   -0.246731
 -0.0349605    0.107282    0.993614   0.0671197
          0           0           0           1
shift: 0.263161
-----------------------------------------------------------------
(Processed/Input): (68 / 68)
-----------------------------------------------------------------
Sequence number: 4998
Number of scan points: 6809 points.
Number of filtered scan points: 1288 points.
transformed_scan_ptr: 6809 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.4035
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0626196, -0.34743, 0.0214334, 0.128621, 0.0212147, -0.00223369)
Transformation Matrix:
   0.999772  0.00493615   0.0207513   0.0626196
-0.00223318    0.991731   -0.128313    -0.34743
 -0.0212131    0.128238    0.991517   0.0214334
          0           0           0           1
shift: 0.353028
-----------------------------------------------------------------
(Processed/Input): (69 / 69)
-----------------------------------------------------------------
Sequence number: 4999
Number of scan points: 6893 points.
Number of filtered scan points: 1304 points.
transformed_scan_ptr: 6893 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.2005
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0224305, -0.352227, -0.00741645, 0.122195, 0.0112681, -0.00239096)
Transformation Matrix:
   0.999934  0.00374659   0.0108924   0.0224305
-0.00239081    0.992537   -0.121917   -0.352227
 -0.0112679    0.121883     0.99248 -0.00741645
          0           0           0           1
shift: 0.352941
-----------------------------------------------------------------
(Processed/Input): (70 / 70)
-----------------------------------------------------------------
Sequence number: 5000
Number of scan points: 6833 points.
Number of filtered scan points: 1314 points.
transformed_scan_ptr: 6833 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.3025
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0453446, -0.293043, -0.0236796, 0.108044, 0.00807443, -0.00275272)
Transformation Matrix:
   0.999964  0.00360734  0.00773039   0.0453446
-0.00275262    0.994163   -0.107856   -0.293043
-0.00807434    0.107831    0.994137  -0.0236796
          0           0           0           1
shift: 0.296531
-----------------------------------------------------------------
(Processed/Input): (71 / 71)
-----------------------------------------------------------------
Sequence number: 5001
Number of scan points: 6941 points.
Number of filtered scan points: 1316 points.
transformed_scan_ptr: 6941 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.3806
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0694761, -0.228277, -0.0280666, 0.0953724, 0.00793961, -0.00472414)
Transformation Matrix:
   0.999957   0.0054587  0.00745349   0.0694761
-0.00472397    0.995441  -0.0952641   -0.228277
-0.00793952   0.0952249    0.995424  -0.0280666
          0           0           0           1
shift: 0.238615
-----------------------------------------------------------------
(Processed/Input): (72 / 72)
-----------------------------------------------------------------
Sequence number: 5002
Number of scan points: 7072 points.
Number of filtered scan points: 1294 points.
transformed_scan_ptr: 7072 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.186
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.021761, -0.251179, -0.0322059, 0.0878234, 0.00781419, -0.00331421)
Transformation Matrix:
   0.999964  0.00398681  0.00749326    0.021761
 -0.0033141    0.996138  -0.0877359   -0.251179
-0.00781411   0.0877079    0.996116  -0.0322059
          0           0           0           1
shift: 0.25212
-----------------------------------------------------------------
(Processed/Input): (73 / 73)
-----------------------------------------------------------------
Sequence number: 5003
Number of scan points: 7144 points.
Number of filtered scan points: 1329 points.
transformed_scan_ptr: 7144 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.6537
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0445624, -0.225607, -0.0341698, 0.0850653, 0.0068585, -0.00133489)
Transformation Matrix:
   0.999976  0.00191278  0.00672023   0.0445624
-0.00133486    0.996382  -0.0849718   -0.225607
-0.00685845   0.0849608    0.996361  -0.0341698
          0           0           0           1
shift: 0.229966
-----------------------------------------------------------------
(Processed/Input): (74 / 74)
-----------------------------------------------------------------
Sequence number: 5004
Number of scan points: 7198 points.
Number of filtered scan points: 1324 points.
transformed_scan_ptr: 7198 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.4446
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.045699, -0.217478, -0.0321544, 0.0835451, 0.00433797, -0.00180756)
Transformation Matrix:
   0.999989  0.00216325  0.00417198    0.045699
-0.00180754     0.99651  -0.0834556   -0.217478
-0.00433795   0.0834471    0.996503  -0.0321544
          0           0           0           1
shift: 0.222227
-----------------------------------------------------------------
(Processed/Input): (75 / 75)
-----------------------------------------------------------------
Sequence number: 5005
Number of scan points: 7169 points.
Number of filtered scan points: 1309 points.
transformed_scan_ptr: 7169 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.1473
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0139973, -0.189024, -0.0319642, 0.0800322, 0.00127121, -0.000176858)
Transformation Matrix:
    0.999999  0.000277922     0.001253    0.0139973
-0.000176858     0.996799   -0.0799471    -0.189024
 -0.00127121    0.0799468     0.996798   -0.0319642
           0            0            0            1
shift: 0.189542
-----------------------------------------------------------------
(Processed/Input): (76 / 76)
-----------------------------------------------------------------
Sequence number: 5006
Number of scan points: 7171 points.
Number of filtered scan points: 1309 points.
transformed_scan_ptr: 7171 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.6113
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0158754, -0.17725, -0.0315814, 0.0743493, -0.00157327, -0.00322627)
Transformation Matrix:
   0.999994  0.00310049 -0.00180857   0.0158754
-0.00322626    0.997233  -0.0742754    -0.17725
 0.00157327   0.0742807    0.997236  -0.0315814
          0           0           0           1
shift: 0.17796
-----------------------------------------------------------------
(Processed/Input): (77 / 77)
-----------------------------------------------------------------
Sequence number: 5007
Number of scan points: 7277 points.
Number of filtered scan points: 1344 points.
transformed_scan_ptr: 7277 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.0072
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.024833, -0.223288, -0.0290111, 0.0650161, -0.00382537, -0.00362141)
Transformation Matrix:
   0.999986  0.00336522 -0.00405254    0.024833
-0.00362138    0.997882  -0.0649561   -0.223288
 0.00382536   0.0649698     0.99788  -0.0290111
          0           0           0           1
shift: 0.224665
-----------------------------------------------------------------
(Processed/Input): (78 / 78)
-----------------------------------------------------------------
Sequence number: 5008
Number of scan points: 7097 points.
Number of filtered scan points: 1334 points.
transformed_scan_ptr: 7097 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.2816
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.00216084, -0.167606, -0.022939, 0.0549996, -0.00457897, -0.00386086)
Transformation Matrix:
   0.999982  0.00360331 -0.00478424  0.00216084
-0.00386081    0.998481  -0.0549538   -0.167606
 0.00457896   0.0549713    0.998477   -0.022939
          0           0           0           1
shift: 0.16762
-----------------------------------------------------------------
(Processed/Input): (79 / 79)
-----------------------------------------------------------------
Sequence number: 5009
Number of scan points: 7185 points.
Number of filtered scan points: 1322 points.
transformed_scan_ptr: 7185 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5792
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0142493, -0.116603, -0.0193155, 0.0425342, -0.00357364, -0.00210435)
Transformation Matrix:
   0.999991  0.00195049 -0.00365988   0.0142493
-0.00210434    0.999094  -0.0425138   -0.116603
 0.00357363   0.0425211    0.999089  -0.0193155
          0           0           0           1
shift: 0.117471
-----------------------------------------------------------------
(Processed/Input): (80 / 80)
-----------------------------------------------------------------
Sequence number: 5010
Number of scan points: 7220 points.
Number of filtered scan points: 1344 points.
transformed_scan_ptr: 7220 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.7013
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0217211, -0.0786046, -0.0141213, 0.0242126, -0.00270576, -0.0039994)
Transformation Matrix:
   0.999988  0.00393271 -0.00280177   0.0217211
-0.00399937    0.999699  -0.0241992  -0.0786046
 0.00270576   0.0242101    0.999703  -0.0141213
          0           0           0           1
shift: 0.0815505
-----------------------------------------------------------------
(Processed/Input): (81 / 81)
-----------------------------------------------------------------
Sequence number: 5011
Number of scan points: 7285 points.
Number of filtered scan points: 1335 points.
transformed_scan_ptr: 7285 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5794
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0117311, -0.0798609, -0.0127032, 0.0144534, -0.00276512, -0.00294879)
Transformation Matrix:
   0.999992  0.00290853 -0.00280743   0.0117311
-0.00294878    0.999891  -0.0144447  -0.0798609
 0.00276512   0.0144528    0.999892  -0.0127032
          0           0           0           1
shift: 0.0807179
-----------------------------------------------------------------
(Processed/Input): (82 / 82)
-----------------------------------------------------------------
Sequence number: 5012
Number of scan points: 7177 points.
Number of filtered scan points: 1350 points.
transformed_scan_ptr: 7177 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.8153
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0212812, -0.0521394, -0.0120538, 0.00667716, -0.0025992, -0.0057494)
Transformation Matrix:
    0.99998  0.00573189 -0.00263748   0.0212812
-0.00574935    0.999961 -0.00666206  -0.0521394
  0.0025992  0.00667709    0.999974  -0.0120538
          0           0           0           1
shift: 0.0563153
-----------------------------------------------------------------
(Processed/Input): (83 / 83)
-----------------------------------------------------------------
Sequence number: 5013
Number of scan points: 7215 points.
Number of filtered scan points: 1339 points.
transformed_scan_ptr: 7215 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.8248
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0234503, -0.00524644, -0.0144821, 0.000218835, -0.00155633, -0.00156327)
Transformation Matrix:
    0.999998   0.00156293  -0.00155667    0.0234503
 -0.00156326     0.999999 -0.000216415  -0.00524644
  0.00155633  0.000218835     0.999999   -0.0144821
           0            0            0            1
shift: 0.02403
-----------------------------------------------------------------
(Processed/Input): (84 / 84)
-----------------------------------------------------------------
Sequence number: 5014
Number of scan points: 7267 points.
Number of filtered scan points: 1332 points.
transformed_scan_ptr: 7267 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5995
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0355194, -0.0117753, -0.0174698, -0.00144651, -0.000750538, 0.000134567)
Transformation Matrix:
           1 -0.000133502 -0.000750728    0.0355194
 0.000134567     0.999999   0.00144641   -0.0117753
 0.000750538  -0.00144651     0.999999   -0.0174698
           0            0            0            1
shift: 0.0374204
-----------------------------------------------------------------
(Processed/Input): (85 / 85)
-----------------------------------------------------------------
Sequence number: 5015
Number of scan points: 7162 points.
Number of filtered scan points: 1332 points.
transformed_scan_ptr: 7162 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0915
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0336162, -0.0285909, -0.0249187, -0.00150705, -0.00105962, -0.000594787)
Transformation Matrix:
    0.999999  0.000596361  -0.00105874    0.0336162
-0.000594787     0.999999   0.00150767   -0.0285909
  0.00105962  -0.00150704     0.999998   -0.0249187
           0            0            0            1
shift: 0.0441303
-----------------------------------------------------------------
(Processed/Input): (86 / 86)
-----------------------------------------------------------------
Sequence number: 5016
Number of scan points: 7186 points.
Number of filtered scan points: 1331 points.
transformed_scan_ptr: 7186 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0589
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0436718, -0.0464959, -0.0311566, 0.00117319, -0.00134243, -0.000421815)
Transformation Matrix:
    0.999999  0.000420248  -0.00134292    0.0436718
-0.000421815     0.999999  -0.00117263   -0.0464959
  0.00134243   0.00117319     0.999998   -0.0311566
           0            0            0            1
shift: 0.0637894
-----------------------------------------------------------------
(Processed/Input): (87 / 87)
-----------------------------------------------------------------
Sequence number: 5017
Number of scan points: 7269 points.
Number of filtered scan points: 1340 points.
transformed_scan_ptr: 7269 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.3447
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0394017, -0.0464102, -0.0351532, 0.0032901, -0.00144364, -0.00167024)
Transformation Matrix:
   0.999998  0.00166547 -0.00144914   0.0394017
-0.00167024    0.999993 -0.00328767  -0.0464102
 0.00144364  0.00329009    0.999994  -0.0351532
          0           0           0           1
shift: 0.0608802
-----------------------------------------------------------------
(Processed/Input): (88 / 88)
-----------------------------------------------------------------
Sequence number: 5018
Number of scan points: 7266 points.
Number of filtered scan points: 1337 points.
transformed_scan_ptr: 7266 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.4681
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0487053, -0.0392284, -0.0437709, 0.00345524, -0.00137476, -0.00188965)
Transformation Matrix:
   0.999997  0.00188489 -0.00138128   0.0487053
-0.00188965    0.999992 -0.00345263  -0.0392284
 0.00137476  0.00345523    0.999993  -0.0437709
          0           0           0           1
shift: 0.0625386
-----------------------------------------------------------------
(Processed/Input): (89 / 89)
-----------------------------------------------------------------
Sequence number: 5019
Number of scan points: 7151 points.
Number of filtered scan points: 1322 points.
transformed_scan_ptr: 7151 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.4554
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0303628, -0.0156007, -0.0513662, -0.00148694, -0.00153602, -0.00053624)
Transformation Matrix:
    0.999999  0.000538537  -0.00153522    0.0303628
-0.000536239     0.999999   0.00148776   -0.0156007
  0.00153602  -0.00148693     0.999998   -0.0513662
           0            0            0            1
shift: 0.0341362
-----------------------------------------------------------------
(Processed/Input): (90 / 90)
-----------------------------------------------------------------
Sequence number: 5020
Number of scan points: 7102 points.
Number of filtered scan points: 1313 points.
transformed_scan_ptr: 7102 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.3591
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0058644, -0.0066732, -0.0559401, -0.0121747, -0.00128827, -0.00476505)
Transformation Matrix:
   0.999988  0.00478036 -0.00123016   0.0058644
-0.00476503    0.999914   0.0121804  -0.0066732
 0.00128827  -0.0121744    0.999925  -0.0559401
          0           0           0           1
shift: 0.00888385
-----------------------------------------------------------------
(Processed/Input): (91 / 91)
-----------------------------------------------------------------
Sequence number: 5021
Number of scan points: 7115 points.
Number of filtered scan points: 1325 points.
transformed_scan_ptr: 7115 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.199
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.00825556, 0.00467373, -0.0596311, -0.0277242, -0.00248579, -0.00169857)
Transformation Matrix:
   0.999995  0.00176683 -0.00243774  0.00825556
-0.00169856    0.999614   0.0277248  0.00467373
 0.00248579  -0.0277206    0.999613  -0.0596311
          0           0           0           1
shift: 0.00948673
-----------------------------------------------------------------
(Processed/Input): (92 / 92)
-----------------------------------------------------------------
Sequence number: 5022
Number of scan points: 7143 points.
Number of filtered scan points: 1319 points.
transformed_scan_ptr: 7143 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.7587
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0249613, 0.0594188, -0.0651992, -0.0466628, -0.00426436, -0.00493584)
Transformation Matrix:
   0.999979  0.00512936 -0.00402942   0.0249613
-0.00493578    0.998898   0.0466664   0.0594188
 0.00426434  -0.0466455    0.998902  -0.0651992
          0           0           0           1
shift: 0.0644489
-----------------------------------------------------------------
(Processed/Input): (93 / 93)
-----------------------------------------------------------------
Sequence number: 5023
Number of scan points: 6947 points.
Number of filtered scan points: 1322 points.
transformed_scan_ptr: 6947 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.2299
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.00290237, 0.111845, -0.0585938, -0.0622268, -0.00706523, -6.42067e-05)
Transformation Matrix:
    0.999975  0.000503438  -0.00704751  -0.00290237
-6.42051e-05     0.998065    0.0621871     0.111845
  0.00706517   -0.0621851      0.99804   -0.0585938
           0            0            0            1
shift: 0.111883
-----------------------------------------------------------------
(Processed/Input): (94 / 94)
-----------------------------------------------------------------
Sequence number: 5024
Number of scan points: 7066 points.
Number of filtered scan points: 1374 points.
transformed_scan_ptr: 7066 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.4287
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0447949, 0.151675, -0.0498557, -0.077091, -0.00974932, -0.00284288)
Transformation Matrix:
   0.999948  0.00358526 -0.00950123  -0.0447949
-0.00284274    0.997024    0.077042    0.151675
 0.00974916   -0.077011    0.996983  -0.0498557
          0           0           0           1
shift: 0.158151
-----------------------------------------------------------------
(Processed/Input): (95 / 95)
-----------------------------------------------------------------
Sequence number: 5025
Number of scan points: 7036 points.
Number of filtered scan points: 1351 points.
transformed_scan_ptr: 7036 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.9988
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0508213, 0.117942, -0.0365139, -0.0809699, -0.0106748, -0.00355391)
Transformation Matrix:
  0.999937 0.00440564 -0.0103521 -0.0508213
-0.0035537   0.996714  0.0809188   0.117942
 0.0106746 -0.0808768   0.996667 -0.0365139
         0          0          0          1
shift: 0.128425
-----------------------------------------------------------------
(Processed/Input): (96 / 96)
-----------------------------------------------------------------
Sequence number: 5026
Number of scan points: 7241 points.
Number of filtered scan points: 1380 points.
transformed_scan_ptr: 7241 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.0505
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.00313494, 0.13825, -0.0235561, -0.0757679, -0.00853554, -0.00236601)
Transformation Matrix:
   0.999961  0.00300531 -0.00833183 -0.00313494
-0.00236592    0.997127   0.0757154     0.13825
 0.00853544  -0.0756927    0.997095  -0.0235561
          0           0           0           1
shift: 0.138286
-----------------------------------------------------------------
(Processed/Input): (97 / 97)
-----------------------------------------------------------------
Sequence number: 5027
Number of scan points: 7178 points.
Number of filtered scan points: 1385 points.
transformed_scan_ptr: 7178 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.6863
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0325517, 0.0990408, -0.0083144, -0.0611661, -0.0062466, -0.00175301)
Transformation Matrix:
   0.999979  0.00213157 -0.00612771   0.0325517
-0.00175298    0.998128   0.0611388   0.0990408
 0.00624656  -0.0611268     0.99811  -0.0083144
          0           0           0           1
shift: 0.104253
-----------------------------------------------------------------
(Processed/Input): (98 / 98)
(Processed/Input): (99 / 99)
-----------------------------------------------------------------
Sequence number: 5028
Number of scan points: 7238 points.
Number of filtered scan points: 1308 points.
transformed_scan_ptr: 7238 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.6463
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0406336, 0.0710913, 0.000544692, -0.0471956, -0.00129338, -0.00570591)
Transformation Matrix:
   0.999983  0.00576055 -0.00102273   0.0406336
-0.00570588     0.99887   0.0471847   0.0710913
 0.00129338  -0.0471781    0.998886 0.000544692
          0           0           0           1
shift: 0.0818844
-----------------------------------------------------------------
-----------------------------------------------------------------
Sequence number: 5029
Number of scan points: 7162 points.
Number of filtered scan points: 1304 points.
transformed_scan_ptr: 7162 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.027
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0297307, 0.0673409, 0.00444536, -0.0387459, 0.00308806, -0.00481902)
Transformation Matrix:
   0.999984  0.00469576  0.00327238   0.0297307
-0.00481898    0.999238   0.0387209   0.0673409
-0.00308806   -0.038736    0.999245  0.00444536
          0           0           0           1
shift: 0.0736119
-----------------------------------------------------------------
(Processed/Input): (100 / 100)
-----------------------------------------------------------------
Sequence number: 5030
Number of scan points: 7098 points.
Number of filtered scan points: 1299 points.
transformed_scan_ptr: 7098 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.314
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0347806, 0.0428292, 0.00175324, -0.0386025, 0.00584424, -0.005206)
Transformation Matrix:
   0.999969  0.00497655  0.00604069   0.0347806
-0.00520589    0.999243    0.038562   0.0428292
 -0.0058442  -0.0385922    0.999238  0.00175324
          0           0           0           1
shift: 0.0551728
-----------------------------------------------------------------
(Processed/Input): (101 / 101)
-----------------------------------------------------------------
Sequence number: 5031
Number of scan points: 7107 points.
Number of filtered scan points: 1299 points.
transformed_scan_ptr: 7107 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.0196
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0520548, 0.037951, -0.00449463, -0.0462126, 0.00774637, -0.0095279)
Transformation Matrix:
   0.999925  0.00915976  0.00817782   0.0520548
-0.00952747     0.99889   0.0461203    0.037951
-0.00774629  -0.0461947    0.998902 -0.00449463
          0           0           0           1
shift: 0.0644203
-----------------------------------------------------------------
(Processed/Input): (102 / 102)
-----------------------------------------------------------------
Sequence number: 5032
Number of scan points: 7156 points.
Number of filtered scan points: 1333 points.
transformed_scan_ptr: 7156 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.0203
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0658653, 0.0587338, -0.0162472, -0.0558933, 0.00801693, -0.00802672)
Transformation Matrix:
   0.999936  0.00756626  0.00845246   0.0658653
-0.00802637     0.99841   0.0557982   0.0587338
-0.00801684  -0.0558625    0.998406  -0.0162472
          0           0           0           1
shift: 0.088249
-----------------------------------------------------------------
(Processed/Input): (103 / 103)
-----------------------------------------------------------------
Sequence number: 5033
Number of scan points: 7179 points.
Number of filtered scan points: 1329 points.
transformed_scan_ptr: 7179 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.8256
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0346017, 0.121714, -0.0184858, -0.0693123, 0.00517053, -0.00371475)
Transformation Matrix:
    0.99998  0.00334773  0.00541533   0.0346017
-0.00371469    0.997593   0.0692372    0.121714
-0.00517051  -0.0692559    0.997586  -0.0184858
          0           0           0           1
shift: 0.126537
-----------------------------------------------------------------
(Processed/Input): (104 / 104)
-----------------------------------------------------------------
Sequence number: 5034
Number of scan points: 6943 points.
Number of filtered scan points: 1318 points.
transformed_scan_ptr: 6943 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.1471
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0247091, 0.156226, -0.0129416, -0.0843474, 0.00107793, -0.000935366)
Transformation Matrix:
    0.999999  0.000841227    0.0011529    0.0247091
-0.000935366     0.996444    0.0842464     0.156226
 -0.00107793   -0.0842474     0.996444   -0.0129416
           0            0            0            1
shift: 0.158168
-----------------------------------------------------------------
(Processed/Input): (105 / 105)
-----------------------------------------------------------------
Sequence number: 5035
Number of scan points: 7021 points.
Number of filtered scan points: 1288 points.
transformed_scan_ptr: 7021 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.0749
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.00970651, 0.156048, -0.0062631, -0.092238, -0.00159972, -0.0045137)
Transformation Matrix:
   0.999989  0.00464184 -0.00117716  0.00970651
-0.00451368    0.995738   0.0921135    0.156048
 0.00159972  -0.0921071    0.995748  -0.0062631
          0           0           0           1
shift: 0.156349
-----------------------------------------------------------------
(Processed/Input): (106 / 106)
-----------------------------------------------------------------
Sequence number: 5036
Number of scan points: 7087 points.
Number of filtered scan points: 1359 points.
transformed_scan_ptr: 7087 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.4661
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0105241, 0.239044, 0.0128923, -0.118965, -0.0190743, -0.00318948)
Transformation Matrix:
   0.999813   0.0054306  -0.0185597  -0.0105241
-0.00318889     0.99292    0.118744    0.239044
  0.0190732   -0.118663    0.992751   0.0128923
          0           0           0           1
shift: 0.239275
-----------------------------------------------------------------
(Processed/Input): (107 / 107)
-----------------------------------------------------------------
Sequence number: 5037
Number of scan points: 7148 points.
Number of filtered scan points: 1378 points.
transformed_scan_ptr: 7148 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.4261
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.087753, 0.22428, 0.0532115, -0.0986031, -0.0437387, -0.000510655)
Transformation Matrix:
    0.999043   0.00481259   -0.0434621    -0.087753
-0.000510167      0.99514    0.0984657      0.22428
   0.0437247   -0.0983493     0.994191    0.0532115
           0            0            0            1
shift: 0.240837
-----------------------------------------------------------------
(Processed/Input): (108 / 108)
-----------------------------------------------------------------
Sequence number: 5038
Number of scan points: 7486 points.
Number of filtered scan points: 1350 points.
transformed_scan_ptr: 7486 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.553
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0638638, 0.236441, 0.0566867, -0.0974776, -0.0356233, -0.00254898)
Transformation Matrix:
   0.999362  0.00600311  -0.0351985  -0.0638638
-0.00254736    0.995241   0.0974133    0.236441
  0.0356157  -0.0972616    0.994621   0.0566867
          0           0           0           1
shift: 0.244915
-----------------------------------------------------------------
(Processed/Input): (109 / 109)
-----------------------------------------------------------------
Sequence number: 5039
Number of scan points: 7162 points.
Number of filtered scan points: 1334 points.
transformed_scan_ptr: 7162 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.177
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0728958, 0.144014, 0.0446987, -0.0653509, 0.0120427, -0.00295255)
Transformation Matrix:
   0.999923  0.00215982   0.0122094   0.0728958
-0.00295233    0.997863   0.0652686    0.144014
 -0.0120424  -0.0652997    0.997793   0.0446987
          0           0           0           1
shift: 0.161412
-----------------------------------------------------------------
(Processed/Input): (110 / 110)
-----------------------------------------------------------------
Sequence number: 5040
Number of scan points: 7214 points.
Number of filtered scan points: 1260 points.
transformed_scan_ptr: 7214 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.3922
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.141429, 0.127669, 0.0522555, -0.0624499, 0.0464664, -0.00470368)
Transformation Matrix:
    0.99891  0.00179563   0.0466522    0.141429
-0.00469859    0.998053   0.0621906    0.127669
 -0.0464497   -0.062342    0.996973   0.0522555
          0           0           0           1
shift: 0.19053
-----------------------------------------------------------------
(Processed/Input): (111 / 111)
-----------------------------------------------------------------
Sequence number: 5041
Number of scan points: 7135 points.
Number of filtered scan points: 1307 points.
transformed_scan_ptr: 7135 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.0538
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.171025, 0.183441, 0.0459838, -0.0880297, 0.0552914, -0.00758553)
Transformation Matrix:
   0.998443  0.00269772   0.0557145    0.171025
-0.00757386    0.996136    0.087496    0.183441
 -0.0552632  -0.0877817    0.994606   0.0459838
          0           0           0           1
shift: 0.250799
-----------------------------------------------------------------
(Processed/Input): (112 / 112)
-----------------------------------------------------------------
Sequence number: 5042
Number of scan points: 7021 points.
Number of filtered scan points: 1299 points.
transformed_scan_ptr: 7021 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.376
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0544326, 0.257448, 0.00823661, -0.119426, 0.0338982, -0.00510256)
Transformation Matrix:
   0.999412  0.00102832   0.0342578   0.0544326
-0.00509961    0.992885    0.118969    0.257448
 -0.0338917   -0.119073    0.992307  0.00823661
          0           0           0           1
shift: 0.263139
-----------------------------------------------------------------
(Processed/Input): (113 / 113)
-----------------------------------------------------------------
Sequence number: 5043
Number of scan points: 6962 points.
Number of filtered scan points: 1297 points.
transformed_scan_ptr: 6962 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.0076
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0788897, 0.208983, -0.0160042, -0.103391, 0.0167502, -0.00102167)
Transformation Matrix:
    0.999859 -0.000712449    0.0167654    0.0788897
 -0.00102152     0.994661      0.10319     0.208983
  -0.0167494    -0.103192      0.99452   -0.0160042
           0            0            0            1
shift: 0.223377
-----------------------------------------------------------------
(Processed/Input): (114 / 114)
-----------------------------------------------------------------
Sequence number: 5044
Number of scan points: 7036 points.
Number of filtered scan points: 1287 points.
transformed_scan_ptr: 7036 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.4816
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0521499, 0.162203, -0.0274677, -0.0865956, 0.0126607, -0.00329399)
Transformation Matrix:
   0.999914  0.00218668   0.0128978   0.0521499
-0.00329372    0.996251   0.0864454    0.162203
 -0.0126604  -0.0864805    0.996173  -0.0274677
          0           0           0           1
shift: 0.170381
-----------------------------------------------------------------
(Processed/Input): (115 / 115)
-----------------------------------------------------------------
Sequence number: 5045
Number of scan points: 7048 points.
Number of filtered scan points: 1301 points.
transformed_scan_ptr: 7048 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.1778
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0603184, 0.139445, -0.0262469, -0.075938, 0.0108267, -0.00972072)
Transformation Matrix:
   0.999894  0.00887123   0.0115322   0.0603184
-0.00971999    0.997079   0.0757565    0.139445
 -0.0108265  -0.0758606     0.99706  -0.0262469
          0           0           0           1
shift: 0.151932
-----------------------------------------------------------------
(Processed/Input): (116 / 116)
-----------------------------------------------------------------
Sequence number: 5046
Number of scan points: 7113 points.
Number of filtered scan points: 1300 points.
transformed_scan_ptr: 7113 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.5113
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0543637, 0.101657, -0.0355411, -0.0683677, 0.00794078, -0.00445222)
Transformation Matrix:
   0.999959  0.00389935  0.00822621   0.0543637
-0.00445206    0.997656   0.0682785    0.101657
-0.00794069  -0.0683123    0.997632  -0.0355411
          0           0           0           1
shift: 0.115281
-----------------------------------------------------------------
(Processed/Input): (117 / 117)
-----------------------------------------------------------------
Sequence number: 5047
Number of scan points: 7082 points.
Number of filtered scan points: 1239 points.
transformed_scan_ptr: 7082 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.3354
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0357579, 0.159898, -0.0320713, -0.061768, 0.00315277, 0.00144383)
Transformation Matrix:
   0.999994 -0.00163569  0.00305763   0.0357579
 0.00144382    0.998092   0.0617332    0.159898
-0.00315277  -0.0617284    0.998088  -0.0320713
          0           0           0           1
shift: 0.163847
-----------------------------------------------------------------
(Processed/Input): (118 / 118)
-----------------------------------------------------------------
Sequence number: 5048
Number of scan points: 7129 points.
Number of filtered scan points: 1234 points.
transformed_scan_ptr: 7129 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.1935
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0424806, 0.158281, -0.0306162, -0.0569944, -0.000325097, -0.00144805)
Transformation Matrix:
    0.999999   0.00146422 -0.000242082    0.0424806
 -0.00144805     0.998375     0.056964     0.158281
 0.000325097   -0.0569636     0.998376   -0.0306162
           0            0            0            1
shift: 0.163882
-----------------------------------------------------------------
(Processed/Input): (119 / 119)
-----------------------------------------------------------------
Sequence number: 5049
Number of scan points: 7283 points.
Number of filtered scan points: 1267 points.
transformed_scan_ptr: 7283 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.6219
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.00686913, 0.135246, -0.0330518, -0.0551299, -0.00292746, 0.000314221)
Transformation Matrix:
    0.999996 -0.000152437  -0.00294032  -0.00686913
  0.00031422     0.998481     0.055101     0.135246
  0.00292746   -0.0551017     0.998476   -0.0330518
           0            0            0            1
shift: 0.135421
-----------------------------------------------------------------
(Processed/Input): (120 / 120)
-----------------------------------------------------------------
Sequence number: 5050
Number of scan points: 7326 points.
Number of filtered scan points: 1278 points.
transformed_scan_ptr: 7326 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.9809
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.000383658, 0.152533, -0.0329768, -0.0517321, -0.00437964, -0.00134324)
Transformation Matrix:
    0.99999  0.00156791 -0.00430431 0.000383658
-0.00134323    0.998661   0.0517149    0.152533
 0.00437963  -0.0517085    0.998653  -0.0329768
          0           0           0           1
shift: 0.152534
-----------------------------------------------------------------
(Processed/Input): (121 / 121)
-----------------------------------------------------------------
Sequence number: 5051
Number of scan points: 7305 points.
Number of filtered scan points: 1297 points.
transformed_scan_ptr: 7305 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.0025
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0191319, 0.132873, -0.0265598, -0.046045, -0.00410037, -0.00257072)
Transformation Matrix:
   0.999988  0.00275672 -0.00397768   0.0191319
-0.00257069    0.998936   0.0460391    0.132873
 0.00410036  -0.0460283    0.998932  -0.0265598
          0           0           0           1
shift: 0.134244
-----------------------------------------------------------------
(Processed/Input): (122 / 122)
-----------------------------------------------------------------
Sequence number: 5052
Number of scan points: 7276 points.
Number of filtered scan points: 1287 points.
transformed_scan_ptr: 7276 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5321
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0189404, 0.0766622, -0.0278394, -0.0383174, -0.00234698, -3.78246e-05)
Transformation Matrix:
    0.999997  0.000127704   -0.0023438    0.0189404
-3.78245e-05     0.999266    0.0383081    0.0766622
  0.00234698   -0.0383079     0.999263   -0.0278394
           0            0            0            1
shift: 0.0789673
-----------------------------------------------------------------
(Processed/Input): (123 / 123)
-----------------------------------------------------------------
Sequence number: 5053
Number of scan points: 7337 points.
Number of filtered scan points: 1310 points.
transformed_scan_ptr: 7337 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.8907
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0257338, 0.066903, -0.0292084, -0.0314757, 0.000193211, -0.00381369)
Transformation Matrix:
    0.999993   0.00380571   0.00031313    0.0257338
 -0.00381368     0.999497    0.0314695     0.066903
-0.000193211   -0.0314705     0.999505   -0.0292084
           0            0            0            1
shift: 0.0716815
-----------------------------------------------------------------
(Processed/Input): (124 / 124)
-----------------------------------------------------------------
Sequence number: 5054
Number of scan points: 7194 points.
Number of filtered scan points: 1292 points.
transformed_scan_ptr: 7194 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.2644
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.00784761, 0.0424817, -0.0276488, -0.0236135, 0.00116694, -0.000136461)
Transformation Matrix:
    0.999999  0.000108869   0.00116983   0.00784761
-0.000136461     0.999721    0.0236112    0.0424817
 -0.00116694   -0.0236113     0.999721   -0.0276488
           0            0            0            1
shift: 0.0432004
-----------------------------------------------------------------
(Processed/Input): (125 / 125)
-----------------------------------------------------------------
Sequence number: 5055
Number of scan points: 7231 points.
Number of filtered scan points: 1283 points.
transformed_scan_ptr: 7231 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.9718
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0346694, 0.00742362, -0.0309244, -0.011013, 0.00109989, -0.00155081)
Transformation Matrix:
   0.999998   0.0015386  0.00111691   0.0346694
-0.00155081    0.999938    0.011011  0.00742362
-0.00109989  -0.0110128    0.999939  -0.0309244
          0           0           0           1
shift: 0.0354553
-----------------------------------------------------------------
(Processed/Input): (126 / 126)
-----------------------------------------------------------------
Sequence number: 5056
Number of scan points: 7129 points.
Number of filtered scan points: 1295 points.
transformed_scan_ptr: 7129 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0703
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0222326, -0.0258289, -0.0332112, -0.00300699, 0.000460467, -0.0058936)
Transformation Matrix:
    0.999983   0.00589215  0.000478182    0.0222326
 -0.00589357     0.999978   0.00300422   -0.0258289
-0.000460467  -0.00300699     0.999995   -0.0332112
           0            0            0            1
shift: 0.0340796
-----------------------------------------------------------------
(Processed/Input): (127 / 127)
-----------------------------------------------------------------
Sequence number: 5057
Number of scan points: 6990 points.
Number of filtered scan points: 1295 points.
transformed_scan_ptr: 6990 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.8237
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.00626604, -0.0314568, -0.0365902, 0.00321551, -0.00024553, -0.00693753)
Transformation Matrix:
    0.999976   0.00693664 -0.000267833   0.00626604
 -0.00693747     0.999971  -0.00321373   -0.0314568
  0.00024553   0.00321551     0.999995   -0.0365902
           0            0            0            1
shift: 0.0320748
-----------------------------------------------------------------
(Processed/Input): (128 / 128)
-----------------------------------------------------------------
Sequence number: 5058
Number of scan points: 7155 points.
Number of filtered scan points: 1312 points.
transformed_scan_ptr: 7155 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.8602
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0447957, -0.0446781, -0.0414114, 0.00909352, -0.00032547, -0.000509477)
Transformation Matrix:
           1  0.000506495 -0.000330091    0.0447957
-0.000509477     0.999959  -0.00909323   -0.0446781
  0.00032547    0.0090934     0.999959   -0.0414114
           0            0            0            1
shift: 0.0632675
-----------------------------------------------------------------
(Processed/Input): (129 / 129)
-----------------------------------------------------------------
Sequence number: 5059
Number of scan points: 7124 points.
Number of filtered scan points: 1301 points.
transformed_scan_ptr: 7124 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0566
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0546452, -0.0402249, -0.0402044, 0.0151256, -0.00177421, -0.000574136)
Transformation Matrix:
    0.999998  0.000547237  -0.00178269    0.0546452
-0.000574135     0.999885    -0.015124   -0.0402249
  0.00177421     0.015125     0.999884   -0.0402044
           0            0            0            1
shift: 0.0678538
-----------------------------------------------------------------
(Processed/Input): (130 / 130)
-----------------------------------------------------------------
Sequence number: 5060
Number of scan points: 7137 points.
Number of filtered scan points: 1295 points.
transformed_scan_ptr: 7137 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0773
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.00857523, -0.0891031, -0.0386838, 0.0243924, -0.00430001, -0.0029264)
Transformation Matrix:
   0.999986  0.00282065 -0.00437007  0.00857523
-0.00292637    0.999699  -0.0243773  -0.0891031
 0.00429999   0.0243897    0.999693  -0.0386838
          0           0           0           1
shift: 0.0895148
-----------------------------------------------------------------
(Processed/Input): (131 / 131)
-----------------------------------------------------------------
Sequence number: 5061
Number of scan points: 7305 points.
Number of filtered scan points: 1343 points.
transformed_scan_ptr: 7305 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.2538
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0303551, -0.101639, -0.0366099, 0.0342179, -0.0038531, -0.00139076)
Transformation Matrix:
   0.999992  0.00125813 -0.00389841   0.0303551
-0.00139075    0.999414  -0.0342058   -0.101639
 0.00385309    0.034211    0.999407  -0.0366099
          0           0           0           1
shift: 0.106075
-----------------------------------------------------------------
(Processed/Input): (132 / 132)
-----------------------------------------------------------------
Sequence number: 5062
Number of scan points: 7421 points.
Number of filtered scan points: 1365 points.
transformed_scan_ptr: 7421 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.7281
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0615892, -0.138856, -0.0291789, 0.0438394, -0.00153285, -1.47598e-05)
Transformation Matrix:
    0.999999 -5.24316e-05  -0.00153202    0.0615892
-1.47598e-05     0.999039   -0.0438253    -0.138856
  0.00153285    0.0438253     0.999038   -0.0291789
           0            0            0            1
shift: 0.151902
-----------------------------------------------------------------
(Processed/Input): (133 / 133)
-----------------------------------------------------------------
Sequence number: 5063
Number of scan points: 7212 points.
Number of filtered scan points: 1359 points.
transformed_scan_ptr: 7212 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.3472
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.015472, -0.118521, -0.0240647, 0.0545069, 0.000887762, -0.000781599)
Transformation Matrix:
    0.999999  0.000828803  0.000843862     0.015472
-0.000781599     0.998515   -0.0544806    -0.118521
-0.000887762    0.0544799     0.998514   -0.0240647
           0            0            0            1
shift: 0.119526
-----------------------------------------------------------------
(Processed/Input): (134 / 134)
-----------------------------------------------------------------
Sequence number: 5064
Number of scan points: 7206 points.
Number of filtered scan points: 1347 points.
transformed_scan_ptr: 7206 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5796
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0318306, -0.176032, -0.0202559, 0.0664452, 0.00377339, 0.000883863)
Transformation Matrix:
    0.999992 -0.000631373   0.00382374    0.0318306
 0.000883857     0.997793    -0.066393    -0.176032
 -0.00377338    0.0663959     0.997786   -0.0202559
           0            0            0            1
shift: 0.178887
-----------------------------------------------------------------
(Processed/Input): (135 / 135)
-----------------------------------------------------------------
Sequence number: 5065
Number of scan points: 7278 points.
Number of filtered scan points: 1359 points.
transformed_scan_ptr: 7278 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.2346
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0617918, -0.226578, -0.023551, 0.0746609, 0.00429005, 0.00187488)
Transformation Matrix:
   0.999989 -0.00154965  0.00441793   0.0617918
 0.00187486    0.997213  -0.0745834   -0.226578
-0.00429004   0.0745909    0.997205   -0.023551
          0           0           0           1
shift: 0.234852
-----------------------------------------------------------------
(Processed/Input): (136 / 136)
-----------------------------------------------------------------
Sequence number: 5066
Number of scan points: 7237 points.
Number of filtered scan points: 1345 points.
transformed_scan_ptr: 7237 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.9008
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0567735, -0.211758, -0.0257359, 0.0785888, 0.00403826, 0.00375636)
Transformation Matrix:
   0.999985 -0.00342772  0.00432066   0.0567735
 0.00375632    0.996908  -0.0784922   -0.211758
-0.00403825   0.0785073    0.996905  -0.0257359
          0           0           0           1
shift: 0.219236
-----------------------------------------------------------------
(Processed/Input): (137 / 137)
-----------------------------------------------------------------
Sequence number: 5067
Number of scan points: 7294 points.
Number of filtered scan points: 1389 points.
transformed_scan_ptr: 7294 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.1291
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0469371, -0.174889, -0.0283088, 0.0787356, 0.00347016, 0.000478587)
Transformation Matrix:
    0.999994 -0.000204163   0.00349705    0.0469371
 0.000478584     0.996902   -0.0786526    -0.174889
 -0.00347015    0.0786538     0.996896   -0.0283088
           0            0            0            1
shift: 0.181078
-----------------------------------------------------------------
(Processed/Input): (138 / 138)
-----------------------------------------------------------------
Sequence number: 5068
Number of scan points: 7224 points.
Number of filtered scan points: 1359 points.
transformed_scan_ptr: 7224 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.2572
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0311445, -0.190609, -0.0291862, 0.0736098, 0.00110611, -0.000298279)
Transformation Matrix:
    0.999999  0.000378819   0.00108118    0.0311445
-0.000298278     0.997292   -0.0735437    -0.190609
 -0.00110611    0.0735433     0.997291   -0.0291862
           0            0            0            1
shift: 0.193137
-----------------------------------------------------------------
(Processed/Input): (139 / 139)
-----------------------------------------------------------------
Sequence number: 5069
Number of scan points: 7254 points.
Number of filtered scan points: 1413 points.
transformed_scan_ptr: 7254 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.702
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0127686, -0.235002, -0.0313169, 0.0661448, -0.00138082, -0.00124018)
Transformation Matrix:
   0.999998   0.0011462 -0.00145977  -0.0127686
-0.00124017    0.997813  -0.0660948   -0.235002
 0.00138082   0.0660965    0.997812  -0.0313169
          0           0           0           1
shift: 0.235349
-----------------------------------------------------------------
(Processed/Input): (140 / 140)
-----------------------------------------------------------------
Sequence number: 5070
Number of scan points: 7421 points.
Number of filtered scan points: 1381 points.
transformed_scan_ptr: 7421 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0304
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.00225626, -0.131751, -0.0329548, 0.0540285, -0.00282732, -0.00519893)
Transformation Matrix:
   0.999982  0.00503864  -0.0031039  0.00225626
-0.00519888    0.998528  -0.0539868   -0.131751
 0.00282732    0.054002    0.998537  -0.0329548
          0           0           0           1
shift: 0.13177
-----------------------------------------------------------------
(Processed/Input): (141 / 141)
-----------------------------------------------------------------
Sequence number: 5071
Number of scan points: 7291 points.
Number of filtered scan points: 1351 points.
transformed_scan_ptr: 7291 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.8368
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0432318, -0.135728, -0.0303857, 0.0411924, -0.00283609, 3.82458e-05)
Transformation Matrix:
    0.999996 -0.000155002   -0.0028321    0.0432318
 3.82456e-05     0.999152   -0.0411808    -0.135728
  0.00283608    0.0411805     0.999148   -0.0303857
           0            0            0            1
shift: 0.142447
-----------------------------------------------------------------
(Processed/Input): (142 / 142)
-----------------------------------------------------------------
Sequence number: 5072
Number of scan points: 7382 points.
Number of filtered scan points: 1350 points.
transformed_scan_ptr: 7382 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.8915
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.00129579, -0.0992464, -0.0328765, 0.026123, -0.00112726, -0.00575553)
Transformation Matrix:
   0.999983  0.00572409 -0.00127721 -0.00129579
 -0.0057555    0.999642  -0.0261131  -0.0992464
 0.00112726     0.02612    0.999658  -0.0328765
          0           0           0           1
shift: 0.0992548
-----------------------------------------------------------------
(Processed/Input): (143 / 143)
-----------------------------------------------------------------
Sequence number: 5073
Number of scan points: 7494 points.
Number of filtered scan points: 1369 points.
transformed_scan_ptr: 7494 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0969
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0218422, -0.0525822, -0.0353581, 0.0170763, 0.000668426, 0.00135214)
Transformation Matrix:
    0.999999  -0.00134053  0.000691416    0.0218422
  0.00135214     0.999853   -0.0170746   -0.0525822
-0.000668426    0.0170755     0.999854   -0.0353581
           0            0            0            1
shift: 0.0569383
-----------------------------------------------------------------
(Processed/Input): (144 / 144)
-----------------------------------------------------------------
Sequence number: 5074
Number of scan points: 7386 points.
Number of filtered scan points: 1358 points.
transformed_scan_ptr: 7386 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.6698
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.00864692, -0.0924676, -0.0405845, 0.0106825, 0.00193265, -0.00107217)
Transformation Matrix:
   0.999998  0.00109274  0.00192109  0.00864692
-0.00107217    0.999942  -0.0106844  -0.0924676
-0.00193265   0.0106823    0.999941  -0.0405845
          0           0           0           1
shift: 0.092871
-----------------------------------------------------------------
(Processed/Input): (145 / 145)
-----------------------------------------------------------------
Sequence number: 5075
Number of scan points: 7405 points.
Number of filtered scan points: 1350 points.
transformed_scan_ptr: 7405 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.6014
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.023236, -0.0201137, -0.0445342, 0.00550841, -0.000809378, -0.0022675)
Transformation Matrix:
    0.999997   0.00226301 -0.000821851     0.023236
  -0.0022675     0.999982  -0.00550654   -0.0201137
 0.000809378   0.00550838     0.999985   -0.0445342
           0            0            0            1
shift: 0.0307322
-----------------------------------------------------------------
(Processed/Input): (146 / 146)
-----------------------------------------------------------------
Sequence number: 5076
Number of scan points: 7666 points.
Number of filtered scan points: 1336 points.
transformed_scan_ptr: 7666 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.1564
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0295558, -0.0120297, -0.0451296, 0.00223987, -0.00623609, -0.00350073)
Transformation Matrix:
   0.999974  0.00348674 -0.00624384   0.0295558
-0.00350066    0.999991 -0.00221801  -0.0120297
 0.00623605  0.00223983    0.999978  -0.0451296
          0           0           0           1
shift: 0.0319102
-----------------------------------------------------------------
(Processed/Input): (147 / 147)
-----------------------------------------------------------------
Sequence number: 5077
Number of scan points: 7476 points.
Number of filtered scan points: 1325 points.
transformed_scan_ptr: 7476 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.1329
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0409506, -0.0149253, -0.0400517, 0.000654922, -0.00955437, -0.00839366)
Transformation Matrix:
    0.999919    0.0083873  -0.00955938    0.0409506
 -0.00839317     0.999965 -0.000574699   -0.0149253
  0.00955422  0.000654892     0.999954   -0.0400517
           0            0            0            1
shift: 0.0435857
-----------------------------------------------------------------
(Processed/Input): (148 / 148)
-----------------------------------------------------------------
Sequence number: 5078
Number of scan points: 7526 points.
Number of filtered scan points: 1332 points.
transformed_scan_ptr: 7526 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.6233
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.00545582, 0.0720231, -0.0366036, -0.00203198, -0.00783109, -0.00621034)
Transformation Matrix:
    0.99995   0.0062262 -0.00781823 -0.00545582
-0.00621011    0.999979  0.00208055   0.0720231
 0.00783101 -0.00203192    0.999967  -0.0366036
          0           0           0           1
shift: 0.0722295
-----------------------------------------------------------------
(Processed/Input): (149 / 149)
-----------------------------------------------------------------
Sequence number: 5079
Number of scan points: 7624 points.
Number of filtered scan points: 1321 points.
transformed_scan_ptr: 7624 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5332
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0194779, 0.0348234, -0.0361052, -0.00199737, -0.00344412, -0.00131314)
Transformation Matrix:
   0.999993  0.00132004 -0.00344147   0.0194779
-0.00131313    0.999997  0.00200191   0.0348234
 0.00344411 -0.00199736    0.999992  -0.0361052
          0           0           0           1
shift: 0.0399006
-----------------------------------------------------------------
(Processed/Input): (150 / 150)
-----------------------------------------------------------------
Sequence number: 5080
Number of scan points: 7737 points.
Number of filtered scan points: 1321 points.
transformed_scan_ptr: 7737 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.7247
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0332124, 0.0157398, -0.0357834, -7.2823e-05, -4.69519e-05, -0.000955666)
Transformation Matrix:
           1  0.000955669 -4.68798e-05    0.0332124
-0.000955665            1  7.28695e-05    0.0157398
 4.69519e-05  -7.2823e-05            1   -0.0357834
           0            0            0            1
shift: 0.0367533
-----------------------------------------------------------------
(Processed/Input): (151 / 151)
-----------------------------------------------------------------
Sequence number: 5081
Number of scan points: 7694 points.
Number of filtered scan points: 1317 points.
transformed_scan_ptr: 7694 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.7753
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0340195, 0.0175734, -0.0424919, 0.00209918, 0.000513233, -0.00888018)
Transformation Matrix:
     0.99996   0.00888112  0.000494564    0.0340195
 -0.00888006     0.999958  -0.00210365    0.0175734
-0.000513233   0.00209918     0.999998   -0.0424919
           0            0            0            1
shift: 0.0382904
-----------------------------------------------------------------
(Processed/Input): (152 / 152)
-----------------------------------------------------------------
Sequence number: 5082
Number of scan points: 7838 points.
Number of filtered scan points: 1311 points.
transformed_scan_ptr: 7838 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.6994
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0220248, 0.0222601, -0.0473676, 0.00475422, -0.00327161, -0.000343909)
Transformation Matrix:
    0.999995   0.00032837   -0.0032732    0.0220248
-0.000343907     0.999989  -0.00475308    0.0222601
   0.0032716   0.00475418     0.999983   -0.0473676
           0            0            0            1
shift: 0.0313146
-----------------------------------------------------------------
(Processed/Input): (153 / 153)
-----------------------------------------------------------------
Sequence number: 5083
Number of scan points: 7899 points.
Number of filtered scan points: 1280 points.
transformed_scan_ptr: 7899 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.8434
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.038895, -0.0314626, -0.0528095, 0.00584214, -0.00405583, 0.0013786)
Transformation Matrix:
   0.999991  -0.0014023 -0.00404768    0.038895
 0.00137859    0.999982  -0.0058477  -0.0314626
 0.00405582  0.00584206    0.999975  -0.0528095
          0           0           0           1
shift: 0.0500272
-----------------------------------------------------------------
(Processed/Input): (154 / 154)
-----------------------------------------------------------------
Sequence number: 5084
Number of scan points: 7791 points.
Number of filtered scan points: 1262 points.
transformed_scan_ptr: 7791 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5757
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0584129, -0.00881163, -0.0529777, 0.0114655, -0.00582401, -0.000817818)
Transformation Matrix:
    0.999983  0.000751011  -0.00583297    0.0584129
-0.000817804     0.999934   -0.0114605  -0.00881163
  0.00582398    0.0114651     0.999917   -0.0529777
           0            0            0            1
shift: 0.0590738
-----------------------------------------------------------------
(Processed/Input): (155 / 155)
-----------------------------------------------------------------
Sequence number: 5085
Number of scan points: 7817 points.
Number of filtered scan points: 1263 points.
transformed_scan_ptr: 7817 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5611
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0251445, -0.0469493, -0.0503099, 0.0124893, -0.00513143, -0.0032899)
Transformation Matrix:
   0.999981  0.00322557 -0.00517206   0.0251445
-0.00328985    0.999917   -0.012472  -0.0469493
 0.00513141   0.0124888    0.999909  -0.0503099
          0           0           0           1
shift: 0.0532587
-----------------------------------------------------------------
(Processed/Input): (156 / 156)
-----------------------------------------------------------------
Sequence number: 5086
Number of scan points: 7854 points.
Number of filtered scan points: 1266 points.
transformed_scan_ptr: 7854 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.4141
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0265637, -0.0782145, -0.0523898, 0.0192876, -0.00429492, -0.00218482)
Transformation Matrix:
   0.999988  0.00210157 -0.00433623   0.0265637
 -0.0021848    0.999812   -0.019277  -0.0782145
  0.0042949   0.0192863    0.999805  -0.0523898
          0           0           0           1
shift: 0.0826023
-----------------------------------------------------------------
(Processed/Input): (157 / 157)
-----------------------------------------------------------------
Sequence number: 5087
Number of scan points: 7962 points.
Number of filtered scan points: 1270 points.
transformed_scan_ptr: 7962 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5552
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.0368588, -0.0756898, -0.0579301, 0.0277418, -0.00429329, -0.00194628)
Transformation Matrix:
   0.999989  0.00182644  -0.0043456  -0.0368588
-0.00194626    0.999614  -0.0277298  -0.0756898
 0.00429327   0.0277379    0.999606  -0.0579301
          0           0           0           1
shift: 0.0841874
-----------------------------------------------------------------
(Processed/Input): (158 / 158)
-----------------------------------------------------------------
Sequence number: 5088
Number of scan points: 7836 points.
Number of filtered scan points: 1230 points.
transformed_scan_ptr: 7836 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.1038
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0167895, -0.109621, -0.0563621, 0.0346734, -0.00566695, -0.00304725)
Transformation Matrix:
   0.999979  0.00284896 -0.00576913   0.0167895
-0.00304719    0.999395   -0.034649   -0.109621
 0.00566692   0.0346659    0.999383  -0.0563621
          0           0           0           1
shift: 0.110899
-----------------------------------------------------------------
(Processed/Input): (159 / 159)
-----------------------------------------------------------------
Sequence number: 5089
Number of scan points: 8021 points.
Number of filtered scan points: 1239 points.
transformed_scan_ptr: 8021 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.3871
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.0128441, -0.0589515, -0.0589724, 0.039945, -0.0052483, -0.00518416)
Transformation Matrix:
   0.999973  0.00497042 -0.00545104  -0.0128441
-0.00518406     0.99919  -0.0399067  -0.0589515
 0.00524828   0.0399338    0.999189  -0.0589724
          0           0           0           1
shift: 0.0603345
-----------------------------------------------------------------
(Processed/Input): (160 / 160)
-----------------------------------------------------------------
Sequence number: 5090
Number of scan points: 7996 points.
Number of filtered scan points: 1290 points.
transformed_scan_ptr: 7996 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.9607
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0325749, -0.0858909, -0.0557056, 0.0390083, -0.00611984, -0.00926574)
Transformation Matrix:
   0.999938  0.00901991 -0.00647622  -0.0325749
-0.00926543    0.999199  -0.0389401  -0.0858909
  0.0061198   0.0389977    0.999221  -0.0557056
          0           0           0           1
shift: 0.0918606
-----------------------------------------------------------------
(Processed/Input): (161 / 161)
-----------------------------------------------------------------
Sequence number: 5091
Number of scan points: 8109 points.
Number of filtered scan points: 1246 points.
transformed_scan_ptr: 8109 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.6929
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(4.67571e-05, -0.0870761, -0.0548296, 0.0351405, -0.00704508, -0.00270456)
Transformation Matrix:
   0.999972  0.00245537 -0.00713567 4.67571e-05
-0.00270449     0.99938  -0.0351141  -0.0870761
 0.00704502   0.0351324    0.999358  -0.0548296
          0           0           0           1
shift: 0.0870761
-----------------------------------------------------------------
(Processed/Input): (162 / 162)
-----------------------------------------------------------------
Sequence number: 5092
Number of scan points: 8135 points.
Number of filtered scan points: 1254 points.
transformed_scan_ptr: 8135 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.6588
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.00826185, -0.109608, -0.0546238, 0.030486, -0.00506037, -0.00332156)
Transformation Matrix:
   0.999982  0.00316576 -0.00515922  0.00826185
-0.00332151     0.99953  -0.0304643   -0.109608
 0.00506035   0.0304809    0.999523  -0.0546238
          0           0           0           1
shift: 0.109919
-----------------------------------------------------------------
(Processed/Input): (163 / 163)
-----------------------------------------------------------------
Sequence number: 5093
Number of scan points: 8035 points.
Number of filtered scan points: 1241 points.
transformed_scan_ptr: 8035 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.7541
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(6.74163e-05, -0.0495511, -0.0527582, 0.0206139, -0.00332542, -0.00427102)
Transformation Matrix:
   0.999985  0.00420155 -0.00341272 6.74163e-05
-0.00427098    0.999779   -0.020598  -0.0495511
 0.00332542   0.0206123    0.999782  -0.0527582
          0           0           0           1
shift: 0.0495511
-----------------------------------------------------------------
(Processed/Input): (164 / 164)
-----------------------------------------------------------------
Sequence number: 5094
Number of scan points: 8172 points.
Number of filtered scan points: 1229 points.
transformed_scan_ptr: 8172 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.1371
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0122484, 0.00450126, -0.0589682, 0.0130838, -0.00213937, -0.00290667)
Transformation Matrix:
   0.999994  0.00287842 -0.00217721  -0.0122484
-0.00290666     0.99991  -0.0130771  0.00450126
 0.00213936   0.0130834    0.999912  -0.0589682
          0           0           0           1
shift: 0.0130493
-----------------------------------------------------------------
(Processed/Input): (165 / 165)
-----------------------------------------------------------------
Sequence number: 5095
Number of scan points: 8318 points.
Number of filtered scan points: 1233 points.
transformed_scan_ptr: 8318 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.2617
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.00189643, 0.0142544, -0.0542964, 0.00279182, -0.00233623, 0.00331477)
Transformation Matrix:
   0.999992 -0.00332129 -0.00232693 -0.00189643
 0.00331476    0.999991 -0.00279956   0.0142544
 0.00233623  0.00279181    0.999993  -0.0542964
          0           0           0           1
shift: 0.01438
-----------------------------------------------------------------
(Processed/Input): (166 / 166)
-----------------------------------------------------------------
Sequence number: 5096
Number of scan points: 8300 points.
Number of filtered scan points: 1238 points.
transformed_scan_ptr: 8300 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.3683
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0165698, 0.0504369, -0.058792, -0.00487548, -0.00435324, 0.00776925)
Transformation Matrix:
    0.99996 -0.00774787 -0.00439091   0.0165698
  0.0077691    0.999958   0.0048415   0.0504369
 0.00435323 -0.00487541    0.999979   -0.058792
          0           0           0           1
shift: 0.053089
-----------------------------------------------------------------
(Processed/Input): (167 / 167)
-----------------------------------------------------------------
Sequence number: 5097
Number of scan points: 8405 points.
Number of filtered scan points: 1233 points.
transformed_scan_ptr: 8405 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.0239
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.032465, 0.016221, -0.0641239, -0.00543138, -0.00386544, 0.00254936)
Transformation Matrix:
   0.999989 -0.00252832 -0.00387921    0.032465
 0.00254934    0.999982  0.00542148    0.016221
 0.00386543 -0.00543132    0.999978  -0.0641239
          0           0           0           1
shift: 0.0362918
-----------------------------------------------------------------
(Processed/Input): (168 / 168)
-----------------------------------------------------------------
Sequence number: 5098
Number of scan points: 8344 points.
Number of filtered scan points: 1211 points.
transformed_scan_ptr: 8344 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.1274
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.00851852, 0.006399, -0.0630711, -0.00772027, -0.00369247, -0.00247701)
Transformation Matrix:
    0.99999  0.00250543 -0.00367323 -0.00851852
-0.00247699    0.999967  0.00772931    0.006399
 0.00369246 -0.00772014    0.999963  -0.0630711
          0           0           0           1
shift: 0.0106542
-----------------------------------------------------------------
(Processed/Input): (169 / 169)
-----------------------------------------------------------------
Sequence number: 5099
Number of scan points: 8299 points.
Number of filtered scan points: 1195 points.
transformed_scan_ptr: 8299 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.0492
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0368276, 0.0124083, -0.062151, -0.00953932, -0.00337597, 0.00396136)
Transformation Matrix:
   0.999986 -0.00392895 -0.00341359  -0.0368276
 0.00396132    0.999947  0.00952572   0.0124083
 0.00337597 -0.00953912    0.999949   -0.062151
          0           0           0           1
shift: 0.0388618
-----------------------------------------------------------------
(Processed/Input): (170 / 170)
-----------------------------------------------------------------
Sequence number: 5100
Number of scan points: 8344 points.
Number of filtered scan points: 1195 points.
transformed_scan_ptr: 8344 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.6995
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0767269, 0.0111568, -0.0610843, -0.0115625, -0.00364051, -0.00030395)
Transformation Matrix:
    0.999993  0.000346039  -0.00363674   -0.0767269
-0.000303948     0.999933    0.0115634    0.0111568
   0.0036405   -0.0115622     0.999927   -0.0610843
           0            0            0            1
shift: 0.0775338
-----------------------------------------------------------------
(Processed/Input): (171 / 171)
-----------------------------------------------------------------
Sequence number: 5101
Number of scan points: 8472 points.
Number of filtered scan points: 1191 points.
transformed_scan_ptr: 8472 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.2256
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0404418, 0.0166068, -0.0745399, -0.00970625, -0.00327622, 0.00459255)
Transformation Matrix:
   0.999984 -0.00456052  -0.0033206  -0.0404418
 0.00459251    0.999942  0.00969095   0.0166068
 0.00327621 -0.00970604    0.999948  -0.0745399
          0           0           0           1
shift: 0.0437187
-----------------------------------------------------------------
(Processed/Input): (172 / 172)
-----------------------------------------------------------------
Sequence number: 5102
Number of scan points: 8343 points.
Number of filtered scan points: 1180 points.
transformed_scan_ptr: 8343 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.4994
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0308884, 0.00661241, -0.0698628, -0.0103873, -0.00420248, 0.00273567)
Transformation Matrix:
   0.999987 -0.00269187 -0.00423064  -0.0308884
 0.00273564    0.999942   0.0103756  0.00661241
 0.00420247  -0.0103871    0.999937  -0.0698628
          0           0           0           1
shift: 0.0315883
-----------------------------------------------------------------
(Processed/Input): (173 / 173)
-----------------------------------------------------------------
Sequence number: 5103
Number of scan points: 8298 points.
Number of filtered scan points: 1194 points.
transformed_scan_ptr: 8298 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.3658
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.048113, 0.0303335, -0.0699999, -0.0102267, -0.00374872, 0.00665562)
Transformation Matrix:
   0.999971 -0.00661688 -0.00381652   -0.048113
 0.00665553    0.999926   0.0102014   0.0303335
 0.00374872  -0.0102265    0.999941  -0.0699999
          0           0           0           1
shift: 0.0568769
-----------------------------------------------------------------
(Processed/Input): (174 / 174)
-----------------------------------------------------------------
Sequence number: 5104
Number of scan points: 8308 points.
Number of filtered scan points: 1164 points.
transformed_scan_ptr: 8308 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.9544
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.00459517, 0.0382371, -0.0726884, -0.0100576, -0.00395448, 0.00463087)
Transformation Matrix:
   0.999981 -0.00459085  -0.0040008 -0.00459517
 0.00463082    0.999939    0.010039   0.0382371
 0.00395447  -0.0100573    0.999942  -0.0726884
          0           0           0           1
shift: 0.0385122
-----------------------------------------------------------------
(Processed/Input): (175 / 175)
-----------------------------------------------------------------
Sequence number: 5105
Number of scan points: 8256 points.
Number of filtered scan points: 1165 points.
transformed_scan_ptr: 8256 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.5094
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0123146, -0.00203909, -0.0752026, -0.010309, -0.0047144, -0.00055375)
Transformation Matrix:
    0.999989  0.000602332  -0.00470842    0.0123146
-0.000553744     0.999947    0.0103115  -0.00203909
  0.00471438   -0.0103087     0.999936   -0.0752026
           0            0            0            1
shift: 0.0124823
-----------------------------------------------------------------
(Processed/Input): (176 / 176)
-----------------------------------------------------------------
Sequence number: 5106
Number of scan points: 8318 points.
Number of filtered scan points: 1187 points.
transformed_scan_ptr: 8318 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.8617
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.00442708, 0.0228598, -0.0805056, -0.0092046, -0.0042781, -0.000913275)
Transformation Matrix:
     0.99999  0.000952635   -0.0042695   0.00442708
-0.000913267     0.999957   0.00920838    0.0228598
  0.00427809  -0.00920439     0.999948   -0.0805056
           0            0            0            1
shift: 0.0232846
-----------------------------------------------------------------
(Processed/Input): (177 / 177)
-----------------------------------------------------------------
Sequence number: 5107
Number of scan points: 8189 points.
Number of filtered scan points: 1191 points.
transformed_scan_ptr: 8189 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.0207
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.00617777, 0.0368639, -0.0834557, -0.00834602, -0.00470616, 0.00782392)
Transformation Matrix:
   0.999958 -0.00778429 -0.00477113 -0.00617777
 0.00782375    0.999935  0.00830885   0.0368639
 0.00470614 -0.00834583    0.999954  -0.0834557
          0           0           0           1
shift: 0.037378
-----------------------------------------------------------------
(Processed/Input): (178 / 178)
-----------------------------------------------------------------
Sequence number: 5108
Number of scan points: 8268 points.
Number of filtered scan points: 1224 points.
transformed_scan_ptr: 8268 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0485
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0455261, 0.05691, -0.0858241, -0.00616855, -0.00502853, 0.0169626)
Transformation Matrix:
   0.999843  -0.0169304 -0.00513234  -0.0455261
  0.0169615    0.999838  0.00608232     0.05691
 0.00502851 -0.00616844    0.999968  -0.0858241
          0           0           0           1
shift: 0.0728792
-----------------------------------------------------------------
(Processed/Input): (179 / 179)
-----------------------------------------------------------------
Sequence number: 5109
Number of scan points: 8130 points.
Number of filtered scan points: 1197 points.
transformed_scan_ptr: 8130 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.1569
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.0952435, 0.0561557, -0.0692598, -0.00733587, -0.00516021, 0.0163344)
Transformation Matrix:
  0.999853 -0.0162953 -0.0052792 -0.0952435
 0.0163334    0.99984 0.00725053  0.0561557
0.00516019 -0.0073357    0.99996 -0.0692598
         0          0          0          1
shift: 0.110566
-----------------------------------------------------------------
(Processed/Input): (180 / 180)
-----------------------------------------------------------------
Sequence number: 5110
Number of scan points: 8114 points.
Number of filtered scan points: 1205 points.
transformed_scan_ptr: 8114 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.1783
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0938466, 0.0682382, -0.0680877, -0.00754846, -0.00545681, 0.0154915)
Transformation Matrix:
   0.999865  -0.0154493 -0.00557293  -0.0938466
  0.0154907    0.999852  0.00746294   0.0682382
 0.00545679 -0.00754827    0.999957  -0.0680877
          0           0           0           1
shift: 0.116033
-----------------------------------------------------------------
(Processed/Input): (181 / 181)
-----------------------------------------------------------------
Sequence number: 5111
Number of scan points: 8171 points.
Number of filtered scan points: 1232 points.
transformed_scan_ptr: 8171 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.4131
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.125944, 0.0942106, -0.0715467, -0.0105286, -0.00294111, 0.0163524)
Transformation Matrix:
  0.999862 -0.0163198 -0.0031127  -0.125944
 0.0163516   0.999811  0.0104789  0.0942106
0.00294111 -0.0105283    0.99994 -0.0715467
         0          0          0          1
shift: 0.157282
-----------------------------------------------------------------
(Processed/Input): (182 / 182)
-----------------------------------------------------------------
Sequence number: 5112
Number of scan points: 7999 points.
Number of filtered scan points: 1241 points.
transformed_scan_ptr: 7999 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.4093
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.120654, 0.0669056, -0.0669824, -0.0147858, -0.00274987, 0.013144)
Transformation Matrix:
    0.99991  -0.0131015 -0.00294366   -0.120654
  0.0131436    0.999805   0.0147479   0.0669056
 0.00274987  -0.0147852    0.999887  -0.0669824
          0           0           0           1
shift: 0.137963
-----------------------------------------------------------------
(Processed/Input): (183 / 183)
-----------------------------------------------------------------
Sequence number: 5113
Number of scan points: 8127 points.
Number of filtered scan points: 1263 points.
transformed_scan_ptr: 8127 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.4699
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.0208482, 0.083414, -0.0669238, -0.0175677, -0.00250011, -0.00251025)
Transformation Matrix:
   0.999994  0.00255378 -0.00245562  -0.0208482
-0.00251024    0.999842    0.017573    0.083414
 0.00250011  -0.0175668    0.999843  -0.0669238
          0           0           0           1
shift: 0.0859799
-----------------------------------------------------------------
(Processed/Input): (184 / 184)
-----------------------------------------------------------------
Sequence number: 5114
Number of scan points: 7991 points.
Number of filtered scan points: 1268 points.
transformed_scan_ptr: 7991 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0953
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(0.0190588, 0.0756336, -0.0636118, -0.0198815, -0.000626746, -0.00338892)
Transformation Matrix:
    0.999994   0.00340071 -0.000559239    0.0190588
 -0.00338892     0.999797    0.0198822    0.0756336
 0.000626746   -0.0198802     0.999802   -0.0636118
           0            0            0            1
shift: 0.0779979
-----------------------------------------------------------------
(Processed/Input): (185 / 185)
-----------------------------------------------------------------
Sequence number: 5115
Number of scan points: 7935 points.
Number of filtered scan points: 1312 points.
transformed_scan_ptr: 7935 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.8241
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0328881, 0.0792092, -0.0690907, -0.0231114, 0.00101298, -0.0040005)
Transformation Matrix:
   0.999991  0.00397601  0.00110516   0.0328881
-0.00400048    0.999725   0.0231051   0.0792092
-0.00101298  -0.0231094    0.999732  -0.0690907
          0           0           0           1
shift: 0.0857655
-----------------------------------------------------------------
(Processed/Input): (186 / 186)
-----------------------------------------------------------------
Sequence number: 5116
Number of scan points: 7874 points.
Number of filtered scan points: 1274 points.
transformed_scan_ptr: 7874 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.8931
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0402942, 0.0890737, -0.0624666, -0.0211108, -0.0019727, -0.00234798)
Transformation Matrix:
   0.999995   0.0023891 -0.00192269   0.0402942
-0.00234798    0.999774   0.0211138   0.0890737
  0.0019727  -0.0211092    0.999775  -0.0624666
          0           0           0           1
shift: 0.0977637
-----------------------------------------------------------------
(Processed/Input): (187 / 187)
-----------------------------------------------------------------
Sequence number: 5117
Number of scan points: 7767 points.
Number of filtered scan points: 1252 points.
transformed_scan_ptr: 7767 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.8845
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0236716, 0.0787701, -0.0702031, -0.0224029, -0.00469595, -0.00131577)
Transformation Matrix:
   0.999988  0.00142064 -0.00466527   0.0236716
-0.00131575    0.999748   0.0224072   0.0787701
 0.00469593  -0.0224008    0.999738  -0.0702031
          0           0           0           1
shift: 0.0822501
-----------------------------------------------------------------
(Processed/Input): (188 / 188)
-----------------------------------------------------------------
Sequence number: 5118
Number of scan points: 7914 points.
Number of filtered scan points: 1304 points.
transformed_scan_ptr: 7914 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.3019
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0114647, 0.0221298, -0.0527873, -0.0149976, -0.00518848, -0.00811211)
Transformation Matrix:
   0.999954  0.00818892 -0.00506605   0.0114647
-0.00811191    0.999854   0.0150386   0.0221298
 0.00518846  -0.0149968    0.999874  -0.0527873
          0           0           0           1
shift: 0.0249232
-----------------------------------------------------------------
(Processed/Input): (189 / 189)
-----------------------------------------------------------------
Sequence number: 5119
Number of scan points: 7783 points.
Number of filtered scan points: 1299 points.
transformed_scan_ptr: 7783 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0304
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0409292, -0.0239848, -0.0501815, -0.0125959, -0.00355789, -0.00756478)
Transformation Matrix:
   0.999965  0.00760892 -0.00346222   0.0409292
-0.00756466    0.999892   0.0126221  -0.0239848
 0.00355789  -0.0125955    0.999914  -0.0501815
          0           0           0           1
shift: 0.0474391
-----------------------------------------------------------------
(Processed/Input): (190 / 190)
-----------------------------------------------------------------
Sequence number: 5120
Number of scan points: 7931 points.
Number of filtered scan points: 1346 points.
transformed_scan_ptr: 7931 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.1241
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0207143, 0.00370689, -0.0506635, -0.00916813, -0.00334478, -0.00789028)
Transformation Matrix:
   0.999963  0.00792053 -0.00327221   0.0207143
-0.00789016    0.999927  0.00919411  0.00370689
 0.00334478 -0.00916795    0.999952  -0.0506635
          0           0           0           1
shift: 0.0210434
-----------------------------------------------------------------
(Processed/Input): (191 / 191)
-----------------------------------------------------------------
Sequence number: 5121
Number of scan points: 7886 points.
Number of filtered scan points: 1388 points.
transformed_scan_ptr: 7886 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.4724
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0274553, 0.0388842, -0.0448617, -0.00216434, -0.00290587, -0.00235491)
Transformation Matrix:
   0.999993   0.0023612 -0.00290074  -0.0274553
 -0.0023549    0.999995  0.00217118   0.0388842
 0.00290586 -0.00216433    0.999993  -0.0448617
          0           0           0           1
shift: 0.0476002
-----------------------------------------------------------------
(Processed/Input): (192 / 192)
-----------------------------------------------------------------
Sequence number: 5122
Number of scan points: 7942 points.
Number of filtered scan points: 1420 points.
transformed_scan_ptr: 7942 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.6031
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0269207, -0.0770206, -0.0414376, 0.00326096, -0.00457072, 0.00118903)
Transformation Matrix:
   0.999989  -0.0012039 -0.00456681   0.0269207
 0.00118901    0.999994 -0.00326638  -0.0770206
 0.00457071  0.00326092    0.999984  -0.0414376
          0           0           0           1
shift: 0.0815898
-----------------------------------------------------------------
(Processed/Input): (193 / 193)
-----------------------------------------------------------------
Sequence number: 5123
Number of scan points: 7862 points.
Number of filtered scan points: 1426 points.
transformed_scan_ptr: 7862 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.2105
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.032652, -0.035986, -0.0283973, 0.00360245, -0.00414802, -0.00180909)
Transformation Matrix:
    0.99999  0.00179415 -0.00415448    0.032652
-0.00180907    0.999992 -0.00359495   -0.035986
 0.00414801  0.00360241    0.999985  -0.0283973
          0           0           0           1
shift: 0.0485916
-----------------------------------------------------------------
(Processed/Input): (194 / 194)
-----------------------------------------------------------------
Sequence number: 5124
Number of scan points: 7783 points.
Number of filtered scan points: 1429 points.
transformed_scan_ptr: 7783 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.0957
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.00707019, -0.0610464, -0.020262, 0.00579402, -0.00444099, -0.00784533)
Transformation Matrix:
   0.999959  0.00781938 -0.00448623  0.00707019
-0.00784517    0.999953 -0.00575896  -0.0610464
 0.00444097  0.00579393    0.999973   -0.020262
          0           0           0           1
shift: 0.0614545
-----------------------------------------------------------------
(Processed/Input): (195 / 195)
-----------------------------------------------------------------
Sequence number: 5125
Number of scan points: 7702 points.
Number of filtered scan points: 1387 points.
transformed_scan_ptr: 7702 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.3316
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0141061, -0.024128, -0.0171447, 0.00785203, -0.00295611, -0.00467602)
Transformation Matrix:
   0.999985  0.00465264  -0.0029927   0.0141061
-0.00467598    0.999958 -0.00783804   -0.024128
 0.00295611  0.00785192    0.999965  -0.0171447
          0           0           0           1
shift: 0.0279489
-----------------------------------------------------------------
(Processed/Input): (196 / 196)
-----------------------------------------------------------------
Sequence number: 5126
Number of scan points: 7629 points.
Number of filtered scan points: 1376 points.
transformed_scan_ptr: 7629 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.1874
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0283543, -0.036094, -0.0128679, 0.00827775, -0.00160883, -0.00207041)
Transformation Matrix:
   0.999997  0.00205702  -0.0016259   0.0283543
 -0.0020704    0.999964 -0.00827431   -0.036094
 0.00160883  0.00827764    0.999964  -0.0128679
          0           0           0           1
shift: 0.0458993
-----------------------------------------------------------------
(Processed/Input): (197 / 197)
-----------------------------------------------------------------
Sequence number: 5127
Number of scan points: 7791 points.
Number of filtered scan points: 1394 points.
transformed_scan_ptr: 7791 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.0499
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0232144, -0.0116303, -0.018992, 0.0104893, -0.00151966, -0.00138283)
Transformation Matrix:
   0.999998  0.00136681 -0.00153407   0.0232144
-0.00138282    0.999944   -0.010487  -0.0116303
 0.00151966   0.0104891    0.999944   -0.018992
          0           0           0           1
shift: 0.0259649
-----------------------------------------------------------------
(Processed/Input): (198 / 198)
(Processed/Input): (199 / 199)
-----------------------------------------------------------------
Sequence number: 5128
Number of scan points: 7931 points.
Number of filtered scan points: 1397 points.
transformed_scan_ptr: 7931 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.2619
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.044866, -0.0244868, -0.0224024, 0.016127, 0.000348965, 0.00240797)
Transformation Matrix:
    0.999997  -0.00240203  0.000387746     0.044866
  0.00240797     0.999867   -0.0161254   -0.0244868
-0.000348965    0.0161263      0.99987   -0.0224024
           0            0            0            1
shift: 0.0511132
-----------------------------------------------------------------
-----------------------------------------------------------------
Sequence number: 5129
Number of scan points: 7781 points.
Number of filtered scan points: 1405 points.
transformed_scan_ptr: 7781 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.7845
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0313537, -0.0561562, -0.0230133, 0.0174921, 0.000651425, 0.000644714)
Transformation Matrix:
           1 -0.000633219  0.000662604    0.0313537
 0.000644713     0.999847   -0.0174908   -0.0561562
-0.000651425    0.0174913     0.999847   -0.0230133
           0            0            0            1
shift: 0.0643162
-----------------------------------------------------------------
(Processed/Input): (200 / 200)
-----------------------------------------------------------------
Sequence number: 5130
Number of scan points: 7646 points.
Number of filtered scan points: 1353 points.
transformed_scan_ptr: 7646 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.7759
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0247291, -0.0457173, -0.0266789, 0.0166835, -0.000600803, -0.000786361)
Transformation Matrix:
           1  0.000776228 -0.000613838    0.0247291
 -0.00078636     0.999861   -0.0166823   -0.0457173
 0.000600803    0.0166828     0.999861   -0.0266789
           0            0            0            1
shift: 0.0519769
-----------------------------------------------------------------
(Processed/Input): (201 / 201)
-----------------------------------------------------------------
Sequence number: 5131
Number of scan points: 7844 points.
Number of filtered scan points: 1389 points.
transformed_scan_ptr: 7844 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.1385
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0401738, -0.0870751, -0.0360069, 0.0189073, 0.000715269, -0.00306394)
Transformation Matrix:
    0.999995   0.00307691  0.000657213    0.0401738
 -0.00306393     0.999817   -0.0189082   -0.0870751
-0.000715269    0.0189061     0.999821   -0.0360069
           0            0            0            1
shift: 0.0958958
-----------------------------------------------------------------
(Processed/Input): (202 / 202)
-----------------------------------------------------------------
Sequence number: 5132
Number of scan points: 7826 points.
Number of filtered scan points: 1397 points.
transformed_scan_ptr: 7826 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.4538
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0248496, -0.10618, -0.0358529, 0.0148399, 0.000444091, -0.00997606)
Transformation Matrix:
     0.99995   0.00998139  0.000296002   -0.0248496
  -0.0099759      0.99984    -0.014843     -0.10618
-0.000444091    0.0148393      0.99989   -0.0358529
           0            0            0            1
shift: 0.109049
-----------------------------------------------------------------
(Processed/Input): (203 / 203)
-----------------------------------------------------------------
Sequence number: 5133
Number of scan points: 7744 points.
Number of filtered scan points: 1346 points.
transformed_scan_ptr: 7744 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.5996
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.00828794, -0.0651988, -0.0337323, 0.0101017, -0.000313915, -0.0033379)
Transformation Matrix:
    0.999994   0.00333456 -0.000347608   0.00828794
  -0.0033379     0.999943   -0.0101005   -0.0651988
 0.000313915    0.0101016     0.999949   -0.0337323
           0            0            0            1
shift: 0.0657234
-----------------------------------------------------------------
(Processed/Input): (204 / 204)
-----------------------------------------------------------------
Sequence number: 5134
Number of scan points: 7749 points.
Number of filtered scan points: 1337 points.
transformed_scan_ptr: 7749 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.5313
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0132526, -0.0274017, -0.0316558, 0.00566596, -0.00166966, -0.00717293)
Transformation Matrix:
   0.999973  0.00716329 -0.00171025  -0.0132526
-0.00717286    0.999958 -0.00565381  -0.0274017
 0.00166966  0.00566593    0.999983  -0.0316558
          0           0           0           1
shift: 0.0304382
-----------------------------------------------------------------
(Processed/Input): (205 / 205)
-----------------------------------------------------------------
Sequence number: 5135
Number of scan points: 7904 points.
Number of filtered scan points: 1311 points.
transformed_scan_ptr: 7904 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.3926
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0282655, -0.00498527, -0.0355699, 0.003207, -0.000892827, -0.00250904)
Transformation Matrix:
    0.999996   0.00250615 -0.000900882    0.0282655
 -0.00250903     0.999992  -0.00320474  -0.00498527
 0.000892827   0.00320699     0.999994   -0.0355699
           0            0            0            1
shift: 0.0287018
-----------------------------------------------------------------
(Processed/Input): (206 / 206)
-----------------------------------------------------------------
Sequence number: 5136
Number of scan points: 7805 points.
Number of filtered scan points: 1260 points.
transformed_scan_ptr: 7805 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.2774
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0348977, -0.0343962, -0.0377596, 0.00350343, 0.00156352, -0.00113419)
Transformation Matrix:
   0.999998  0.00113965  0.00155955   0.0348977
-0.00113419    0.999993 -0.00350519  -0.0343962
-0.00156352  0.00350342    0.999993  -0.0377596
          0           0           0           1
shift: 0.0489995
-----------------------------------------------------------------
(Processed/Input): (207 / 207)
-----------------------------------------------------------------
Sequence number: 5137
Number of scan points: 7842 points.
Number of filtered scan points: 1246 points.
transformed_scan_ptr: 7842 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.5381
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0588554, -0.0618045, -0.05063, 0.0042964, 0.00225607, -0.00557639)
Transformation Matrix:
   0.999982  0.00558601  0.00223205   0.0588554
-0.00557635    0.999975  -0.0043089  -0.0618045
-0.00225607  0.00429637    0.999988    -0.05063
          0           0           0           1
shift: 0.0853449
-----------------------------------------------------------------
(Processed/Input): (208 / 208)
-----------------------------------------------------------------
Sequence number: 5138
Number of scan points: 7834 points.
Number of filtered scan points: 1214 points.
transformed_scan_ptr: 7834 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.6192
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0646678, -0.0803488, -0.0623183, 0.00665607, -0.00051686, -0.00242843)
Transformation Matrix:
   0.999997  0.00242494   -0.000533   0.0646678
-0.00242843    0.999975 -0.00665474  -0.0803488
 0.00051686  0.00665602    0.999978  -0.0623183
          0           0           0           1
shift: 0.10314
-----------------------------------------------------------------
(Processed/Input): (209 / 209)
-----------------------------------------------------------------
Sequence number: 5139
Number of scan points: 7849 points.
Number of filtered scan points: 1208 points.
transformed_scan_ptr: 7849 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.9341
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0304127, -0.0167854, -0.0662003, 0.0101534, -0.00299099, -0.00624215)
Transformation Matrix:
   0.999976  0.00621143 -0.00305415   0.0304127
-0.00624209    0.999929  -0.0101344  -0.0167854
 0.00299099   0.0101532    0.999944  -0.0662003
          0           0           0           1
shift: 0.0347373
-----------------------------------------------------------------
(Processed/Input): (210 / 210)
-----------------------------------------------------------------
Sequence number: 5140
Number of scan points: 7841 points.
Number of filtered scan points: 1202 points.
transformed_scan_ptr: 7841 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 30.4287
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0571233, -0.0405708, -0.0510233, 0.0111394, -0.00344795, -0.0013111)
Transformation Matrix:
   0.999993   0.0012726 -0.00346234   0.0571233
 -0.0013111    0.999937  -0.0111347  -0.0405708
 0.00344794   0.0111391    0.999932  -0.0510233
          0           0           0           1
shift: 0.0700647
-----------------------------------------------------------------
(Processed/Input): (211 / 211)
-----------------------------------------------------------------
Sequence number: 5141
Number of scan points: 8008 points.
Number of filtered scan points: 1229 points.
transformed_scan_ptr: 8008 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.8821
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.00809955, -0.00611001, -0.0463901, 0.011668, -0.00115343, -0.00410013)
Transformation Matrix:
   0.999991  0.00408637 -0.00120119  0.00809955
-0.00410011    0.999924  -0.0116629 -0.00611001
 0.00115343   0.0116677    0.999931  -0.0463901
          0           0           0           1
shift: 0.0101457
-----------------------------------------------------------------
(Processed/Input): (212 / 212)
-----------------------------------------------------------------
Sequence number: 5142
Number of scan points: 7983 points.
Number of filtered scan points: 1212 points.
transformed_scan_ptr: 7983 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.9114
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0558217, -0.0686033, -0.0477295, 0.0139878, -0.00364264, 0.00221669)
Transformation Matrix:
   0.999991 -0.00226743 -0.00361126   0.0558217
 0.00221668      0.9999  -0.0139954  -0.0686033
 0.00364263   0.0139873    0.999896  -0.0477295
          0           0           0           1
shift: 0.0884447
-----------------------------------------------------------------
(Processed/Input): (213 / 213)
-----------------------------------------------------------------
Sequence number: 5143
Number of scan points: 8122 points.
Number of filtered scan points: 1251 points.
transformed_scan_ptr: 8122 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 30.6267
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.083328, -0.0607016, -0.0498598, 0.0144917, -0.000665749, 0.00341155)
Transformation Matrix:
    0.999994  -0.00342083 -0.000616245     0.083328
  0.00341154     0.999889   -0.0144933   -0.0607016
 0.000665749    0.0144912     0.999895   -0.0498598
           0            0            0            1
shift: 0.103093
-----------------------------------------------------------------
(Processed/Input): (214 / 214)
-----------------------------------------------------------------
Sequence number: 5144
Number of scan points: 7858 points.
Number of filtered scan points: 1237 points.
transformed_scan_ptr: 7858 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 30.2948
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0805397, -0.0747217, -0.046224, 0.014566, -0.00160663, 0.00190457)
Transformation Matrix:
   0.999997 -0.00192777 -0.00157872   0.0805397
 0.00190457    0.999892  -0.0145685  -0.0747217
 0.00160663   0.0145654    0.999893   -0.046224
          0           0           0           1
shift: 0.109863
-----------------------------------------------------------------
(Processed/Input): (215 / 215)
-----------------------------------------------------------------
Sequence number: 5145
Number of scan points: 7842 points.
Number of filtered scan points: 1251 points.
transformed_scan_ptr: 7842 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 30.105
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0656631, -0.0402586, -0.0476449, 0.016316, -0.00279502, -0.00356123)
Transformation Matrix:
    0.99999  0.00351514 -0.00285274   0.0656631
-0.00356121    0.999861  -0.0163052  -0.0402586
 0.00279502   0.0163152    0.999863  -0.0476449
          0           0           0           1
shift: 0.0770221
-----------------------------------------------------------------
(Processed/Input): (216 / 216)
-----------------------------------------------------------------
Sequence number: 5146
Number of scan points: 7846 points.
Number of filtered scan points: 1251 points.
transformed_scan_ptr: 7846 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.5763
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0251926, 0.000359632, -0.0377522, 0.011619, -0.00290901, -0.00401059)
Transformation Matrix:
   0.999988  0.00397651 -0.00295538   0.0251926
-0.00401056    0.999925   -0.011607 0.000359632
 0.00290901   0.0116187    0.999928  -0.0377522
          0           0           0           1
shift: 0.0251952
-----------------------------------------------------------------
(Processed/Input): (217 / 217)
-----------------------------------------------------------------
Sequence number: 5147
Number of scan points: 8015 points.
Number of filtered scan points: 1260 points.
transformed_scan_ptr: 8015 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.4528
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.00108288, 0.00604012, -0.0359345, 0.00554944, -0.00154418, -0.00895192)
Transformation Matrix:
   0.999959  0.00894309 -0.00159378 -0.00108288
-0.00895179    0.999945 -0.00553536  0.00604012
 0.00154418   0.0055494    0.999983  -0.0359345
          0           0           0           1
shift: 0.00613642
-----------------------------------------------------------------
(Processed/Input): (218 / 218)
-----------------------------------------------------------------
Sequence number: 5148
Number of scan points: 7928 points.
Number of filtered scan points: 1269 points.
transformed_scan_ptr: 7928 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.6114
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0117181, 0.00622045, -0.0245687, 0.00314985, -0.00198412, -0.0103565)
Transformation Matrix:
   0.999944   0.0103501 -0.00201661   0.0117181
 -0.0103563    0.999942 -0.00312913  0.00622045
 0.00198412  0.00314983    0.999993  -0.0245687
          0           0           0           1
shift: 0.0132668
-----------------------------------------------------------------
(Processed/Input): (219 / 219)
-----------------------------------------------------------------
Sequence number: 5149
Number of scan points: 8115 points.
Number of filtered scan points: 1311 points.
transformed_scan_ptr: 8115 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.5265
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0449237, 0.00883929, -0.0173159, 0.00244705, -0.00125242, -0.00285885)
Transformation Matrix:
   0.999995  0.00285577 -0.00125941   0.0449237
-0.00285884    0.999993 -0.00244346  0.00883929
 0.00125242  0.00244705    0.999996  -0.0173159
          0           0           0           1
shift: 0.045785
-----------------------------------------------------------------
(Processed/Input): (220 / 220)
-----------------------------------------------------------------
Sequence number: 5150
Number of scan points: 7910 points.
Number of filtered scan points: 1327 points.
transformed_scan_ptr: 7910 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.8332
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.00635932, -0.0180055, -0.00853132, 9.30245e-05, -0.0016989, -0.00324778)
Transformation Matrix:
    0.999993   0.00324761  -0.00169919   0.00635932
 -0.00324777     0.999995 -8.74948e-05   -0.0180055
   0.0016989  9.30244e-05     0.999999  -0.00853132
           0            0            0            1
shift: 0.0190956
-----------------------------------------------------------------
(Processed/Input): (221 / 221)
-----------------------------------------------------------------
Sequence number: 5151
Number of scan points: 8055 points.
Number of filtered scan points: 1340 points.
transformed_scan_ptr: 8055 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.5421
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0748613, 0.0455188, -0.00901008, 0.00229704, 0.00110455, -0.00130224)
Transformation Matrix:
   0.999999  0.00130478  0.00110155   0.0748613
-0.00130224    0.999996 -0.00229848   0.0455188
-0.00110455  0.00229703    0.999997 -0.00901008
          0           0           0           1
shift: 0.0876138
-----------------------------------------------------------------
(Processed/Input): (222 / 222)
-----------------------------------------------------------------
Sequence number: 5152
Number of scan points: 8075 points.
Number of filtered scan points: 1363 points.
transformed_scan_ptr: 8075 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.333
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0348317, 0.0406486, -0.0148413, 0.00591601, 0.00115868, -0.00350142)
Transformation Matrix:
   0.999993  0.00350821  0.00113794   0.0348317
-0.00350141    0.999976    -0.00592   0.0406486
-0.00115868  0.00591597    0.999982  -0.0148413
          0           0           0           1
shift: 0.0535309
-----------------------------------------------------------------
(Processed/Input): (223 / 223)
-----------------------------------------------------------------
Sequence number: 5153
Number of scan points: 7840 points.
Number of filtered scan points: 1338 points.
transformed_scan_ptr: 7840 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.5763
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.00095639, 0.00339218, -0.00904606, 0.00853511, -0.000803071, -0.00325863)
Transformation Matrix:
    0.999994   0.00325166 -0.000830835   0.00095639
 -0.00325863     0.999958  -0.00853235   0.00339218
 0.000803071   0.00853501     0.999963  -0.00904606
           0            0            0            1
shift: 0.00352443
-----------------------------------------------------------------
(Processed/Input): (224 / 224)
-----------------------------------------------------------------
Sequence number: 5154
Number of scan points: 7807 points.
Number of filtered scan points: 1327 points.
transformed_scan_ptr: 7807 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.4771
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.00174911, -0.0647229, -0.0106508, 0.0133257, -0.000385657, -0.00638216)
Transformation Matrix:
     0.99998   0.00637641 -0.000470648   0.00174911
 -0.00638212     0.999891   -0.0133225   -0.0647229
 0.000385657    0.0133253     0.999911   -0.0106508
           0            0            0            1
shift: 0.0647465
-----------------------------------------------------------------
(Processed/Input): (225 / 225)
-----------------------------------------------------------------
Sequence number: 5155
Number of scan points: 7979 points.
Number of filtered scan points: 1373 points.
transformed_scan_ptr: 7979 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.4853
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.00308548, -0.0653253, -0.0130443, 0.0131187, 0.000961482, -0.00752167)
Transformation Matrix:
    0.999971   0.00753357   0.00086269  -0.00308548
  -0.0075216     0.999886   -0.0131251   -0.0653253
-0.000961482    0.0131183     0.999913   -0.0130443
           0            0            0            1
shift: 0.0653982
-----------------------------------------------------------------
(Processed/Input): (226 / 226)
-----------------------------------------------------------------
Sequence number: 5156
Number of scan points: 7680 points.
Number of filtered scan points: 1360 points.
transformed_scan_ptr: 7680 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5877
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.00547567, -0.029764, -0.00668601, 0.009938, -0.000574583, -0.00374143)
Transformation Matrix:
    0.999993   0.00373552 -0.000611752   0.00547567
 -0.00374142     0.999944  -0.00993561    -0.029764
 0.000574583   0.00993783      0.99995  -0.00668601
           0            0            0            1
shift: 0.0302634
-----------------------------------------------------------------
(Processed/Input): (227 / 227)
-----------------------------------------------------------------
Sequence number: 5157
Number of scan points: 7950 points.
Number of filtered scan points: 1374 points.
transformed_scan_ptr: 7950 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.0135
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0153684, -0.009521, -0.0115581, 0.00749574, -0.000214423, -0.00385736)
Transformation Matrix:
    0.999993   0.00385564 -0.000243306    0.0153684
 -0.00385735     0.999964  -0.00749479    -0.009521
 0.000214423   0.00749567     0.999972   -0.0115581
           0            0            0            1
shift: 0.0180786
-----------------------------------------------------------------
(Processed/Input): (228 / 228)
-----------------------------------------------------------------
Sequence number: 5158
Number of scan points: 7735 points.
Number of filtered scan points: 1398 points.
transformed_scan_ptr: 7735 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.6154
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.024556, 0.00749326, 0.00372043, 0.00255457, -0.00192917, -0.00288808)
Transformation Matrix:
   0.999994  0.00288313 -0.00193654   -0.024556
-0.00288807    0.999993 -0.00254898  0.00749326
 0.00192917  0.00255457    0.999995  0.00372043
          0           0           0           1
shift: 0.0256739
-----------------------------------------------------------------
(Processed/Input): (229 / 229)
-----------------------------------------------------------------
Sequence number: 5159
Number of scan points: 7874 points.
Number of filtered scan points: 1364 points.
transformed_scan_ptr: 7874 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.3883
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0227486, -0.0309777, 0.0125413, 0.0009881, -0.00167846, 0.00151925)
Transformation Matrix:
    0.999997   -0.0015209  -0.00167696    0.0227486
  0.00151925     0.999998 -0.000990638   -0.0309777
  0.00167846  0.000988098     0.999998    0.0125413
           0            0            0            1
shift: 0.0384333
-----------------------------------------------------------------
(Processed/Input): (230 / 230)
-----------------------------------------------------------------
Sequence number: 5160
Number of scan points: 7873 points.
Number of filtered scan points: 1344 points.
transformed_scan_ptr: 7873 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.9618
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.043729, -0.0365165, 0.0171647, 0.00167438, 0.00129951, -0.000861826)
Transformation Matrix:
    0.999999  0.000864004   0.00129807     0.043729
-0.000861825     0.999998   -0.0016755   -0.0365165
 -0.00129951   0.00167438     0.999998    0.0171647
           0            0            0            1
shift: 0.0569709
-----------------------------------------------------------------
(Processed/Input): (231 / 231)
-----------------------------------------------------------------
Sequence number: 5161
Number of scan points: 7810 points.
Number of filtered scan points: 1361 points.
transformed_scan_ptr: 7810 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.3094
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0211566, -0.0154897, 0.019619, 0.000419022, 0.00154244, 0.00278933)
Transformation Matrix:
    0.999995  -0.00278867    0.0015436    0.0211566
  0.00278932     0.999996 -0.000414702   -0.0154897
 -0.00154244  0.000419021     0.999999     0.019619
           0            0            0            1
shift: 0.0262208
-----------------------------------------------------------------
(Processed/Input): (232 / 232)
-----------------------------------------------------------------
Sequence number: 5162
Number of scan points: 7701 points.
Number of filtered scan points: 1358 points.
transformed_scan_ptr: 7701 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.4106
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0564978, -0.0212409, 0.0115475, 0.00136295, 0.00151976, -0.00116525)
Transformation Matrix:
   0.999998   0.0011673  0.00151818   0.0564978
-0.00116524    0.999998  -0.0013647  -0.0212409
-0.00151976  0.00136294    0.999998   0.0115475
          0           0           0           1
shift: 0.0603588
-----------------------------------------------------------------
(Processed/Input): (233 / 233)
-----------------------------------------------------------------
Sequence number: 5163
Number of scan points: 7637 points.
Number of filtered scan points: 1358 points.
transformed_scan_ptr: 7637 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.7809
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0320243, -0.00590918, -0.00277933, 0.00261988, 0.00253872, -0.00172312)
Transformation Matrix:
   0.999995  0.00172978  0.00253418   0.0320243
-0.00172311    0.999995 -0.00262426 -0.00590918
-0.00253872  0.00261987    0.999993 -0.00277933
          0           0           0           1
shift: 0.0325649
-----------------------------------------------------------------
(Processed/Input): (234 / 234)
-----------------------------------------------------------------
Sequence number: 5164
Number of scan points: 7780 points.
Number of filtered scan points: 1336 points.
transformed_scan_ptr: 7780 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.2708
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.020381, -0.00119912, -0.0192271, 0.00343734, 0.00246311, -0.00427226)
Transformation Matrix:
   0.999988  0.00428068  0.00244841    0.020381
-0.00427224    0.999985 -0.00344781 -0.00119912
-0.00246311  0.00343732    0.999991  -0.0192271
          0           0           0           1
shift: 0.0204163
-----------------------------------------------------------------
(Processed/Input): (235 / 235)
-----------------------------------------------------------------
Sequence number: 5165
Number of scan points: 7894 points.
Number of filtered scan points: 1331 points.
transformed_scan_ptr: 7894 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.1698
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0615665, -0.0238644, -0.0393917, 0.00623765, 0.00269428, 0.00192641)
Transformation Matrix:
   0.999995 -0.00190957  0.00270624   0.0615665
  0.0019264    0.999979 -0.00623241  -0.0238644
-0.00269428  0.00623759    0.999977  -0.0393917
          0           0           0           1
shift: 0.0660299
-----------------------------------------------------------------
(Processed/Input): (236 / 236)
-----------------------------------------------------------------
Sequence number: 5166
Number of scan points: 7957 points.
Number of filtered scan points: 1323 points.
transformed_scan_ptr: 7957 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.6291
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0887279, -0.0454633, -0.0372775, 0.00406925, 0.00361678, 0.00718346)
Transformation Matrix:
   0.999968 -0.00716861   0.0036459   0.0887279
 0.00718335    0.999966 -0.00404314  -0.0454633
-0.00361677  0.00406921    0.999985  -0.0372775
          0           0           0           1
shift: 0.0996973
-----------------------------------------------------------------
(Processed/Input): (237 / 237)
-----------------------------------------------------------------
Sequence number: 5167
Number of scan points: 8043 points.
Number of filtered scan points: 1300 points.
transformed_scan_ptr: 8043 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0803
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0540359, -0.0386743, -0.0552513, 0.00671753, 0.00176724, -0.00186471)
Transformation Matrix:
   0.999997  0.00187654  0.00175467   0.0540359
-0.00186471    0.999976 -0.00672076  -0.0386743
-0.00176724  0.00671747    0.999976  -0.0552513
          0           0           0           1
shift: 0.0664499
-----------------------------------------------------------------
(Processed/Input): (238 / 238)
-----------------------------------------------------------------
Sequence number: 5168
Number of scan points: 7907 points.
Number of filtered scan points: 1293 points.
transformed_scan_ptr: 7907 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.192
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0267551, -0.00755634, -0.0469453, 0.00525725, 0.0020856, -0.00267985)
Transformation Matrix:
   0.999994  0.00269078  0.00207147  -0.0267551
-0.00267984    0.999983  -0.0052628 -0.00755634
 -0.0020856  0.00525721    0.999984  -0.0469453
          0           0           0           1
shift: 0.0278017
-----------------------------------------------------------------
(Processed/Input): (239 / 239)
-----------------------------------------------------------------
Sequence number: 5169
Number of scan points: 7870 points.
Number of filtered scan points: 1316 points.
transformed_scan_ptr: 7870 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0129
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0530727, 0.0087941, -0.0597686, 0.00807794, 0.00321478, 0.00280021)
Transformation Matrix:
   0.999991 -0.00277416  0.00323727   0.0530727
 0.00280019    0.999964 -0.00806882   0.0087941
-0.00321477  0.00807781    0.999962  -0.0597686
          0           0           0           1
shift: 0.0537964
-----------------------------------------------------------------
(Processed/Input): (240 / 240)
-----------------------------------------------------------------
Sequence number: 5170
Number of scan points: 7942 points.
Number of filtered scan points: 1295 points.
transformed_scan_ptr: 7942 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5965
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0885222, 0.0163017, -0.059322, 0.00606238, 0.000296522, 0.00956642)
Transformation Matrix:
    0.999954   -0.0095643   0.00035452    0.0885222
  0.00956627     0.999936  -0.00605923    0.0163017
-0.000296522   0.00606234     0.999982    -0.059322
           0            0            0            1
shift: 0.0900107
-----------------------------------------------------------------
(Processed/Input): (241 / 241)
-----------------------------------------------------------------
Sequence number: 5171
Number of scan points: 8002 points.
Number of filtered scan points: 1292 points.
transformed_scan_ptr: 8002 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.4117
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.0182276, -0.0128681, -0.0620757, 0.00774551, -0.00183992, -0.00086795)
Transformation Matrix:
    0.999998   0.00085367  -0.00184658   -0.0182276
-0.000867949      0.99997  -0.00774383   -0.0128681
  0.00183992   0.00774542     0.999968   -0.0620757
           0            0            0            1
shift: 0.0223122
-----------------------------------------------------------------
(Processed/Input): (242 / 242)
-----------------------------------------------------------------
Sequence number: 5172
Number of scan points: 8094 points.
Number of filtered scan points: 1291 points.
transformed_scan_ptr: 8094 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.6301
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.0271686, -0.0241566, -0.059718, 0.00833182, -0.00149583, -0.00642011)
Transformation Matrix:
   0.999978  0.00640738 -0.00154923  -0.0271686
-0.00642006    0.999945 -0.00832195  -0.0241566
 0.00149583  0.00833171    0.999964   -0.059718
          0           0           0           1
shift: 0.0363548
-----------------------------------------------------------------
(Processed/Input): (243 / 243)
-----------------------------------------------------------------
Sequence number: 5173
Number of scan points: 8136 points.
Number of filtered scan points: 1279 points.
transformed_scan_ptr: 8136 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5518
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.022017, 0.00239305, -0.0615903, 0.00860766, -0.000680069, -0.00489939)
Transformation Matrix:
    0.999988   0.00489334 -0.000722232    -0.022017
 -0.00489937     0.999951  -0.00860412   0.00239305
 0.000680069   0.00860756     0.999963   -0.0615903
           0            0            0            1
shift: 0.0221467
-----------------------------------------------------------------
(Processed/Input): (244 / 244)
-----------------------------------------------------------------
Sequence number: 5174
Number of scan points: 8104 points.
Number of filtered scan points: 1255 points.
transformed_scan_ptr: 8104 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.1825
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0365018, -0.038508, -0.0580871, 0.00798302, 0.000407949, -0.00355593)
Transformation Matrix:
    0.999994   0.00355907  0.000379554    0.0365018
 -0.00355593     0.999962  -0.00798433    -0.038508
-0.000407949   0.00798293     0.999968   -0.0580871
           0            0            0            1
shift: 0.0530589
-----------------------------------------------------------------
(Processed/Input): (245 / 245)
-----------------------------------------------------------------
Sequence number: 5175
Number of scan points: 8177 points.
Number of filtered scan points: 1290 points.
transformed_scan_ptr: 8177 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.6652
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0241409, -0.0152816, -0.0734753, 0.0128346, 0.00209277, -0.000503953)
Transformation Matrix:
    0.999998  0.000530775   0.00208612    0.0241409
-0.000503952     0.999918   -0.0128353   -0.0152816
 -0.00209277    0.0128342     0.999915   -0.0734753
           0            0            0            1
shift: 0.0285712
-----------------------------------------------------------------
(Processed/Input): (246 / 246)
-----------------------------------------------------------------
Sequence number: 5176
Number of scan points: 8139 points.
Number of filtered scan points: 1294 points.
transformed_scan_ptr: 8139 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.3578
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.00589906, -0.0528346, -0.0805056, 0.0145176, 0.00102763, -0.00695321)
Transformation Matrix:
   0.999975  0.00696734 0.000926565  0.00589906
-0.00695315     0.99987  -0.0145238  -0.0528346
-0.00102763    0.014517    0.999894  -0.0805056
          0           0           0           1
shift: 0.0531629
-----------------------------------------------------------------
(Processed/Input): (247 / 247)
-----------------------------------------------------------------
Sequence number: 5177
Number of scan points: 8173 points.
Number of filtered scan points: 1307 points.
transformed_scan_ptr: 8173 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.5618
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0306689, -0.0537412, -0.0827513, 0.0119867, -0.000971984, -0.00866451)
Transformation Matrix:
   0.999962  0.00865213 -0.00107573  -0.0306689
 -0.0086644    0.999891  -0.0119776  -0.0537412
0.000971984   0.0119864    0.999928  -0.0827513
          0           0           0           1
shift: 0.0618765
-----------------------------------------------------------------
(Processed/Input): (248 / 248)
-----------------------------------------------------------------
Sequence number: 5178
Number of scan points: 8317 points.
Number of filtered scan points: 1344 points.
transformed_scan_ptr: 8317 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.2897
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0288379, -0.0573513, -0.06627, 0.00307259, -0.00300983, -0.0123385)
Transformation Matrix:
   0.999919   0.0123289 -0.00304748  -0.0288379
 -0.0123381    0.999919 -0.00303522  -0.0573513
 0.00300982  0.00307257    0.999991    -0.06627
          0           0           0           1
shift: 0.0641934
-----------------------------------------------------------------
(Processed/Input): (249 / 249)
-----------------------------------------------------------------
Sequence number: 5179
Number of scan points: 8173 points.
Number of filtered scan points: 1305 points.
transformed_scan_ptr: 8173 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.2804
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.00356726, 0.00767195, -0.0521978, -0.00108635, -0.00342838, 0.00137834)
Transformation Matrix:
   0.999993 -0.00137462 -0.00342986  0.00356726
 0.00137833    0.999998  0.00108164  0.00767195
 0.00342837 -0.00108635    0.999994  -0.0521978
          0           0           0           1
shift: 0.00846074
-----------------------------------------------------------------
(Processed/Input): (250 / 250)
-----------------------------------------------------------------
Sequence number: 5180
Number of scan points: 8063 points.
Number of filtered scan points: 1278 points.
transformed_scan_ptr: 8063 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.0512
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.0204955, 0.0336291, -0.0407785, -0.00300271, -0.00275767, -0.0041844)
Transformation Matrix:
   0.999987  0.00419266 -0.00274505  -0.0204955
-0.00418437    0.999987  0.00301423   0.0336291
 0.00275767 -0.00300269    0.999992  -0.0407785
          0           0           0           1
shift: 0.0393825
-----------------------------------------------------------------
(Processed/Input): (251 / 251)
-----------------------------------------------------------------
Sequence number: 5181
Number of scan points: 8233 points.
Number of filtered scan points: 1308 points.
transformed_scan_ptr: 8233 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.6911
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.00992038, 0.0342593, -0.0454239, -0.00374402, -0.000553663, -0.00175667)
Transformation Matrix:
    0.999998   0.00175872 -0.000547085  -0.00992038
 -0.00175666     0.999991   0.00374498    0.0342593
 0.000553663  -0.00374402     0.999993   -0.0454239
           0            0            0            1
shift: 0.0356667
-----------------------------------------------------------------
(Processed/Input): (252 / 252)
-----------------------------------------------------------------
Sequence number: 5182
Number of scan points: 8392 points.
Number of filtered scan points: 1338 points.
transformed_scan_ptr: 8392 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.1692
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0577895, 0.0351498, -0.0481134, -0.00219929, -0.00010404, -0.000298837)
Transformation Matrix:
           1  0.000299063  -0.00010339    0.0577895
-0.000298837     0.999998   0.00219932    0.0351498
  0.00010404  -0.00219929     0.999998   -0.0481134
           0            0            0            1
shift: 0.0676398
-----------------------------------------------------------------
(Processed/Input): (253 / 253)
-----------------------------------------------------------------
Sequence number: 5183
Number of scan points: 8364 points.
Number of filtered scan points: 1361 points.
transformed_scan_ptr: 8364 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.2478
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(0.0144972, 0.0489172, -0.0462768, 0.00172379, -0.000361851, -0.00178123)
Transformation Matrix:
    0.999998   0.00178059 -0.000364942    0.0144972
 -0.00178122     0.999997  -0.00172314    0.0489172
 0.000361851   0.00172379     0.999998   -0.0462768
           0            0            0            1
shift: 0.0510202
-----------------------------------------------------------------
(Processed/Input): (254 / 254)
-----------------------------------------------------------------
Sequence number: 5184
Number of scan points: 8232 points.
Number of filtered scan points: 1356 points.
transformed_scan_ptr: 8232 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.6267
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0425802, 0.0176745, -0.0400915, 0.00240261, -0.00158298, -0.00779875)
Transformation Matrix:
   0.999968  0.00779485 -0.00160165  -0.0425802
-0.00779866    0.999967 -0.00239019   0.0176745
 0.00158298   0.0024026    0.999996  -0.0400915
          0           0           0           1
shift: 0.0461027
-----------------------------------------------------------------
(Processed/Input): (255 / 255)
-----------------------------------------------------------------
Sequence number: 5185
Number of scan points: 8247 points.
Number of filtered scan points: 1311 points.
transformed_scan_ptr: 8247 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.8897
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0644669, -0.0121179, -0.0362896, 0.00336703, -0.00118184, -0.0118851)
Transformation Matrix:
   0.999929   0.0118807 -0.00122175  -0.0644669
 -0.0118848    0.999924 -0.00335274  -0.0121179
 0.00118184  0.00336702    0.999994  -0.0362896
          0           0           0           1
shift: 0.065596
-----------------------------------------------------------------
(Processed/Input): (256 / 256)
-----------------------------------------------------------------
Sequence number: 5186
Number of scan points: 8194 points.
Number of filtered scan points: 1315 points.
transformed_scan_ptr: 8194 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.0047
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0256498, -0.0167016, -0.0330139, 0.0044943, -0.000637245, -0.00958767)
Transformation Matrix:
    0.999954   0.00958456 -0.000680312   -0.0256498
 -0.00958752     0.999944  -0.00448797   -0.0167016
 0.000637245   0.00449428      0.99999   -0.0330139
           0            0            0            1
shift: 0.0306081
-----------------------------------------------------------------
(Processed/Input): (257 / 257)
-----------------------------------------------------------------
Sequence number: 5187
Number of scan points: 8192 points.
Number of filtered scan points: 1332 points.
transformed_scan_ptr: 8192 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.2607
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0146399, 0.0386071, -0.031684, 0.00353561, 5.4223e-05, -0.00164152)
Transformation Matrix:
   0.999999   0.0016417 4.84413e-05   0.0146399
-0.00164152    0.999992 -0.00353569   0.0386071
-5.4223e-05   0.0035356    0.999994   -0.031684
          0           0           0           1
shift: 0.0412897
-----------------------------------------------------------------
(Processed/Input): (258 / 258)
-----------------------------------------------------------------
Sequence number: 5188
Number of scan points: 8081 points.
Number of filtered scan points: 1294 points.
transformed_scan_ptr: 8081 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.0251
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0103925, -0.046083, -0.0265728, 0.00527207, 0.00198461, -0.00473069)
Transformation Matrix:
   0.999987  0.00474107  0.00195963   0.0103925
-0.00473067    0.999975 -0.00528137   -0.046083
 -0.0019846  0.00527204    0.999984  -0.0265728
          0           0           0           1
shift: 0.0472403
-----------------------------------------------------------------
(Processed/Input): (259 / 259)
-----------------------------------------------------------------
Sequence number: 5189
Number of scan points: 8047 points.
Number of filtered scan points: 1267 points.
transformed_scan_ptr: 8047 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.6527
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.00576676, -0.0723444, -0.0260248, 0.00412888, 0.00470133, 0.00113543)
Transformation Matrix:
   0.999988 -0.00111601  0.00470596 -0.00576676
 0.00113541    0.999991 -0.00412353  -0.0723444
-0.00470132  0.00412882     0.99998  -0.0260248
          0           0           0           1
shift: 0.0725739
-----------------------------------------------------------------
(Processed/Input): (260 / 260)
-----------------------------------------------------------------
Sequence number: 5190
Number of scan points: 8119 points.
Number of filtered scan points: 1298 points.
transformed_scan_ptr: 8119 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.721
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0361137, -0.0515989, -0.0271955, 0.00398641, 0.00760516, -0.00144583)
Transformation Matrix:
    0.99997  0.00147612  0.00759926   0.0361137
-0.00144579    0.999991 -0.00399738  -0.0515989
-0.00760509  0.00398628    0.999963  -0.0271955
          0           0           0           1
shift: 0.0629813
-----------------------------------------------------------------
(Processed/Input): (261 / 261)
-----------------------------------------------------------------
Sequence number: 5191
Number of scan points: 8044 points.
Number of filtered scan points: 1274 points.
transformed_scan_ptr: 8044 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.8673
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.000984923, -0.055968, -0.0309574, 0.00335494, 0.00869182, -0.00395386)
Transformation Matrix:
   0.999954  0.00398298  0.00867834 0.000984923
 -0.0039537    0.999986 -0.00338926   -0.055968
-0.00869171   0.0033548    0.999957  -0.0309574
          0           0           0           1
shift: 0.0559767
-----------------------------------------------------------------
(Processed/Input): (262 / 262)
-----------------------------------------------------------------
Sequence number: 5192
Number of scan points: 7975 points.
Number of filtered scan points: 1262 points.
transformed_scan_ptr: 7975 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.8408
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0019158, -0.0139872, -0.0322896, 0.00294, 0.00960963, -0.0128992)
Transformation Matrix:
   0.999871   0.0129271  0.00957072   0.0019158
 -0.0128983    0.999912  -0.0030637  -0.0139872
-0.00960949  0.00293986     0.99995  -0.0322896
          0           0           0           1
shift: 0.0141177
-----------------------------------------------------------------
(Processed/Input): (263 / 263)
-----------------------------------------------------------------
Sequence number: 5193
Number of scan points: 8115 points.
Number of filtered scan points: 1282 points.
transformed_scan_ptr: 8115 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.8555
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0919221, 0.00592718, -0.0430206, 0.00253843, 0.00911776, -0.00753694)
Transformation Matrix:
    0.99993  0.00755998  0.00909822   0.0919221
-0.00753655    0.999968 -0.00260705  0.00592718
-0.00911763  0.00253832    0.999955  -0.0430206
          0           0           0           1
shift: 0.092113
-----------------------------------------------------------------
(Processed/Input): (264 / 264)
-----------------------------------------------------------------
Sequence number: 5194
Number of scan points: 7900 points.
Number of filtered scan points: 1273 points.
transformed_scan_ptr: 7900 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.5335
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0231745, -0.00327787, -0.0462627, 0.00633187, 0.00275582, 0.00504745)
Transformation Matrix:
   0.999983 -0.00502987  0.00278768   0.0231745
 0.00504741    0.999967 -0.00631784 -0.00327787
-0.00275581  0.00633181    0.999976  -0.0462627
          0           0           0           1
shift: 0.0234052
-----------------------------------------------------------------
(Processed/Input): (265 / 265)
-----------------------------------------------------------------
Sequence number: 5195
Number of scan points: 7794 points.
Number of filtered scan points: 1299 points.
transformed_scan_ptr: 7794 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.1279
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0311208, 0.00969823, -0.0509223, 0.00714743, -0.000149069, 2.96205e-06)
Transformation Matrix:
           1 -4.02811e-06 -0.000149044    0.0311208
 2.96205e-06     0.999974  -0.00714737   0.00969823
 0.000149069   0.00714737     0.999974   -0.0509223
           0            0            0            1
shift: 0.0325969
-----------------------------------------------------------------
(Processed/Input): (266 / 266)
-----------------------------------------------------------------
Sequence number: 5196
Number of scan points: 7864 points.
Number of filtered scan points: 1293 points.
transformed_scan_ptr: 7864 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.6248
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.00794633, 0.0826549, -0.0458292, 0.00338802, 0.000116622, -0.0040635)
Transformation Matrix:
    0.999992   0.00406386  0.000102849   0.00794633
 -0.00406349     0.999986  -0.00338846    0.0826549
-0.000116622   0.00338802     0.999994   -0.0458292
           0            0            0            1
shift: 0.083036
-----------------------------------------------------------------
(Processed/Input): (267 / 267)
-----------------------------------------------------------------
Sequence number: 5197
Number of scan points: 8103 points.
Number of filtered scan points: 1321 points.
transformed_scan_ptr: 8103 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.6964
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0040148, 0.121085, -0.0437799, -0.0023596, 0.00247364, -0.00658646)
Transformation Matrix:
   0.999975  0.00658056  0.00248913   0.0040148
 -0.0065864    0.999976  0.00234324    0.121085
-0.00247364 -0.00235959    0.999994  -0.0437799
          0           0           0           1
shift: 0.121152
-----------------------------------------------------------------
(Processed/Input): (268 / 268)
-----------------------------------------------------------------
Sequence number: 5198
Number of scan points: 7709 points.
Number of filtered scan points: 1285 points.
transformed_scan_ptr: 7709 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.7283
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0444419, 0.0745009, -0.0382496, -0.00382427, 0.00447598, -0.000195127)
Transformation Matrix:
     0.99999   0.00017801   0.00447668    0.0444419
-0.000195125     0.999993   0.00382338    0.0745009
 -0.00447597  -0.00382422     0.999983   -0.0382496
           0            0            0            1
shift: 0.0867494
-----------------------------------------------------------------
(Processed/Input): (269 / 269)
-----------------------------------------------------------------
Sequence number: 5199
Number of scan points: 7626 points.
Number of filtered scan points: 1276 points.
transformed_scan_ptr: 7626 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.5915
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0157397, 0.0448518, -0.0345262, -0.00475116, 0.00583836, -0.00417867)
Transformation Matrix:
   0.999974  0.00415087  0.00585807   0.0157397
-0.00417859     0.99998   0.0047267   0.0448518
-0.00583832 -0.00475106    0.999972  -0.0345262
          0           0           0           1
shift: 0.0475334
-----------------------------------------------------------------
(Processed/Input): (270 / 270)
-----------------------------------------------------------------
Sequence number: 5200
Number of scan points: 7685 points.
Number of filtered scan points: 1295 points.
transformed_scan_ptr: 7685 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.8623
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.00976921, 0.0210253, -0.0408589, -0.00349459, 0.00769044, 0.00906748)
Transformation Matrix:
   0.999929 -0.00909417  0.00765832  0.00976921
 0.00906709    0.999953  0.00356415   0.0210253
-0.00769036 -0.00349448    0.999964  -0.0408589
          0           0           0           1
shift: 0.0231841
-----------------------------------------------------------------
(Processed/Input): (271 / 271)
-----------------------------------------------------------------
Sequence number: 5201
Number of scan points: 7502 points.
Number of filtered scan points: 1288 points.
transformed_scan_ptr: 7502 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.4462
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0613534, -0.0148195, -0.0447271, -0.0046208, 0.00923234, 0.0054512)
Transformation Matrix:
   0.999943 -0.00549376   0.0092068   0.0613534
 0.00545094    0.999974  0.00467103  -0.0148195
-0.00923221 -0.00462059    0.999947  -0.0447271
          0           0           0           1
shift: 0.0631178
-----------------------------------------------------------------
(Processed/Input): (272 / 272)
-----------------------------------------------------------------
Sequence number: 5202
Number of scan points: 7609 points.
Number of filtered scan points: 1283 points.
transformed_scan_ptr: 7609 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.4595
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0450273, -0.0481877, -0.0415594, -0.0030732, 0.0108039, -0.00264859)
Transformation Matrix:
   0.999938  0.00261539   0.0108117   0.0450273
-0.00264844    0.999992  0.00304458  -0.0481877
 -0.0108037 -0.00307301    0.999937  -0.0415594
          0           0           0           1
shift: 0.0659508
-----------------------------------------------------------------
(Processed/Input): (273 / 273)
-----------------------------------------------------------------
Sequence number: 5203
Number of scan points: 7534 points.
Number of filtered scan points: 1265 points.
transformed_scan_ptr: 7534 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.5023
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.020782, -0.0106422, -0.0451411, -0.00302318, 0.00669919, -0.010396)
Transformation Matrix:
   0.999924   0.0103755  0.00673018    0.020782
 -0.0103955    0.999942  0.00295337  -0.0106422
-0.00669914 -0.00302311    0.999973  -0.0451411
          0           0           0           1
shift: 0.0233484
-----------------------------------------------------------------
(Processed/Input): (274 / 274)
-----------------------------------------------------------------
Sequence number: 5204
Number of scan points: 7335 points.
Number of filtered scan points: 1250 points.
transformed_scan_ptr: 7335 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.2233
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0251596, 0.00984729, -0.0375286, -0.00761023, 0.00389468, 0.00170164)
Transformation Matrix:
   0.999991 -0.00173124  0.00388159   0.0251596
 0.00170162     0.99997  0.00761678  0.00984729
-0.00389467  -0.0076101    0.999963  -0.0375286
          0           0           0           1
shift: 0.0270181
-----------------------------------------------------------------
(Processed/Input): (275 / 275)
-----------------------------------------------------------------
Sequence number: 5205
Number of scan points: 7346 points.
Number of filtered scan points: 1260 points.
transformed_scan_ptr: 7346 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.9635
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0132933, 0.0179975, -0.0348606, -0.0131705, 0.00208262, -0.000764769)
Transformation Matrix:
    0.999998  0.000737278   0.00209251    0.0132933
-0.000764767     0.999913    0.0131686    0.0179975
 -0.00208262   -0.0131701     0.999911   -0.0348606
           0            0            0            1
shift: 0.0223746
-----------------------------------------------------------------
(Processed/Input): (276 / 276)
-----------------------------------------------------------------
Sequence number: 5206
Number of scan points: 7518 points.
Number of filtered scan points: 1275 points.
transformed_scan_ptr: 7518 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.414
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0180262, 0.0221229, -0.0400892, -0.0209328, 0.000651498, -0.00659177)
Transformation Matrix:
    0.999978   0.00657664  0.000789319    0.0180262
 -0.00659172     0.999759    0.0209265    0.0221229
-0.000651498   -0.0209313     0.999781   -0.0400892
           0            0            0            1
shift: 0.0285371
-----------------------------------------------------------------
(Processed/Input): (277 / 277)
-----------------------------------------------------------------
Sequence number: 5207
Number of scan points: 7615 points.
Number of filtered scan points: 1318 points.
transformed_scan_ptr: 7615 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.667
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0480619, -0.00184465, -0.0417096, -0.0296509, 0.00199143, 0.00104824)
Transformation Matrix:
   0.999997 -0.00110683  0.00195947   0.0480619
 0.00104824     0.99956   0.0296486 -0.00184465
-0.00199143  -0.0296465    0.999558  -0.0417096
          0           0           0           1
shift: 0.0480973
-----------------------------------------------------------------
(Processed/Input): (278 / 278)
-----------------------------------------------------------------
Sequence number: 5208
Number of scan points: 7457 points.
Number of filtered scan points: 1299 points.
transformed_scan_ptr: 7457 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.4216
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0330728, 0.0445188, -0.0408633, -0.0402954, 0.00235962, -0.00549715)
Transformation Matrix:
   0.999982   0.0053976  0.00257913   0.0330728
 -0.0054971    0.999174   0.0402709   0.0445188
-0.00235962  -0.0402844    0.999185  -0.0408633
          0           0           0           1
shift: 0.0554593
-----------------------------------------------------------------
(Processed/Input): (279 / 279)
-----------------------------------------------------------------
Sequence number: 5209
Number of scan points: 7530 points.
Number of filtered scan points: 1333 points.
transformed_scan_ptr: 7530 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.9984
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0594407, 0.0373375, -0.0454848, -0.0502114, 0.00546648, -0.00438769)
Transformation Matrix:
   0.999975  0.00410778  0.00567973   0.0594407
-0.00438761    0.998731   0.0501659   0.0373375
-0.00546645  -0.0501896    0.998725  -0.0454848
          0           0           0           1
shift: 0.0701946
-----------------------------------------------------------------
(Processed/Input): (280 / 280)
-----------------------------------------------------------------
Sequence number: 5210
Number of scan points: 7721 points.
Number of filtered scan points: 1350 points.
transformed_scan_ptr: 7721 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.4257
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.074082, 0.0426988, -0.0644582, -0.0548392, 0.011305, -0.00907023)
Transformation Matrix:
   0.999895  0.00843686   0.0117844    0.074082
-0.00906952    0.998461   0.0547071   0.0426988
 -0.0113047  -0.0548082    0.998433  -0.0644582
          0           0           0           1
shift: 0.0855063
-----------------------------------------------------------------
(Processed/Input): (281 / 281)
-----------------------------------------------------------------
Sequence number: 5211
Number of scan points: 7429 points.
Number of filtered scan points: 1354 points.
transformed_scan_ptr: 7429 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.4294
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0896882, 0.0342609, -0.0803806, -0.0555882, 0.0155743, -0.00499917)
Transformation Matrix:
   0.999866  0.00412617   0.0158271   0.0896882
-0.00499854    0.998447   0.0554812   0.0342609
 -0.0155736  -0.0555529    0.998334  -0.0803806
          0           0           0           1
shift: 0.0960093
-----------------------------------------------------------------
(Processed/Input): (282 / 282)
-----------------------------------------------------------------
Sequence number: 5212
Number of scan points: 7425 points.
Number of filtered scan points: 1357 points.
transformed_scan_ptr: 7425 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.5895
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0849744, 0.061929, -0.111862, -0.0481086, 0.0177627, -0.00499957)
Transformation Matrix:
    0.99983  0.00413961   0.0179814   0.0849744
-0.00499876    0.998835   0.0480007    0.061929
 -0.0177618  -0.0480824    0.998685   -0.111862
          0           0           0           1
shift: 0.105147
-----------------------------------------------------------------
(Processed/Input): (283 / 283)
-----------------------------------------------------------------
Sequence number: 5213
Number of scan points: 7280 points.
Number of filtered scan points: 1306 points.
transformed_scan_ptr: 7280 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.7408
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0187874, 0.0892031, -0.145284, -0.0317471, 0.0177606, 0.00244836)
Transformation Matrix:
   0.999839 -0.00301083    0.017673   0.0187874
 0.00244797    0.999492   0.0317851   0.0892031
 -0.0177597  -0.0317367    0.999339   -0.145284
          0           0           0           1
shift: 0.0911601
-----------------------------------------------------------------
(Processed/Input): (284 / 284)
-----------------------------------------------------------------
Sequence number: 5214
Number of scan points: 7112 points.
Number of filtered scan points: 1276 points.
transformed_scan_ptr: 7112 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 30.1131
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0829025, -0.00111748, -0.177522, -0.00897069, 0.0200096, -0.00466185)
Transformation Matrix:
   0.999789  0.00448215   0.0200491   0.0829025
 -0.0046609     0.99995  0.00887719 -0.00111748
 -0.0200083 -0.00896877     0.99976   -0.177522
          0           0           0           1
shift: 0.08291
-----------------------------------------------------------------
(Processed/Input): (285 / 285)
-----------------------------------------------------------------
Sequence number: 5215
Number of scan points: 7075 points.
Number of filtered scan points: 1240 points.
transformed_scan_ptr: 7075 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 30.6658
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0794521, -0.0825648, -0.205781, 0.0155484, 0.0190271, 0.000352682)
Transformation Matrix:
    0.999819 -5.68132e-05    0.0190291    0.0794521
 0.000352618     0.999879   -0.0155411   -0.0825648
  -0.0190259     0.015545     0.999698    -0.205781
           0            0            0            1
shift: 0.114584
-----------------------------------------------------------------
(Processed/Input): (286 / 286)
-----------------------------------------------------------------
Sequence number: 5216
Number of scan points: 6598 points.
Number of filtered scan points: 1215 points.
transformed_scan_ptr: 6598 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 31.1893
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(0.0188158, -0.106819, -0.223296, 0.0371271, 0.0162445, -0.0119562)
Transformation Matrix:
  0.999797  0.0125506  0.0157877  0.0188158
-0.0119543   0.999232   -0.03731  -0.106819
-0.0162438  0.0371137   0.999179  -0.223296
         0          0          0          1
shift: 0.108464
-----------------------------------------------------------------
(Processed/Input): (287 / 287)
-----------------------------------------------------------------
Sequence number: 5217
Number of scan points: 6231 points.
Number of filtered scan points: 1129 points.
transformed_scan_ptr: 6231 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.5734
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.0374703, -0.15004, -0.251102, 0.0539887, 0.00853839, -0.014458)
Transformation Matrix:
   0.999859   0.0148971  0.00774479  -0.0374703
  -0.014457    0.998432  -0.0540801    -0.15004
-0.00853828   0.0539605    0.998507   -0.251102
          0           0           0           1
shift: 0.154648
-----------------------------------------------------------------
(Processed/Input): (288 / 288)
-----------------------------------------------------------------
Sequence number: 5218
Number of scan points: 6292 points.
Number of filtered scan points: 1121 points.
transformed_scan_ptr: 6292 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.7399
Number of iteration: 8
(x,y,z,roll,pitch,yaw):
(0.0450248, -0.172411, -0.266643, 0.0634236, -0.00416366, -0.00517718)
Transformation Matrix:
   0.999978  0.00490286 -0.00448336   0.0450248
-0.00517712    0.997977  -0.0633587   -0.172411
 0.00416365   0.0633805    0.997981   -0.266643
          0           0           0           1
shift: 0.178193
-----------------------------------------------------------------
(Processed/Input): (289 / 289)
-----------------------------------------------------------------
Sequence number: 5219
Number of scan points: 6302 points.
Number of filtered scan points: 1136 points.
transformed_scan_ptr: 6302 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.6371
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(-0.00986744, -0.220704, -0.287239, 0.0696216, -0.00996268, -0.00626197)
Transformation Matrix:
   0.999931  0.00555373  -0.0103738 -0.00986744
-0.00626162    0.997562  -0.0695018   -0.220704
 0.00996252   0.0695619    0.997528   -0.287239
          0           0           0           1
shift: 0.220925
-----------------------------------------------------------------
(Processed/Input): (290 / 290)
-----------------------------------------------------------------
Sequence number: 5220
Number of scan points: 6525 points.
Number of filtered scan points: 1119 points.
transformed_scan_ptr: 6525 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.354
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.00623271, -0.207391, -0.286189, 0.072677, -0.0141156, -0.00339221)
Transformation Matrix:
   0.999895  0.00235832  -0.0143241  0.00623271
-0.00339187    0.997358  -0.0725649   -0.207391
  0.0141151   0.0726058    0.997261   -0.286189
          0           0           0           1
shift: 0.207485
-----------------------------------------------------------------
(Processed/Input): (291 / 291)
-----------------------------------------------------------------
Sequence number: 5221
Number of scan points: 6747 points.
Number of filtered scan points: 1154 points.
transformed_scan_ptr: 6747 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.7126
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.0443824, -0.171579, -0.260666, 0.0761553, -0.0184386, -0.00719969)
Transformation Matrix:
  0.999804 0.00577602 -0.0189314 -0.0443824
-0.0071984   0.997086 -0.0759474  -0.171579
 0.0184376  0.0760688   0.996932  -0.260666
         0          0          0          1
shift: 0.177226
-----------------------------------------------------------------
(Processed/Input): (292 / 292)
-----------------------------------------------------------------
Sequence number: 5222
Number of scan points: 6874 points.
Number of filtered scan points: 1189 points.
transformed_scan_ptr: 6874 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.7991
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0718663, -0.165914, -0.254886, 0.0834485, -0.0243117, -0.0111599)
Transformation Matrix:
  0.999642 0.00909476 -0.0251534 -0.0718663
-0.0111564   0.996481 -0.0830762  -0.165914
 0.0243093  0.0833271   0.996226  -0.254886
         0          0          0          1
shift: 0.18081
-----------------------------------------------------------------
(Processed/Input): (293 / 293)
-----------------------------------------------------------------
Sequence number: 5223
Number of scan points: 6809 points.
Number of filtered scan points: 1148 points.
transformed_scan_ptr: 6809 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.1487
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0701424, -0.217803, -0.19713, 0.0866527, -0.0226808, -0.00590817)
Transformation Matrix:
   0.999725  0.00392327  -0.0231047  -0.0701424
-0.00590661    0.996242  -0.0864093   -0.217803
  0.0226788   0.0865221    0.995992    -0.19713
          0           0           0           1
shift: 0.228819
-----------------------------------------------------------------
(Processed/Input): (294 / 294)
-----------------------------------------------------------------
Sequence number: 5224
Number of scan points: 7021 points.
Number of filtered scan points: 1184 points.
transformed_scan_ptr: 7021 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.3907
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0154584, -0.242735, -0.148632, 0.0936467, -0.019262, 0.00246463)
Transformation Matrix:
   0.999811 -0.00425489  -0.0189459   0.0154584
 0.00246417    0.995611  -0.0935569   -0.242735
  0.0192608   0.0934926    0.995434   -0.148632
          0           0           0           1
shift: 0.243227
-----------------------------------------------------------------
(Processed/Input): (295 / 295)
-----------------------------------------------------------------
Sequence number: 5225
Number of scan points: 7126 points.
Number of filtered scan points: 1191 points.
transformed_scan_ptr: 7126 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.266
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0103709, -0.272929, -0.104992, 0.0947741, -0.0117916, -0.00211743)
Transformation Matrix:
   0.999928 0.000992087  -0.0119387   0.0103709
-0.00211728    0.995512  -0.0946072   -0.272929
  0.0117913   0.0946257    0.995443   -0.104992
          0           0           0           1
shift: 0.273126
-----------------------------------------------------------------
(Processed/Input): (296 / 296)
-----------------------------------------------------------------
Sequence number: 5226
Number of scan points: 7169 points.
Number of filtered scan points: 1216 points.
transformed_scan_ptr: 7169 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.2673
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0271334, -0.264578, -0.0669092, 0.0967198, -0.00428659, -0.00290891)
Transformation Matrix:
   0.999987  0.00248137 -0.00454743   0.0271334
-0.00290888    0.995323  -0.0965563   -0.264578
 0.00428657   0.0965682    0.995317  -0.0669092
          0           0           0           1
shift: 0.265966
-----------------------------------------------------------------
(Processed/Input): (297 / 297)
-----------------------------------------------------------------
Sequence number: 5227
Number of scan points: 7078 points.
Number of filtered scan points: 1202 points.
transformed_scan_ptr: 7078 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.3573
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0643628, -0.254807, -0.043885, 0.0971177, 0.00251737, -0.00426886)
Transformation Matrix:
   0.999988  0.00449283  0.00209155   0.0643628
-0.00426883    0.995278  -0.0969749   -0.254807
-0.00251736   0.0969648    0.995285   -0.043885
          0           0           0           1
shift: 0.26281
-----------------------------------------------------------------
(Processed/Input): (298 / 298)
-----------------------------------------------------------------
Sequence number: 5228
Number of scan points: 7004 points.
Number of filtered scan points: 1208 points.
transformed_scan_ptr: 7004 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.4701
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.098718, -0.224351, -0.0373852, 0.0929958, 0.00565675, -0.00513057)
Transformation Matrix:
   0.999971  0.00563366  0.00515577    0.098718
-0.00513046    0.995663  -0.0928895   -0.224351
-0.00565672   0.0928603    0.995663  -0.0373852
          0           0           0           1
shift: 0.24511
-----------------------------------------------------------------
(Processed/Input): (299 / 299)
-----------------------------------------------------------------
Sequence number: 5229
Number of scan points: 7069 points.
Number of filtered scan points: 1192 points.
transformed_scan_ptr: 7069 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.3271
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0624318, -0.21861, -0.0366114, 0.0865338, 0.00456071, 0.00168465)
Transformation Matrix:
   0.999988 -0.00128418  0.00468922   0.0624318
 0.00168463    0.996258   -0.086418    -0.21861
-0.00456069   0.0864249    0.996248  -0.0366114
          0           0           0           1
shift: 0.22735
-----------------------------------------------------------------
(Processed/Input): (300 / 300)
-----------------------------------------------------------------
Sequence number: 5230
Number of scan points: 7124 points.
Number of filtered scan points: 1197 points.
transformed_scan_ptr: 7124 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.4595
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0362716, -0.196201, -0.0384292, 0.0764881, 0.00294635, -0.00269447)
Transformation Matrix:
   0.999992  0.00291173  0.00273183   0.0362716
-0.00269446    0.997072  -0.0764211   -0.196201
-0.00294635   0.0764132    0.997072  -0.0384292
          0           0           0           1
shift: 0.199526
-----------------------------------------------------------------
(Processed/Input): (301 / 301)
-----------------------------------------------------------------
Sequence number: 5231
Number of scan points: 7105 points.
Number of filtered scan points: 1183 points.
transformed_scan_ptr: 7105 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.4081
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0364547, -0.197299, -0.0367511, 0.0645573, 0.0011334, 0.00129405)
Transformation Matrix:
   0.999999 -0.00121823  0.00121452   0.0364547
 0.00129405    0.997916  -0.0645109   -0.197299
 -0.0011334   0.0645124    0.997916  -0.0367511
          0           0           0           1
shift: 0.200638
-----------------------------------------------------------------
(Processed/Input): (302 / 302)
-----------------------------------------------------------------
Sequence number: 5232
Number of scan points: 7082 points.
Number of filtered scan points: 1180 points.
transformed_scan_ptr: 7082 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.096
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.00259166, -0.130443, -0.033699, 0.0556646, 0.0004552, -0.0086298)
Transformation Matrix:
    0.999963   0.00864165 -2.56493e-05   0.00259166
 -0.00862969     0.998414   -0.0556377    -0.130443
  -0.0004552    0.0556359     0.998451    -0.033699
           0            0            0            1
shift: 0.130469
-----------------------------------------------------------------
(Processed/Input): (303 / 303)
-----------------------------------------------------------------
Sequence number: 5233
Number of scan points: 6970 points.
Number of filtered scan points: 1167 points.
transformed_scan_ptr: 6970 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.4075
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.00628604, -0.119205, -0.0273693, 0.0498128, -0.000304946, -0.000524159)
Transformation Matrix:
           1  0.000508325 -0.000330667   0.00628604
-0.000524159     0.998759   -0.0497921    -0.119205
 0.000304946    0.0497922      0.99876   -0.0273693
           0            0            0            1
shift: 0.119371
-----------------------------------------------------------------
(Processed/Input): (304 / 304)
-----------------------------------------------------------------
Sequence number: 5234
Number of scan points: 7055 points.
Number of filtered scan points: 1166 points.
transformed_scan_ptr: 7055 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.9478
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.069382, -0.110639, -0.0188778, 0.0436968, -0.00111402, 0.00583312)
Transformation Matrix:
    0.999982  -0.00587618 -0.000858124     0.069382
  0.00583309     0.999028   -0.0436886    -0.110639
  0.00111402    0.0436829     0.999045   -0.0188778
           0            0            0            1
shift: 0.130594
-----------------------------------------------------------------
(Processed/Input): (305 / 305)
-----------------------------------------------------------------
Sequence number: 5235
Number of scan points: 6996 points.
Number of filtered scan points: 1163 points.
transformed_scan_ptr: 6996 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.159
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0277672, -0.101208, -0.0108099, 0.0364921, -0.00385145, -0.00209652)
Transformation Matrix:
    0.99999  0.00195462 -0.00392536   0.0277672
-0.00209651    0.999332  -0.0364759   -0.101208
 0.00385144   0.0364837    0.999327  -0.0108099
          0           0           0           1
shift: 0.104948
-----------------------------------------------------------------
(Processed/Input): (306 / 306)
-----------------------------------------------------------------
Sequence number: 5236
Number of scan points: 7061 points.
Number of filtered scan points: 1180 points.
transformed_scan_ptr: 7061 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.7375
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0185314, -0.0768396, -0.00638643, 0.0309048, -0.00465081, -0.00324108)
Transformation Matrix:
   0.999984  0.00309582  -0.0047487   0.0185314
-0.00324104    0.999518  -0.0308847  -0.0768396
  0.0046508   0.0308995    0.999512 -0.00638643
          0           0           0           1
shift: 0.0790426
-----------------------------------------------------------------
(Processed/Input): (307 / 307)
-----------------------------------------------------------------
Sequence number: 5237
Number of scan points: 7014 points.
Number of filtered scan points: 1171 points.
transformed_scan_ptr: 7014 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.8789
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0590202, -0.0460478, 0.00267178, 0.0201747, -0.00473317, 0.00145237)
Transformation Matrix:
   0.999988 -0.00154755 -0.00470288   0.0590202
 0.00145235    0.999795  -0.0201802  -0.0460478
 0.00473315   0.0201731    0.999785  0.00267178
          0           0           0           1
shift: 0.0748584
-----------------------------------------------------------------
(Processed/Input): (308 / 308)
-----------------------------------------------------------------
Sequence number: 5238
Number of scan points: 7092 points.
Number of filtered scan points: 1188 points.
transformed_scan_ptr: 7092 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.664
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0319941, -0.0805672, 0.00793775, 0.0123359, -0.00158777, -0.000305139)
Transformation Matrix:
    0.999999  0.000285536  -0.00159141    0.0319941
-0.000305139     0.999924   -0.0123351   -0.0805672
  0.00158777    0.0123356     0.999923   0.00793775
           0            0            0            1
shift: 0.0866873
-----------------------------------------------------------------
(Processed/Input): (309 / 309)
-----------------------------------------------------------------
Sequence number: 5239
Number of scan points: 6974 points.
Number of filtered scan points: 1168 points.
transformed_scan_ptr: 6974 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.6542
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0123744, -0.0468715, 0.0107983, 0.0102675, 0.00224112, -0.000215196)
Transformation Matrix:
    0.999997  0.000238198   0.00223879    0.0123744
-0.000215196     0.999947   -0.0102678   -0.0468715
 -0.00224112    0.0102673     0.999945    0.0107983
           0            0            0            1
shift: 0.0484774
-----------------------------------------------------------------
(Processed/Input): (310 / 310)
-----------------------------------------------------------------
Sequence number: 5240
Number of scan points: 7057 points.
Number of filtered scan points: 1181 points.
transformed_scan_ptr: 7057 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.2852
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0394357, -0.0397418, 0.00997557, 0.00655165, 0.00524195, -0.00336802)
Transformation Matrix:
   0.999981   0.0034023  0.00521971   0.0394357
-0.00336797    0.999973 -0.00656923  -0.0397418
-0.00524193  0.00655151    0.999965  0.00997557
          0           0           0           1
shift: 0.0559873
-----------------------------------------------------------------
(Processed/Input): (311 / 311)
-----------------------------------------------------------------
Sequence number: 5241
Number of scan points: 7086 points.
Number of filtered scan points: 1155 points.
transformed_scan_ptr: 7086 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.5982
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0448813, -0.0437834, 0.00427117, 0.00952466, 0.00471563, -0.00118483)
Transformation Matrix:
   0.999988   0.0012297   0.0047041   0.0448813
-0.00118481    0.999954 -0.00953009  -0.0437834
-0.00471561  0.00952441    0.999943  0.00427117
          0           0           0           1
shift: 0.0627002
-----------------------------------------------------------------
(Processed/Input): (312 / 312)
-----------------------------------------------------------------
Sequence number: 5242
Number of scan points: 7040 points.
Number of filtered scan points: 1155 points.
transformed_scan_ptr: 7040 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.2579
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0478798, -0.0736286, -0.00172364, 0.0187991, 0.00343542, -0.0021401)
Transformation Matrix:
   0.999992   0.0022043  0.00339457   0.0478798
-0.00214009    0.999821  -0.0188053  -0.0736286
-0.00343542   0.0187979    0.999817 -0.00172364
          0           0           0           1
shift: 0.0878273
-----------------------------------------------------------------
(Processed/Input): (313 / 313)
-----------------------------------------------------------------
Sequence number: 5243
Number of scan points: 6914 points.
Number of filtered scan points: 1134 points.
transformed_scan_ptr: 6914 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.0426
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0602234, -0.124521, -0.00706659, 0.0289744, 0.00193603, 0.000848024)
Transformation Matrix:
   0.999998 -0.00079158  0.00195979   0.0602234
0.000848022     0.99958  -0.0289687   -0.124521
-0.00193603   0.0289703    0.999578 -0.00706659
          0           0           0           1
shift: 0.13832
-----------------------------------------------------------------
(Processed/Input): (314 / 314)
-----------------------------------------------------------------
Sequence number: 5244
Number of scan points: 7030 points.
Number of filtered scan points: 1162 points.
transformed_scan_ptr: 7030 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.0454
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0195744, -0.0553842, -0.0142108, 0.043199, 0.000416312, 0.00139002)
Transformation Matrix:
    0.999999  -0.00137075   0.00047595    0.0195744
  0.00139002     0.999066   -0.0431849   -0.0553842
-0.000416312    0.0431856     0.999067   -0.0142108
           0            0            0            1
shift: 0.0587415
-----------------------------------------------------------------
(Processed/Input): (315 / 315)
-----------------------------------------------------------------
Sequence number: 5245
Number of scan points: 6915 points.
Number of filtered scan points: 1153 points.
transformed_scan_ptr: 6915 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.2967
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.00321482, -0.163607, -0.0169831, 0.05557, -0.00148258, -0.00444239)
Transformation Matrix:
   0.999989  0.00435318 -0.00172701 -0.00321482
-0.00444237    0.998447  -0.0555343   -0.163607
 0.00148258   0.0555414    0.998455  -0.0169831
          0           0           0           1
shift: 0.163639
-----------------------------------------------------------------
(Processed/Input): (316 / 316)
-----------------------------------------------------------------
Sequence number: 5246
Number of scan points: 6907 points.
Number of filtered scan points: 1141 points.
transformed_scan_ptr: 6907 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.743
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0107295, -0.195299, -0.0207221, 0.0635129, 0.000946938, -0.0032093)
Transformation Matrix:
    0.999994   0.00326293  0.000741328    0.0107295
  -0.0032093     0.997978   -0.0634729    -0.195299
-0.000946938    0.0634701     0.997983   -0.0207221
           0            0            0            1
shift: 0.195593
-----------------------------------------------------------------
(Processed/Input): (317 / 317)
-----------------------------------------------------------------
Sequence number: 5247
Number of scan points: 6918 points.
Number of filtered scan points: 1149 points.
transformed_scan_ptr: 6918 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.341
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0106173, -0.226631, -0.0206895, 0.0704721, 0.00463346, -0.00549211)
Transformation Matrix:
   0.999974  0.00580471  0.00423515   0.0106173
-0.00549203    0.997501  -0.0704381   -0.226631
-0.00463344   0.0704131    0.997507  -0.0206895
          0           0           0           1
shift: 0.22688
-----------------------------------------------------------------
(Processed/Input): (318 / 318)
-----------------------------------------------------------------
Sequence number: 5248
Number of scan points: 6938 points.
Number of filtered scan points: 1173 points.
transformed_scan_ptr: 6938 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.8619
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0534253, -0.246985, -0.0237534, 0.0785953, 0.0110533, -0.00605346)
Transformation Matrix:
   0.999921  0.00690254   0.0105435   0.0534253
-0.00605305    0.996889  -0.0785796   -0.246985
 -0.0110531   0.0785096    0.996852  -0.0237534
          0           0           0           1
shift: 0.252697
-----------------------------------------------------------------
(Processed/Input): (319 / 319)
-----------------------------------------------------------------
Sequence number: 5249
Number of scan points: 6922 points.
Number of filtered scan points: 1173 points.
transformed_scan_ptr: 6922 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.6064
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0696521, -0.252689, -0.0272174, 0.0851114, 0.015047, 0.00169889)
Transformation Matrix:
    0.999885 -0.000413667    0.0151363    0.0696521
   0.0016987     0.996381   -0.0849831    -0.252689
  -0.0150464    0.0849991     0.996267   -0.0272174
           0            0            0            1
shift: 0.262113
-----------------------------------------------------------------
(Processed/Input): (320 / 320)
-----------------------------------------------------------------
Sequence number: 5250
Number of scan points: 6977 points.
Number of filtered scan points: 1201 points.
transformed_scan_ptr: 6977 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.2178
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0809414, -0.249778, -0.0377089, 0.0893477, 0.016157, 3.9566e-05)
Transformation Matrix:
   0.999869  0.00140221   0.0160954   0.0809414
3.95608e-05    0.996011  -0.0892282   -0.249778
 -0.0161563   0.0892172    0.995881  -0.0377089
          0           0           0           1
shift: 0.262566
-----------------------------------------------------------------
(Processed/Input): (321 / 321)
-----------------------------------------------------------------
Sequence number: 5251
Number of scan points: 7029 points.
Number of filtered scan points: 1199 points.
transformed_scan_ptr: 7029 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.7145
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0904867, -0.260383, -0.0427997, 0.088714, 0.0143395, 0.00760369)
Transformation Matrix:
   0.999868 -0.00630335   0.0149559   0.0904867
 0.00760284    0.996048  -0.0884865   -0.260383
  -0.014339   0.0885885    0.995965  -0.0427997
          0           0           0           1
shift: 0.275657
-----------------------------------------------------------------
(Processed/Input): (322 / 322)
-----------------------------------------------------------------
Sequence number: 5252
Number of scan points: 7174 points.
Number of filtered scan points: 1192 points.
transformed_scan_ptr: 7174 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.8294
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(0.00160655, -0.2122, -0.0501565, 0.08133, 0.0115309, -0.0120349)
Transformation Matrix:
  0.999861  0.0129315   0.010514 0.00160655
-0.0120338   0.996611 -0.0813728    -0.2122
-0.0115307  0.0812349   0.996628 -0.0501565
         0          0          0          1
shift: 0.212206
-----------------------------------------------------------------
(Processed/Input): (323 / 323)
-----------------------------------------------------------------
Sequence number: 5253
Number of scan points: 7023 points.
Number of filtered scan points: 1215 points.
transformed_scan_ptr: 7023 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.4224
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0166025, -0.264128, -0.0584933, 0.0767746, 0.00876031, -0.0154112)
Transformation Matrix:
  0.999843   0.016037 0.00755138  0.0166025
  -0.01541   0.996926 -0.0768247  -0.264128
-0.0087602  0.0766962   0.997016 -0.0584933
         0          0          0          1
shift: 0.264649
-----------------------------------------------------------------
(Processed/Input): (324 / 324)
-----------------------------------------------------------------
Sequence number: 5254
Number of scan points: 7072 points.
Number of filtered scan points: 1217 points.
transformed_scan_ptr: 7072 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.4366
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(-0.0262126, -0.186815, -0.0620907, 0.0718902, 0.00654149, -0.00951522)
Transformation Matrix:
   0.999933  0.00996034  0.00584079  -0.0262126
-0.00951487    0.997367  -0.0718871   -0.186815
-0.00654144   0.0718267    0.997396  -0.0620907
          0           0           0           1
shift: 0.188645
-----------------------------------------------------------------
(Processed/Input): (325 / 325)
-----------------------------------------------------------------
Sequence number: 5255
Number of scan points: 7055 points.
Number of filtered scan points: 1217 points.
transformed_scan_ptr: 7055 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.2149
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.0146186, -0.204427, -0.065982, 0.0639764, 0.00405791, -0.00826552)
Transformation Matrix:
   0.999958  0.00850794  0.00352103  -0.0146186
-0.00826536    0.997918   -0.063964   -0.204427
 -0.0040579   0.0639322    0.997946   -0.065982
          0           0           0           1
shift: 0.204949
-----------------------------------------------------------------
(Processed/Input): (326 / 326)
-----------------------------------------------------------------
Sequence number: 5256
Number of scan points: 7063 points.
Number of filtered scan points: 1196 points.
transformed_scan_ptr: 7063 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.4614
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0202733, -0.136769, -0.0695265, 0.0542928, 0.00335508, -0.00195896)
Transformation Matrix:
   0.999992  0.00213814  0.00324382   0.0202733
-0.00195895    0.998524  -0.0542725   -0.136769
-0.00335508   0.0542658    0.998521  -0.0695265
          0           0           0           1
shift: 0.138263
-----------------------------------------------------------------
(Processed/Input): (327 / 327)
-----------------------------------------------------------------
Sequence number: 5257
Number of scan points: 7139 points.
Number of filtered scan points: 1221 points.
transformed_scan_ptr: 7139 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.9484
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0476328, -0.112583, -0.0714043, 0.0437346, 0.00135447, -0.00533234)
Transformation Matrix:
   0.999985  0.00538644  0.00112003   0.0476328
-0.00533231    0.999029  -0.0437273   -0.112583
-0.00135447   0.0437206    0.999043  -0.0714043
          0           0           0           1
shift: 0.122245
-----------------------------------------------------------------
(Processed/Input): (328 / 328)
-----------------------------------------------------------------
Sequence number: 5258
Number of scan points: 7089 points.
Number of filtered scan points: 1194 points.
transformed_scan_ptr: 7089 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.3439
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0365596, -0.0933771, -0.0751192, 0.0366116, -0.000171688, -0.00359093)
Transformation Matrix:
   0.999994  0.00358223 -0.00030301   0.0365596
-0.00359092    0.999323  -0.0366026  -0.0933771
0.000171688   0.0366034     0.99933  -0.0751192
          0           0           0           1
shift: 0.100279
-----------------------------------------------------------------
(Processed/Input): (329 / 329)
-----------------------------------------------------------------
Sequence number: 5259
Number of scan points: 7078 points.
Number of filtered scan points: 1188 points.
transformed_scan_ptr: 7078 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.518
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0440829, -0.110874, -0.068697, 0.0241438, -0.000256876, -0.000494746)
Transformation Matrix:
           1  0.000488401 -0.000268744    0.0440829
-0.000494746     0.999708   -0.0241413    -0.110874
 0.000256876    0.0241414     0.999709    -0.068697
           0            0            0            1
shift: 0.119317
-----------------------------------------------------------------
(Processed/Input): (330 / 330)
-----------------------------------------------------------------
Sequence number: 5260
Number of scan points: 7138 points.
Number of filtered scan points: 1186 points.
transformed_scan_ptr: 7138 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.5057
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0194919, -0.0418789, -0.0673187, 0.0117634, -0.00337667, -0.0037807)
Transformation Matrix:
   0.999987  0.00374071 -0.00342087   0.0194919
-0.00378067    0.999924  -0.0117503  -0.0418789
 0.00337666   0.0117631    0.999925  -0.0673187
          0           0           0           1
shift: 0.0461928
-----------------------------------------------------------------
(Processed/Input): (331 / 331)
-----------------------------------------------------------------
Sequence number: 5261
Number of scan points: 7081 points.
Number of filtered scan points: 1210 points.
transformed_scan_ptr: 7081 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.9306
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.00950779, -0.0297796, -0.0614041, 0.000666504, -0.00432721, -0.00281897)
Transformation Matrix:
    0.999987   0.00281609  -0.00432906   0.00950779
 -0.00281894     0.999996 -0.000654324   -0.0297796
   0.0043272  0.000666498      0.99999   -0.0614041
           0            0            0            1
shift: 0.0312606
-----------------------------------------------------------------
(Processed/Input): (332 / 332)
-----------------------------------------------------------------
Sequence number: 5262
Number of scan points: 7073 points.
Number of filtered scan points: 1226 points.
transformed_scan_ptr: 7073 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.1734
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.00258701, -0.0210702, -0.0582593, -0.00906405, -0.00449304, -0.000727929)
Transformation Matrix:
     0.99999  0.000768613  -0.00448624   0.00258701
-0.000727922     0.999959    0.0090672   -0.0210702
  0.00449302  -0.00906384     0.999949   -0.0582593
           0            0            0            1
shift: 0.0212284
-----------------------------------------------------------------
(Processed/Input): (333 / 333)
-----------------------------------------------------------------
Sequence number: 5263
Number of scan points: 7116 points.
Number of filtered scan points: 1233 points.
transformed_scan_ptr: 7116 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.4898
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0108622, 0.0549578, -0.0535503, -0.0232746, -0.0040679, -0.00170783)
Transformation Matrix:
    0.99999  0.00180204 -0.00402703  -0.0108622
-0.00170781    0.999727   0.0232794   0.0549578
 0.00406788  -0.0232723    0.999721  -0.0535503
          0           0           0           1
shift: 0.0560209
-----------------------------------------------------------------
(Processed/Input): (334 / 334)
-----------------------------------------------------------------
Sequence number: 5264
Number of scan points: 7090 points.
Number of filtered scan points: 1238 points.
transformed_scan_ptr: 7090 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.1743
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0321878, 0.0571326, -0.053051, -0.0361132, -0.00349434, 0.00169638)
Transformation Matrix:
   0.999992 -0.00156911  -0.0035533  -0.0321878
 0.00169637    0.999347   0.0360994   0.0571326
 0.00349433  -0.0361051    0.999342   -0.053051
          0           0           0           1
shift: 0.0655759
-----------------------------------------------------------------
(Processed/Input): (335 / 335)
-----------------------------------------------------------------
Sequence number: 5265
Number of scan points: 7061 points.
Number of filtered scan points: 1223 points.
transformed_scan_ptr: 7061 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.0041
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0103182, 0.0714623, -0.0468273, -0.0436874, -0.00159908, 0.000873576)
Transformation Matrix:
    0.999998 -0.000802906   -0.0016357    0.0103182
 0.000873574     0.999046    0.0436721    0.0714623
  0.00159908   -0.0436735     0.999045   -0.0468273
           0            0            0            1
shift: 0.0722034
-----------------------------------------------------------------
(Processed/Input): (336 / 336)
-----------------------------------------------------------------
Sequence number: 5266
Number of scan points: 7212 points.
Number of filtered scan points: 1264 points.
transformed_scan_ptr: 7212 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 30.3298
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.00818407, 0.0803421, -0.0421482, -0.0522927, 0.000264805, 0.00295478)
Transformation Matrix:
    0.999996  -0.00296458  0.000109997  -0.00818407
  0.00295478     0.998629    0.0522694    0.0803421
-0.000264805   -0.0522688     0.998633   -0.0421482
           0            0            0            1
shift: 0.0807579
-----------------------------------------------------------------
(Processed/Input): (337 / 337)
-----------------------------------------------------------------
Sequence number: 5267
Number of scan points: 7110 points.
Number of filtered scan points: 1298 points.
transformed_scan_ptr: 7110 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.6158
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0390188, 0.0603136, -0.0404573, -0.0570108, 0.000137804, -0.00303602)
Transformation Matrix:
    0.999995   0.00302323  0.000310573    0.0390188
 -0.00303601     0.998371    0.0569792    0.0603136
-0.000137804   -0.0569799     0.998375   -0.0404573
           0            0            0            1
shift: 0.0718345
-----------------------------------------------------------------
(Processed/Input): (338 / 338)
-----------------------------------------------------------------
Sequence number: 5268
Number of scan points: 7257 points.
Number of filtered scan points: 1295 points.
transformed_scan_ptr: 7257 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 30.5608
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0288527, 0.0642658, -0.0383967, -0.0577477, -0.000729017, -0.00269437)
Transformation Matrix:
    0.999996   0.00273195 -0.000572291    0.0288527
 -0.00269436     0.998329    0.0577173    0.0642658
 0.000729017   -0.0577156     0.998333   -0.0383967
           0            0            0            1
shift: 0.0704456
-----------------------------------------------------------------
(Processed/Input): (339 / 339)
-----------------------------------------------------------------
Sequence number: 5269
Number of scan points: 7087 points.
Number of filtered scan points: 1293 points.
transformed_scan_ptr: 7087 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.6664
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0207932, 0.0860492, -0.0352017, -0.0560194, -0.00233114, 0.00202468)
Transformation Matrix:
   0.999995 -0.00189098 -0.00244084   0.0207932
 0.00202467     0.99843   0.0559852   0.0860492
 0.00233114  -0.0559899    0.998429  -0.0352017
          0           0           0           1
shift: 0.0885258
-----------------------------------------------------------------
(Processed/Input): (340 / 340)
(Processed/Input): (341 / 341)
-----------------------------------------------------------------
Sequence number: 5270
Number of scan points: 7137 points.
Number of filtered scan points: 1270 points.
transformed_scan_ptr: 7137 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.8945
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0222051, 0.111477, -0.0335059, -0.0523631, -0.0034755, -0.00214402)
Transformation Matrix:
   0.999992  0.00232298 -0.00335851   0.0222051
-0.00214401    0.998627   0.0523465    0.111477
  0.0034755  -0.0523388    0.998623  -0.0335059
          0           0           0           1
shift: 0.113667
-----------------------------------------------------------------
-----------------------------------------------------------------
Sequence number: 5271
Number of scan points: 7172 points.
Number of filtered scan points: 1288 points.
transformed_scan_ptr: 7172 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 30.5847
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0276509, 0.0661089, -0.0346806, -0.0466114, -0.00304792, 0.00273572)
Transformation Matrix:
   0.999992 -0.00259073 -0.00317207   0.0276509
  0.0027357    0.998911    0.046586   0.0661089
 0.00304792  -0.0465943    0.998909  -0.0346806
          0           0           0           1
shift: 0.0716586
-----------------------------------------------------------------
(Processed/Input): (342 / 342)
-----------------------------------------------------------------
Sequence number: 5272
Number of scan points: 7222 points.
Number of filtered scan points: 1268 points.
transformed_scan_ptr: 7222 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 30.644
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0215152, 0.0544073, -0.0363872, -0.0361759, -0.00127469, -0.00369981)
Transformation Matrix:
   0.999992  0.00374349 -0.00114003   0.0215152
 -0.0036998    0.999339   0.0361725   0.0544073
 0.00127469   -0.036168    0.999345  -0.0363872
          0           0           0           1
shift: 0.0585069
-----------------------------------------------------------------
(Processed/Input): (343 / 343)
-----------------------------------------------------------------
Sequence number: 5273
Number of scan points: 7247 points.
Number of filtered scan points: 1260 points.
transformed_scan_ptr: 7247 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.9101
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.01744, 0.0213584, -0.0464163, -0.0265441, 0.00191915, -0.00547775)
Transformation Matrix:
   0.999983  0.00542485  0.00206384     0.01744
-0.00547771    0.999633   0.0265301   0.0213584
-0.00191915   -0.026541    0.999646  -0.0464163
          0           0           0           1
shift: 0.0275742
-----------------------------------------------------------------
(Processed/Input): (344 / 344)
-----------------------------------------------------------------
Sequence number: 5274
Number of scan points: 7289 points.
Number of filtered scan points: 1264 points.
transformed_scan_ptr: 7289 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.9506
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0112714, -0.0230841, -0.0627817, -0.00725053, 0.00593116, -0.00412419)
Transformation Matrix:
   0.999974  0.00408108  0.00596081   0.0112714
-0.00412411    0.999965  0.00722595  -0.0230841
-0.00593113 -0.00725034    0.999956  -0.0627817
          0           0           0           1
shift: 0.0256889
-----------------------------------------------------------------
(Processed/Input): (345 / 345)
-----------------------------------------------------------------
Sequence number: 5275
Number of scan points: 7310 points.
Number of filtered scan points: 1267 points.
transformed_scan_ptr: 7310 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 30.2682
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0597182, -0.0376126, -0.0872623, 0.00796952, 0.00889344, 0.000898762)
Transformation Matrix:
     0.99996 -0.000827829    0.0089002    0.0597182
 0.000898727     0.999968  -0.00796144   -0.0376126
 -0.00889332   0.00796912     0.999929   -0.0872623
           0            0            0            1
shift: 0.070576
-----------------------------------------------------------------
(Processed/Input): (346 / 346)
-----------------------------------------------------------------
Sequence number: 5276
Number of scan points: 7347 points.
Number of filtered scan points: 1271 points.
transformed_scan_ptr: 7347 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 30.3087
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.098143, -0.0699493, -0.105713, 0.0239188, 0.00746288, 0.00254893)
Transformation Matrix:
   0.999969  -0.0023697  0.00752162    0.098143
 0.00254885    0.999711  -0.0238974  -0.0699493
-0.00746281   0.0239158    0.999686   -0.105713
          0           0           0           1
shift: 0.12052
-----------------------------------------------------------------
(Processed/Input): (347 / 347)
-----------------------------------------------------------------
Sequence number: 5277
Number of scan points: 7161 points.
Number of filtered scan points: 1238 points.
transformed_scan_ptr: 7161 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 30.0232
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(0.0682065, -0.128604, -0.112432, 0.0368185, 0.00535479, -0.00240327)
Transformation Matrix:
   0.999983  0.00259875  0.00526266   0.0682065
-0.00240323    0.999319  -0.0368229   -0.128604
-0.00535477   0.0368096    0.999308   -0.112432
          0           0           0           1
shift: 0.145571
-----------------------------------------------------------------
(Processed/Input): (348 / 348)
-----------------------------------------------------------------
Sequence number: 5278
Number of scan points: 7299 points.
Number of filtered scan points: 1230 points.
transformed_scan_ptr: 7299 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.6912
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.00541369, -0.18644, -0.114849, 0.0487166, 0.000879839, -0.0073945)
Transformation Matrix:
    0.999972   0.00742851  0.000518689   0.00541369
 -0.00739443     0.998786   -0.0487025     -0.18644
-0.000879839    0.0486973     0.998813    -0.114849
           0            0            0            1
shift: 0.186518
-----------------------------------------------------------------
(Processed/Input): (349 / 349)
-----------------------------------------------------------------
Sequence number: 5279
Number of scan points: 7331 points.
Number of filtered scan points: 1239 points.
transformed_scan_ptr: 7331 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.6311
Number of iteration: 8
(x,y,z,roll,pitch,yaw):
(-0.146362, -0.192223, -0.113273, 0.0529187, -0.00419416, -0.0277416)
Transformation Matrix:
   0.999606   0.0274774 -0.00565386   -0.146362
 -0.0277378    0.998222  -0.0527574   -0.192223
 0.00419415   0.0528935    0.998591   -0.113273
          0           0           0           1
shift: 0.241602
-----------------------------------------------------------------
(Processed/Input): (350 / 350)
-----------------------------------------------------------------
Sequence number: 5280
Number of scan points: 7272 points.
Number of filtered scan points: 1255 points.
transformed_scan_ptr: 7272 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.6257
Number of iteration: 8
(x,y,z,roll,pitch,yaw):
(0.0163692, -0.156384, -0.115459, 0.0532346, -0.0045453, -0.0103614)
Transformation Matrix:
   0.999936   0.0101047 -0.00508991   0.0163692
 -0.0103611    0.998532  -0.0531596   -0.156384
 0.00454529   0.0532089    0.998573   -0.115459
          0           0           0           1
shift: 0.157239
-----------------------------------------------------------------
(Processed/Input): (351 / 351)
-----------------------------------------------------------------
Sequence number: 5281
Number of scan points: 7281 points.
Number of filtered scan points: 1219 points.
transformed_scan_ptr: 7281 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.4734
Number of iteration: 7
(x,y,z,roll,pitch,yaw):
(-0.0257131, -0.146898, -0.125111, 0.051138, -0.00540706, -0.0127445)
Transformation Matrix:
   0.999904   0.0124512 -0.00605094  -0.0257131
  -0.012744    0.998615  -0.0510427   -0.146898
 0.00540703   0.0511149    0.998678   -0.125111
          0           0           0           1
shift: 0.149131
-----------------------------------------------------------------
(Processed/Input): (352 / 352)
-----------------------------------------------------------------
Sequence number: 5282
Number of scan points: 7122 points.
Number of filtered scan points: 1227 points.
transformed_scan_ptr: 7122 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.5728
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.00496222, -0.165197, -0.124935, 0.0468316, -0.00502806, -0.0066438)
Transformation Matrix:
   0.999965  0.00640109 -0.00533343 -0.00496222
-0.00664367    0.998883  -0.0467801   -0.165197
 0.00502804   0.0468139    0.998891   -0.124935
          0           0           0           1
shift: 0.165272
-----------------------------------------------------------------
(Processed/Input): (353 / 353)
-----------------------------------------------------------------
Sequence number: 5283
Number of scan points: 7181 points.
Number of filtered scan points: 1242 points.
transformed_scan_ptr: 7181 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.1712
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0109563, -0.149546, -0.118116, 0.04024, -0.00503696, -0.00345195)
Transformation Matrix:
  0.999981 0.00324652 -0.0051717  0.0109563
-0.0034519   0.999185 -0.0402115  -0.149546
0.00503694  0.0402286   0.999178  -0.118116
         0          0          0          1
shift: 0.149947
-----------------------------------------------------------------
(Processed/Input): (354 / 354)
-----------------------------------------------------------------
Sequence number: 5284
Number of scan points: 7324 points.
Number of filtered scan points: 1255 points.
transformed_scan_ptr: 7324 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.3514
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0187185, -0.115977, -0.118303, 0.0369926, -0.00504401, -0.0151624)
Transformation Matrix:
   0.999872   0.0149649 -0.00560072  -0.0187185
 -0.0151616    0.999204  -0.0369035   -0.115977
 0.00504399   0.0369837    0.999303   -0.118303
          0           0           0           1
shift: 0.117478
-----------------------------------------------------------------
(Processed/Input): (355 / 355)
-----------------------------------------------------------------
Sequence number: 5285
Number of scan points: 7296 points.
Number of filtered scan points: 1250 points.
transformed_scan_ptr: 7296 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.2911
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0724457, -0.128936, -0.124903, 0.0376617, -0.00466648, -0.0210821)
Transformation Matrix:
   0.999767   0.0208899 -0.00545584  -0.0724457
 -0.0210803    0.999073  -0.0375461   -0.128936
 0.00466646   0.0376524     0.99928   -0.124903
          0           0           0           1
shift: 0.147895
-----------------------------------------------------------------
(Processed/Input): (356 / 356)
-----------------------------------------------------------------
Sequence number: 5286
Number of scan points: 7375 points.
Number of filtered scan points: 1259 points.
transformed_scan_ptr: 7375 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.5077
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.054381, -0.112585, -0.125867, 0.0373023, -0.00485217, -0.00848241)
Transformation Matrix:
   0.999952  0.00829546 -0.00516494   -0.054381
-0.00848221     0.99927  -0.0372512   -0.112585
 0.00485215   0.0372932    0.999293   -0.125867
          0           0           0           1
shift: 0.125031
-----------------------------------------------------------------
(Processed/Input): (357 / 357)
-----------------------------------------------------------------
Sequence number: 5287
Number of scan points: 7280 points.
Number of filtered scan points: 1256 points.
transformed_scan_ptr: 7280 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.325
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0264445, -0.130791, -0.136004, 0.0408493, -0.00672493, -0.00406541)
Transformation Matrix:
   0.999969  0.00378738 -0.00688524   0.0264445
 -0.0040653    0.999159  -0.0408103   -0.130791
 0.00672488    0.040837    0.999143   -0.136004
          0           0           0           1
shift: 0.133438
-----------------------------------------------------------------
(Processed/Input): (358 / 358)
-----------------------------------------------------------------
Sequence number: 5288
Number of scan points: 7429 points.
Number of filtered scan points: 1241 points.
transformed_scan_ptr: 7429 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.0419
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0096785, -0.143535, -0.130457, 0.0394229, -0.00696539, -0.00264637)
Transformation Matrix:
   0.999972  0.00236979  -0.0070642   0.0096785
-0.00264631     0.99922  -0.0393942   -0.143535
 0.00696534   0.0394118    0.999199   -0.130457
          0           0           0           1
shift: 0.143861
-----------------------------------------------------------------
(Processed/Input): (359 / 359)
-----------------------------------------------------------------
Sequence number: 5289
Number of scan points: 7387 points.
Number of filtered scan points: 1248 points.
transformed_scan_ptr: 7387 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.5927
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0693996, -0.151227, -0.120141, 0.0370914, -0.00939525, -0.0114211)
Transformation Matrix:
   0.999891   0.0110646 -0.00981155  -0.0693996
 -0.0114203    0.999251  -0.0369733   -0.151227
 0.00939511   0.0370813    0.999268   -0.120141
          0           0           0           1
shift: 0.16639
-----------------------------------------------------------------
(Processed/Input): (360 / 360)
-----------------------------------------------------------------
Sequence number: 5290
Number of scan points: 7558 points.
Number of filtered scan points: 1261 points.
transformed_scan_ptr: 7558 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.3197
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.0455285, -0.170343, -0.113808, 0.038161, -0.00786673, -0.0144433)
Transformation Matrix:
   0.999865   0.0141322 -0.00841111  -0.0455285
 -0.0144423    0.999172  -0.0380342   -0.170343
 0.00786665   0.0381506    0.999241   -0.113808
          0           0           0           1
shift: 0.176323
-----------------------------------------------------------------
(Processed/Input): (361 / 361)
-----------------------------------------------------------------
Sequence number: 5291
Number of scan points: 7493 points.
Number of filtered scan points: 1289 points.
transformed_scan_ptr: 7493 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.4
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.00557117, -0.159048, -0.113326, 0.0401937, -0.0052193, -0.0105179)
Transformation Matrix:
   0.999931   0.0102995 -0.00563741 -0.00557117
 -0.0105175    0.999139  -0.0401258   -0.159048
 0.00521928   0.0401823    0.999179   -0.113326
          0           0           0           1
shift: 0.159146
-----------------------------------------------------------------
(Processed/Input): (362 / 362)
-----------------------------------------------------------------
Sequence number: 5292
Number of scan points: 7408 points.
Number of filtered scan points: 1291 points.
transformed_scan_ptr: 7408 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.7719
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.000619194, -0.11121, -0.104667, 0.0416035, 0.00104195, -0.00526943)
Transformation Matrix:
    0.999986   0.00530818  0.000821878 -0.000619194
  -0.0052694     0.999121   -0.0415964     -0.11121
 -0.00104195    0.0415914     0.999134    -0.104667
           0            0            0            1
shift: 0.111212
-----------------------------------------------------------------
(Processed/Input): (363 / 363)
-----------------------------------------------------------------
Sequence number: 5293
Number of scan points: 7434 points.
Number of filtered scan points: 1279 points.
transformed_scan_ptr: 7434 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.3974
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.00890863, -0.0845343, -0.114645, 0.040113, 0.00398211, -0.00231175)
Transformation Matrix:
   0.999989  0.00246958  0.00388618  0.00890863
-0.00231173    0.999193  -0.0401113  -0.0845343
 -0.0039821   0.0401019    0.999188   -0.114645
          0           0           0           1
shift: 0.0850024
-----------------------------------------------------------------
(Processed/Input): (364 / 364)
-----------------------------------------------------------------
Sequence number: 5294
Number of scan points: 7450 points.
Number of filtered scan points: 1252 points.
transformed_scan_ptr: 7450 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.2452
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0919992, -0.163071, -0.130888, 0.0417906, 0.0019509, 0.000792493)
Transformation Matrix:
    0.999998 -0.000710295   0.00198231    0.0919992
 0.000792492     0.999127   -0.0417769    -0.163071
  -0.0019509    0.0417783     0.999125    -0.130888
           0            0            0            1
shift: 0.187233
-----------------------------------------------------------------
(Processed/Input): (365 / 365)
-----------------------------------------------------------------
Sequence number: 5295
Number of scan points: 7378 points.
Number of filtered scan points: 1235 points.
transformed_scan_ptr: 7378 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.2457
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(0.036234, -0.204799, -0.137641, 0.0423563, -0.00240168, 0.00782822)
Transformation Matrix:
   0.999967 -0.00792281 -0.00206799    0.036234
 0.00782811    0.999072  -0.0423611   -0.204799
 0.00240168   0.0423435      0.9991   -0.137641
          0           0           0           1
shift: 0.207979
-----------------------------------------------------------------
(Processed/Input): (366 / 366)
-----------------------------------------------------------------
Sequence number: 5296
Number of scan points: 7440 points.
Number of filtered scan points: 1248 points.
transformed_scan_ptr: 7440 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.133
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0470983, -0.179008, -0.145997, 0.0464662, -0.0045119, 0.00366752)
Transformation Matrix:
   0.999983 -0.00387313 -0.00433662   0.0470983
 0.00366747    0.998913  -0.0464657   -0.179008
 0.00451188    0.046449     0.99891   -0.145997
          0           0           0           1
shift: 0.1851
-----------------------------------------------------------------
(Processed/Input): (367 / 367)
-----------------------------------------------------------------
Sequence number: 5297
Number of scan points: 7593 points.
Number of filtered scan points: 1247 points.
transformed_scan_ptr: 7593 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.3468
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(-0.0166649, -0.23262, -0.154198, 0.0495369, -0.00634958, -0.00849287)
Transformation Matrix:
   0.999944  0.00816795 -0.00676205  -0.0166649
 -0.0084926     0.99874   -0.049461    -0.23262
 0.00634953   0.0495157    0.998753   -0.154198
          0           0           0           1
shift: 0.233216
-----------------------------------------------------------------
(Processed/Input): (368 / 368)
-----------------------------------------------------------------
Sequence number: 5298
Number of scan points: 7503 points.
Number of filtered scan points: 1253 points.
transformed_scan_ptr: 7503 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.9441
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.105143, -0.268761, -0.150854, 0.048948, -0.00620553, -0.0187932)
Transformation Matrix:
   0.999804    0.018466 -0.00711642   -0.105143
 -0.0187917    0.998632  -0.0488034   -0.268761
 0.00620549   0.0489275    0.998783   -0.150854
          0           0           0           1
shift: 0.288596
-----------------------------------------------------------------
(Processed/Input): (369 / 369)
-----------------------------------------------------------------
Sequence number: 5299
Number of scan points: 7449 points.
Number of filtered scan points: 1258 points.
transformed_scan_ptr: 7449 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.8322
Number of iteration: 8
(x,y,z,roll,pitch,yaw):
(-0.035597, -0.21761, -0.153335, 0.0491581, -0.00437306, -0.0087476)
Transformation Matrix:
   0.999952  0.00852205 -0.00479743   -0.035597
-0.00874741    0.998756  -0.0490982    -0.21761
 0.00437304   0.0491378    0.998782   -0.153335
          0           0           0           1
shift: 0.220502
-----------------------------------------------------------------
(Processed/Input): (370 / 370)
-----------------------------------------------------------------
Sequence number: 5300
Number of scan points: 7576 points.
Number of filtered scan points: 1267 points.
transformed_scan_ptr: 7576 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.4459
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.0639024, -0.155917, -0.154027, 0.0476488, -0.00434585, -0.00299972)
Transformation Matrix:
   0.999986  0.00278931 -0.00448377  -0.0639024
-0.00299968    0.998861  -0.0476176   -0.155917
 0.00434584   0.0476304    0.998856   -0.154027
          0           0           0           1
shift: 0.168504
-----------------------------------------------------------------
(Processed/Input): (371 / 371)
-----------------------------------------------------------------
Sequence number: 5301
Number of scan points: 7543 points.
Number of filtered scan points: 1231 points.
transformed_scan_ptr: 7543 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.1242
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.120676, -0.0879349, -0.156096, 0.0464575, -0.00477126, -0.00526835)
Transformation Matrix:
   0.999975  0.00504106  -0.0050107   -0.120676
-0.00526826    0.998908   -0.046415  -0.0879349
 0.00477124   0.0464402     0.99891   -0.156096
          0           0           0           1
shift: 0.149316
-----------------------------------------------------------------
(Processed/Input): (372 / 372)
-----------------------------------------------------------------
Sequence number: 5302
Number of scan points: 7411 points.
Number of filtered scan points: 1217 points.
transformed_scan_ptr: 7411 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.1293
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.0753026, -0.0927558, -0.154862, 0.0458442, -0.00384315, -0.0134012)
Transformation Matrix:
   0.999903   0.0132107 -0.00445289  -0.0753026
 -0.0134007    0.998862  -0.0457726  -0.0927558
 0.00384314   0.0458278    0.998942   -0.154862
          0           0           0           1
shift: 0.119474
-----------------------------------------------------------------
(Processed/Input): (373 / 373)
-----------------------------------------------------------------
Sequence number: 5303
Number of scan points: 7436 points.
Number of filtered scan points: 1184 points.
transformed_scan_ptr: 7436 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.9931
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0592792, -0.0851028, -0.163956, 0.0466197, -0.00221784, -0.00898482)
Transformation Matrix:
   0.999957  0.00887158 -0.00263405  -0.0592792
-0.00898467    0.998874  -0.0465811  -0.0851028
 0.00221783   0.0466027    0.998911   -0.163956
          0           0           0           1
shift: 0.103714
-----------------------------------------------------------------
(Processed/Input): (374 / 374)
-----------------------------------------------------------------
Sequence number: 5304
Number of scan points: 7591 points.
Number of filtered scan points: 1178 points.
transformed_scan_ptr: 7591 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.0847
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0124994, -0.117581, -0.174502, 0.0476068, -3.70844e-05, 0.000358325)
Transformation Matrix:
           1 -0.000359684 -1.99906e-05    0.0124994
 0.000358325     0.998867   -0.0475888    -0.117581
 3.70844e-05    0.0475888     0.998867    -0.174502
           0            0            0            1
shift: 0.118244
-----------------------------------------------------------------
(Processed/Input): (375 / 375)
-----------------------------------------------------------------
Sequence number: 5305
Number of scan points: 7298 points.
Number of filtered scan points: 1161 points.
transformed_scan_ptr: 7298 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.1264
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(-0.0136234, -0.177132, -0.189223, 0.049655, -0.000541117, -0.00836918)
Transformation Matrix:
    0.999965   0.00833191 -0.000955834   -0.0136234
 -0.00836908     0.998733   -0.0496284    -0.177132
 0.000541117    0.0496346     0.998767    -0.189223
           0            0            0            1
shift: 0.177655
-----------------------------------------------------------------
(Processed/Input): (376 / 376)
-----------------------------------------------------------------
Sequence number: 5306
Number of scan points: 7259 points.
Number of filtered scan points: 1212 points.
transformed_scan_ptr: 7259 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.3807
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.046401, -0.146826, -0.205453, 0.0522758, 0.0012335, -0.00119812)
Transformation Matrix:
   0.999999  0.00126094  0.00116922    0.046401
-0.00119812    0.998633  -0.0522534   -0.146826
 -0.0012335    0.052252    0.998633   -0.205453
          0           0           0           1
shift: 0.153983
-----------------------------------------------------------------
(Processed/Input): (377 / 377)
-----------------------------------------------------------------
Sequence number: 5307
Number of scan points: 7311 points.
Number of filtered scan points: 1230 points.
transformed_scan_ptr: 7311 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.238
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.000271412, -0.182263, -0.224077, 0.0562679, 0.00237905, -0.000982778)
Transformation Matrix:
    0.999997   0.00111502   0.00232001  0.000271412
-0.000982775     0.998417   -0.0562406    -0.182263
 -0.00237905    0.0562381     0.998415    -0.224077
           0            0            0            1
shift: 0.182263
-----------------------------------------------------------------
(Processed/Input): (378 / 378)
-----------------------------------------------------------------
Sequence number: 5308
Number of scan points: 7311 points.
Number of filtered scan points: 1230 points.
transformed_scan_ptr: 7311 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.3048
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.00341836, -0.183474, -0.259251, 0.0627147, 0.00082895, -0.0100073)
Transformation Matrix:
    0.99995   0.0100394 0.000200098 -0.00341836
 -0.0100071    0.997984  -0.0626787   -0.183474
-0.00082895   0.0626735    0.998034   -0.259251
          0           0           0           1
shift: 0.183506
-----------------------------------------------------------------
(Processed/Input): (379 / 379)
-----------------------------------------------------------------
Sequence number: 5309
Number of scan points: 7396 points.
Number of filtered scan points: 1240 points.
transformed_scan_ptr: 7396 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.2591
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.00463925, -0.183292, -0.297473, 0.0684685, 0.0019006, -0.00831384)
Transformation Matrix:
   0.999964  0.00842429   0.0013273  0.00463925
-0.00831373    0.997621  -0.0684284   -0.183292
 -0.0019006   0.0684149    0.997655   -0.297473
          0           0           0           1
shift: 0.183351
-----------------------------------------------------------------
(Processed/Input): (380 / 380)
-----------------------------------------------------------------
Sequence number: 5310
Number of scan points: 7268 points.
Number of filtered scan points: 1189 points.
transformed_scan_ptr: 7268 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.0194
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.04114, -0.223837, -0.307021, 0.0736115, 0.000959261, -0.00383922)
Transformation Matrix:
    0.999992   0.00389936  0.000674301     -0.04114
 -0.00383921     0.997284   -0.0735482    -0.223837
-0.000959261    0.0735451     0.997291    -0.307021
           0            0            0            1
shift: 0.227586
-----------------------------------------------------------------
(Processed/Input): (381 / 381)
-----------------------------------------------------------------
Sequence number: 5311
Number of scan points: 7358 points.
Number of filtered scan points: 1199 points.
transformed_scan_ptr: 7358 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.2301
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.024231, -0.199113, -0.324296, 0.082577, -0.00153341, 0.00317267)
Transformation Matrix:
   0.999994 -0.00328833 -0.00126648   -0.024231
 0.00317266    0.996587  -0.0824877   -0.199113
 0.00153341   0.0824831    0.996591   -0.324296
          0           0           0           1
shift: 0.200582
-----------------------------------------------------------------
(Processed/Input): (382 / 382)
-----------------------------------------------------------------
Sequence number: 5312
Number of scan points: 7417 points.
Number of filtered scan points: 1219 points.
transformed_scan_ptr: 7417 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.6299
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0424451, -0.156806, -0.33786, 0.0910215, -0.00299049, 0.00646719)
Transformation Matrix:
   0.999975 -0.00671219 -0.00239021  -0.0424451
 0.00646711    0.995838  -0.0909132   -0.156806
 0.00299049   0.0908954    0.995856    -0.33786
          0           0           0           1
shift: 0.162449
-----------------------------------------------------------------
(Processed/Input): (383 / 383)
-----------------------------------------------------------------
Sequence number: 5313
Number of scan points: 7418 points.
Number of filtered scan points: 1211 points.
transformed_scan_ptr: 7418 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.0778
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.00532837, -0.237193, -0.354745, 0.0985066, -0.00385497, 0.00146026)
Transformation Matrix:
   0.999991 -0.00183231 -0.00369266 -0.00532837
 0.00146025    0.995151  -0.0983528   -0.237193
 0.00385497   0.0983466    0.995145   -0.354745
          0           0           0           1
shift: 0.237253
-----------------------------------------------------------------
(Processed/Input): (384 / 384)
-----------------------------------------------------------------
Sequence number: 5314
Number of scan points: 7389 points.
Number of filtered scan points: 1186 points.
transformed_scan_ptr: 7389 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.949
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.0165518, -0.278463, -0.355728, 0.103524, -0.00670592, -0.00591503)
Transformation Matrix:
    0.99996  0.00519036  -0.0072811   0.0165518
-0.00591487    0.994633   -0.103298   -0.278463
 0.00670587    0.103337    0.994624   -0.355728
          0           0           0           1
shift: 0.278955
-----------------------------------------------------------------
(Processed/Input): (385 / 385)
-----------------------------------------------------------------
Sequence number: 5315
Number of scan points: 7286 points.
Number of filtered scan points: 1213 points.
transformed_scan_ptr: 7286 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.2941
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0306358, -0.325218, -0.362146, 0.108272, -0.00630096, -0.00812783)
Transformation Matrix:
   0.999947  0.00739929  -0.0071421  -0.0306358
-0.00812758    0.994117   -0.108006   -0.325218
 0.00630092    0.108058    0.994125   -0.362146
          0           0           0           1
shift: 0.326658
-----------------------------------------------------------------
(Processed/Input): (386 / 386)
-----------------------------------------------------------------
Sequence number: 5316
Number of scan points: 7340 points.
Number of filtered scan points: 1192 points.
transformed_scan_ptr: 7340 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.3282
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0343186, -0.28638, -0.386725, 0.106953, -0.00212845, -0.00409796)
Transformation Matrix:
   0.999989  0.00384733 -0.00255372   0.0343186
-0.00409794    0.994279    -0.10674    -0.28638
 0.00212845    0.106749    0.994284   -0.386725
          0           0           0           1
shift: 0.288428
-----------------------------------------------------------------
(Processed/Input): (387 / 387)
-----------------------------------------------------------------
Sequence number: 5317
Number of scan points: 7444 points.
Number of filtered scan points: 1156 points.
transformed_scan_ptr: 7444 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.0649
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0169402, -0.258296, -0.392524, 0.102899, 0.000785353, -0.00630856)
Transformation Matrix:
     0.99998   0.00635582  0.000133184    0.0169402
 -0.00630851      0.99469     -0.10272    -0.258296
-0.000785352     0.102718      0.99471    -0.392524
           0            0            0            1
shift: 0.25885
-----------------------------------------------------------------
(Processed/Input): (388 / 388)
-----------------------------------------------------------------
Sequence number: 5318
Number of scan points: 7143 points.
Number of filtered scan points: 1180 points.
transformed_scan_ptr: 7143 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.702
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.0166999, -0.268779, -0.3439, 0.0914121, -0.00731849, -0.00229296)
Transformation Matrix:
   0.999971  0.00161533 -0.00749716  -0.0166999
 -0.0022929    0.995824  -0.0912679   -0.268779
 0.00731842   0.0912824    0.995798     -0.3439
          0           0           0           1
shift: 0.269297
-----------------------------------------------------------------
(Processed/Input): (389 / 389)
-----------------------------------------------------------------
Sequence number: 5319
Number of scan points: 6793 points.
Number of filtered scan points: 1117 points.
transformed_scan_ptr: 6793 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.7832
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.0222348, -0.250805, -0.321166, 0.0867183, -0.0139296, -0.000956922)
Transformation Matrix:
    0.999903 -0.000253075   -0.0139597   -0.0222348
-0.000956829     0.996243   -0.0865963    -0.250805
   0.0139292    0.0866012     0.996146    -0.321166
           0            0            0            1
shift: 0.251788
-----------------------------------------------------------------
(Processed/Input): (390 / 390)
(Processed/Input): (391 / 391)
-----------------------------------------------------------------
Sequence number: 5320
Number of scan points: 5984 points.
Number of filtered scan points: 1067 points.
transformed_scan_ptr: 5984 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 29.9431
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.0423867, -0.27433, -0.311235, 0.0751318, -0.00435091, -0.00755687)
Transformation Matrix:
   0.999962   0.0072089 -0.00490572  -0.0423867
-0.00755672    0.997153  -0.0750262    -0.27433
  0.0043509   0.0750604    0.997169   -0.311235
          0           0           0           1
shift: 0.277585
-----------------------------------------------------------------
-----------------------------------------------------------------
Sequence number: 5321
Number of scan points: 6893 points.
Number of filtered scan points: 1089 points.
transformed_scan_ptr: 6893 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.6019
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(-0.11446, -0.101019, -0.373225, 0.0584386, -0.0112716, -0.0103621)
Transformation Matrix:
  0.999883 0.00968596 -0.0118567   -0.11446
-0.0103613   0.998246 -0.0582856  -0.101019
 0.0112714  0.0584016    0.99823  -0.373225
         0          0          0          1
shift: 0.152663
-----------------------------------------------------------------
(Processed/Input): (392 / 392)
-----------------------------------------------------------------
Sequence number: 5322
Number of scan points: 7558 points.
Number of filtered scan points: 1163 points.
transformed_scan_ptr: 7558 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.9927
Number of iteration: 7
(x,y,z,roll,pitch,yaw):
(-0.209147, -0.147651, -0.374034, 0.0404006, -0.0214658, -0.0159696)
Transformation Matrix:
  0.999642  0.0150891 -0.0220889  -0.209147
-0.0159652    0.99907  -0.040042  -0.147651
 0.0214642  0.0403803   0.998954  -0.374034
         0          0          0          1
shift: 0.256014
-----------------------------------------------------------------
(Processed/Input): (393 / 393)
-----------------------------------------------------------------
Sequence number: 5323
Number of scan points: 7724 points.
Number of filtered scan points: 1223 points.
transformed_scan_ptr: 7724 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.2865
Number of iteration: 9
(x,y,z,roll,pitch,yaw):
(-0.0130473, -0.156128, -0.326788, 0.0442178, -0.0180348, -0.00129237)
Transformation Matrix:
   0.999837 0.000493958  -0.0180734  -0.0130473
-0.00129216    0.999023  -0.0441801   -0.156128
  0.0180339   0.0441962     0.99886   -0.326788
          0           0           0           1
shift: 0.156672
-----------------------------------------------------------------
(Processed/Input): (394 / 394)
-----------------------------------------------------------------
Sequence number: 5324
Number of scan points: 7692 points.
Number of filtered scan points: 1267 points.
transformed_scan_ptr: 7692 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.1775
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0142147, -0.154384, -0.270344, 0.0517169, -0.0156456, -0.000510661)
Transformation Matrix:
    0.999877 -0.000298764   -0.0156505    0.0142147
-0.000510598     0.998663   -0.0516858    -0.154384
    0.015645    0.0516875     0.998541    -0.270344
           0            0            0            1
shift: 0.155037
-----------------------------------------------------------------
(Processed/Input): (395 / 395)
-----------------------------------------------------------------
Sequence number: 5325
Number of scan points: 7622 points.
Number of filtered scan points: 1293 points.
transformed_scan_ptr: 7622 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.1253
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(0.0883046, -0.113758, -0.234338, 0.0512347, -0.00981861, 0.00170944)
Transformation Matrix:
    0.99995 -0.00221002 -0.00971801   0.0883046
 0.00170936    0.998685   -0.051229   -0.113758
 0.00981845   0.0512099     0.99864   -0.234338
          0           0           0           1
shift: 0.144009
-----------------------------------------------------------------
(Processed/Input): (396 / 396)
-----------------------------------------------------------------
Sequence number: 5326
Number of scan points: 7754 points.
Number of filtered scan points: 1342 points.
transformed_scan_ptr: 7754 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 28.3065
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0698687, -0.145531, -0.221571, 0.0358335, -0.0118461, 0.00744212)
Transformation Matrix:
   0.999902 -0.00786166  -0.0115712   0.0698687
 0.00744153    0.999327  -0.0359129   -0.145531
  0.0118458   0.0358233    0.999288   -0.221571
          0           0           0           1
shift: 0.161434
-----------------------------------------------------------------
(Processed/Input): (397 / 397)
-----------------------------------------------------------------
Sequence number: 5327
Number of scan points: 7605 points.
Number of filtered scan points: 1354 points.
transformed_scan_ptr: 7605 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.9515
Number of iteration: 12
(x,y,z,roll,pitch,yaw):
(-0.165245, 0.00572372, -0.195268, 0.0211202, -0.0151477, -0.0188742)
Transformation Matrix:
  0.999707  0.0185491 -0.0155396  -0.165245
-0.0188709   0.999605 -0.0208291 0.00572372
 0.0151471  0.0211162   0.999662  -0.195268
         0          0          0          1
shift: 0.165344
-----------------------------------------------------------------
(Processed/Input): (398 / 398)
-----------------------------------------------------------------
Sequence number: 5328
Number of scan points: 7691 points.
Number of filtered scan points: 1327 points.
transformed_scan_ptr: 7691 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.6245
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.368371, 0.153936, -0.159013, 0.00909705, -0.0168099, -0.053239)
Transformation Matrix:
   0.998442    0.053059  -0.0172687   -0.368371
 -0.0532064     0.99855 -0.00818959    0.153936
  0.0168091  0.00909564    0.999817   -0.159013
          0           0           0           1
shift: 0.399241
-----------------------------------------------------------------
(Processed/Input): (399 / 399)
-----------------------------------------------------------------
Sequence number: 5329
Number of scan points: 7830 points.
Number of filtered scan points: 1377 points.
transformed_scan_ptr: 7830 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.344
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(-0.348342, 0.246358, -0.113059, 0.00114739, -0.00546759, -0.0680588)
Transformation Matrix:
     0.99767    0.0679999  -0.00553294    -0.348342
  -0.0680052     0.997685 -0.000772909     0.246358
  0.00546757   0.00114738     0.999984    -0.113059
           0            0            0            1
shift: 0.426654
-----------------------------------------------------------------
(Processed/Input): (400 / 400)
-----------------------------------------------------------------
Sequence number: 5330
Number of scan points: 7697 points.
Number of filtered scan points: 1361 points.
transformed_scan_ptr: 7697 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.6019
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(-0.185508, 0.368252, -0.0909879, 0.000947943, 0.00600624, -0.0792927)
Transformation Matrix:
    0.99684   0.0792153  0.00591225   -0.185508
 -0.0792083    0.996857 -0.00142072    0.368252
-0.00600621 0.000947926    0.999982  -0.0909879
          0           0           0           1
shift: 0.412338
-----------------------------------------------------------------
(Processed/Input): (401 / 401)
-----------------------------------------------------------------
Sequence number: 5331
Number of scan points: 7740 points.
Number of filtered scan points: 1357 points.
transformed_scan_ptr: 7740 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.6466
Number of iteration: 10
(x,y,z,roll,pitch,yaw):
(-0.183823, 0.301894, -0.0894599, 0.00217087, 0.00901698, -0.096706)
Transformation Matrix:
   0.995287   0.0965746   0.0087651   -0.183823
 -0.0965514    0.995323 -0.00303134    0.301894
-0.00901686  0.00217078    0.999957  -0.0894599
          0           0           0           1
shift: 0.353455
-----------------------------------------------------------------
(Processed/Input): (402 / 402)
-----------------------------------------------------------------
Sequence number: 5332
Number of scan points: 7782 points.
Number of filtered scan points: 1363 points.
transformed_scan_ptr: 7782 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 27.3726
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.15064, 0.178696, -0.104569, -0.0026813, 0.0129893, -0.0964126)
Transformation Matrix:
   0.995272   0.0962283   0.0131866    -0.15064
 -0.0962552    0.995356   0.0014185    0.178696
 -0.0129889 -0.00268107    0.999912   -0.104569
          0           0           0           1
shift: 0.233719
-----------------------------------------------------------------
(Processed/Input): (403 / 403)
-----------------------------------------------------------------
Sequence number: 5333
Number of scan points: 7763 points.
Number of filtered scan points: 1371 points.
transformed_scan_ptr: 7763 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.459
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.131219, 0.121442, -0.13412, -0.00921155, 0.0233752, -0.0990807)
Transformation Matrix:
  0.994824  0.0987003  0.0241687  -0.131219
-0.0988917   0.995075  0.0068543   0.121442
-0.0233731 -0.0092089   0.999684   -0.13412
         0          0          0          1
shift: 0.178792
-----------------------------------------------------------------
(Processed/Input): (404 / 404)
-----------------------------------------------------------------
Sequence number: 5334
Number of scan points: 7587 points.
Number of filtered scan points: 1342 points.
transformed_scan_ptr: 7587 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.2326
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.116104, 0.0429939, -0.170554, -0.00623488, 0.0265824, -0.0951703)
Transformation Matrix:
   0.995123   0.0948599   0.0270509   -0.116104
 -0.0949931    0.995471  0.00368092   0.0429939
 -0.0265792 -0.00623264    0.999627   -0.170554
          0           0           0           1
shift: 0.123809
-----------------------------------------------------------------
(Processed/Input): (405 / 405)
-----------------------------------------------------------------
Sequence number: 5335
Number of scan points: 7631 points.
Number of filtered scan points: 1335 points.
transformed_scan_ptr: 7631 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.2382
Number of iteration: 9
(x,y,z,roll,pitch,yaw):
(-0.189573, 0.218267, -0.205939, 0.0210675, 0.00774806, -0.0765324)
Transformation Matrix:
   0.997043   0.0766035  0.00611293   -0.189573
 -0.0764555    0.996839  -0.0215966    0.218267
-0.00774798   0.0210654    0.999748   -0.205939
          0           0           0           1
shift: 0.289099
-----------------------------------------------------------------
(Processed/Input): (406 / 406)
-----------------------------------------------------------------
Sequence number: 5336
Number of scan points: 7552 points.
Number of filtered scan points: 1287 points.
transformed_scan_ptr: 7552 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.6487
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(-0.193114, 0.274085, -0.182912, 0.0437624, -0.00771976, -0.0755916)
Transformation Matrix:
  0.997115  0.0751106 -0.0109941  -0.193114
-0.0755174   0.996215 -0.0430411   0.274085
0.00771968  0.0437471   0.999013  -0.182912
         0          0          0          1
shift: 0.335284
-----------------------------------------------------------------
(Processed/Input): (407 / 407)
-----------------------------------------------------------------
Sequence number: 5337
Number of scan points: 7645 points.
Number of filtered scan points: 1291 points.
transformed_scan_ptr: 7645 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.9745
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.223587, 0.264761, -0.148686, 0.0463249, -0.00657297, -0.0748611)
Transformation Matrix:
  0.997178  0.0744074  -0.010011  -0.223587
-0.0747896   0.996152 -0.0456875   0.264761
0.00657293  0.0463073   0.998906  -0.148686
         0          0          0          1
shift: 0.346539
-----------------------------------------------------------------
(Processed/Input): (408 / 408)
-----------------------------------------------------------------
Sequence number: 5338
Number of scan points: 7612 points.
Number of filtered scan points: 1280 points.
transformed_scan_ptr: 7612 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 25.5773
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.258038, 0.288578, -0.124552, 0.0363832, 0.000716376, -0.0796075)
Transformation Matrix:
    0.996833    0.0794968  -0.00217906    -0.258038
  -0.0795234     0.996171   -0.0363169     0.288578
-0.000716376    0.0363752     0.999338    -0.124552
           0            0            0            1
shift: 0.387119
-----------------------------------------------------------------
(Processed/Input): (409 / 409)
-----------------------------------------------------------------
Sequence number: 5339
Number of scan points: 7688 points.
Number of filtered scan points: 1273 points.
transformed_scan_ptr: 7688 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.2242
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.2472, 0.249524, -0.110988, 0.0237477, 0.00504083, -0.0848053)
Transformation Matrix:
   0.996394    0.084799  0.00300995     -0.2472
 -0.0847026    0.996115   -0.024087    0.249524
-0.00504081   0.0237452    0.999705   -0.110988
          0           0           0           1
shift: 0.35124
-----------------------------------------------------------------
(Processed/Input): (410 / 410)
-----------------------------------------------------------------
Sequence number: 5340
Number of scan points: 7735 points.
Number of filtered scan points: 1229 points.
transformed_scan_ptr: 7735 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.0101
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.221198, 0.202744, -0.104329, 0.0206167, 0.00706665, -0.0841385)
Transformation Matrix:
   0.996438   0.0841665  0.00530761   -0.221198
 -0.0840371    0.996238  -0.0211361    0.202744
-0.00706659   0.0206147    0.999763   -0.104329
          0           0           0           1
shift: 0.300056
-----------------------------------------------------------------
(Processed/Input): (411 / 411)
-----------------------------------------------------------------
Sequence number: 5341
Number of scan points: 7697 points.
Number of filtered scan points: 1254 points.
transformed_scan_ptr: 7697 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.4879
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.195277, 0.15594, -0.0976706, 0.0206083, 0.00707255, -0.0834871)
Transformation Matrix:
   0.996492   0.0835177  0.00532794   -0.195277
 -0.0833881    0.996293  -0.0211247     0.15594
-0.00707249   0.0206063    0.999763  -0.0976706
          0           0           0           1
shift: 0.249901
-----------------------------------------------------------------
(Processed/Input): (412 / 412)
-----------------------------------------------------------------
Sequence number: 5342
Number of scan points: 7693 points.
Number of filtered scan points: 1224 points.
transformed_scan_ptr: 7693 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 26.3092
Number of iteration: 11
(x,y,z,roll,pitch,yaw):
(-0.0915001, 0.275002, -0.102562, 0.0207943, 0.00905369, -0.0689815)
Transformation Matrix:
   0.997581   0.0690997   0.0075969  -0.0915001
  -0.068924    0.997393  -0.0213673    0.275002
-0.00905356    0.020792    0.999743   -0.102562
          0           0           0           1
shift: 0.289825
-----------------------------------------------------------------
(Processed/Input): (413 / 413)
-----------------------------------------------------------------
Sequence number: 5343
Number of scan points: 7791 points.
Number of filtered scan points: 1218 points.
transformed_scan_ptr: 7791 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.9908
Number of iteration: 13
(x,y,z,roll,pitch,yaw):
(0.376144, 0.271669, -0.107494, 0.019919, 0.0125524, -0.0771592)
Transformation Matrix:
  0.996946  0.0773167  0.0109769   0.376144
-0.0770766   0.996808 -0.0208258   0.271669
-0.0125521  0.0199161   0.999723  -0.107494
         0          0          0          1
shift: 0.463992
-----------------------------------------------------------------
(Processed/Input): (414 / 414)
-----------------------------------------------------------------
Sequence number: 5344
Number of scan points: 7733 points.
Number of filtered scan points: 1198 points.
transformed_scan_ptr: 7733 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.4634
Number of iteration: 10
(x,y,z,roll,pitch,yaw):
(0.381135, 0.199073, -0.114097, 0.016813, 0.0133252, -0.0645016)
Transformation Matrix:
  0.997832  0.0646713  0.0122116   0.381135
-0.0644512   0.997765  -0.017636   0.199073
-0.0133248  0.0168107    0.99977  -0.114097
         0          0          0          1
shift: 0.429993
-----------------------------------------------------------------
(Processed/Input): (415 / 415)
-----------------------------------------------------------------
Sequence number: 5345
Number of scan points: 7789 points.
Number of filtered scan points: 1166 points.
transformed_scan_ptr: 7789 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.7508
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.372133, 0.120088, -0.113862, 0.0131314, 0.0137308, -0.0536644)
Transformation Matrix:
  0.998466   0.053814  0.0130051   0.372133
-0.0536336   0.998465 -0.0138486   0.120088
-0.0137304  0.0131298    0.99982  -0.113862
         0          0          0          1
shift: 0.39103
-----------------------------------------------------------------
(Processed/Input): (416 / 416)
-----------------------------------------------------------------
Sequence number: 5346
Number of scan points: 7660 points.
Number of filtered scan points: 1158 points.
transformed_scan_ptr: 7660 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.7116
Number of iteration: 12
(x,y,z,roll,pitch,yaw):
(0.0363866, -0.0595365, -0.120868, 0.0126744, 0.0111505, -0.00475104)
Transformation Matrix:
   0.999927  0.00489193    0.011089   0.0363866
-0.00475072    0.999908  -0.0127268  -0.0595365
 -0.0111503   0.0126732    0.999858   -0.120868
          0           0           0           1
shift: 0.0697752
-----------------------------------------------------------------
(Processed/Input): (417 / 417)
-----------------------------------------------------------------
Sequence number: 5347
Number of scan points: 7634 points.
Number of filtered scan points: 1145 points.
transformed_scan_ptr: 7634 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.7095
Number of iteration: 9
(x,y,z,roll,pitch,yaw):
(-0.136153, -0.385063, -0.117474, 0.00255598, 0.00693744, 0.0693479)
Transformation Matrix:
   0.997572  -0.0692744   0.0070978   -0.136153
  0.0692907    0.997594 -0.00206914   -0.385063
-0.00693738  0.00255592    0.999973   -0.117474
          0           0           0           1
shift: 0.408425
-----------------------------------------------------------------
(Processed/Input): (418 / 418)
-----------------------------------------------------------------
Sequence number: 5348
Number of scan points: 7825 points.
Number of filtered scan points: 1167 points.
transformed_scan_ptr: 7825 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.6382
Number of iteration: 30
(x,y,z,roll,pitch,yaw):
(-0.114384, -0.0282907, -0.113555, -0.00415411, 0.00589696, 0.165612)
Transformation Matrix:
     0.9863   -0.164879  0.00513136   -0.114384
   0.164853    0.986305   0.0050694  -0.0282907
-0.00589692 -0.00415402    0.999974   -0.113555
          0           0           0           1
shift: 0.11783
-----------------------------------------------------------------
(Processed/Input): (419 / 419)
-----------------------------------------------------------------
Sequence number: 5349
Number of scan points: 7728 points.
Number of filtered scan points: 1133 points.
transformed_scan_ptr: 7728 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.4489
Number of iteration: 14
(x,y,z,roll,pitch,yaw):
(-0.212984, -0.0513075, -0.112509, -0.00949535, 0.0104434, 0.28335)
Transformation Matrix:
   0.960072   -0.279656  0.00737175   -0.212984
   0.279558    0.960053   0.0120361  -0.0513075
 -0.0104432 -0.00949469      0.9999   -0.112509
          0           0           0           1
shift: 0.219077
-----------------------------------------------------------------
(Processed/Input): (420 / 420)
-----------------------------------------------------------------
Sequence number: 5350
Number of scan points: 7834 points.
Number of filtered scan points: 1129 points.
transformed_scan_ptr: 7834 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.5459
Number of iteration: 8
(x,y,z,roll,pitch,yaw):
(-0.192809, -0.135761, -0.106899, -0.0167854, 0.0155834, 0.387374)
Transformation Matrix:
  0.925792  -0.377948 0.00808555  -0.192809
  0.377713   0.925675  0.0214266  -0.135761
-0.0155827 -0.0167826   0.999738  -0.106899
         0          0          0          1
shift: 0.23581
-----------------------------------------------------------------
(Processed/Input): (421 / 421)
-----------------------------------------------------------------
Sequence number: 5351
Number of scan points: 7774 points.
Number of filtered scan points: 1112 points.
transformed_scan_ptr: 7774 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.0012
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(0.0074286, -0.129102, -0.115454, -0.0183978, 0.0175921, 0.486314)
Transformation Matrix:
  0.883925  -0.467578 0.00695095  0.0074286
  0.467298   0.883761  0.0244841  -0.129102
-0.0175912  -0.018394   0.999676  -0.115454
         0          0          0          1
shift: 0.129315
-----------------------------------------------------------------
(Processed/Input): (422 / 422)
-----------------------------------------------------------------
Sequence number: 5352
Number of scan points: 7679 points.
Number of filtered scan points: 1117 points.
transformed_scan_ptr: 7679 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.8321
Number of iteration: 9
(x,y,z,roll,pitch,yaw):
(0.0334865, -0.0393233, -0.116594, -0.0132082, 0.0171397, 0.561952)
Transformation Matrix:
  0.846092  -0.532984 0.00746425  0.0334865
  0.532761   0.846022  0.0203081 -0.0393233
-0.0171388 -0.0132059   0.999766  -0.116594
         0          0          0          1
shift: 0.0516495
-----------------------------------------------------------------
(Processed/Input): (423 / 423)
-----------------------------------------------------------------
Sequence number: 5353
Number of scan points: 7721 points.
Number of filtered scan points: 1106 points.
transformed_scan_ptr: 7721 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.0786
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.0832411, 0.0808116, -0.114032, -0.00340595, 0.0114249, 0.650445)
Transformation Matrix:
   0.795762   -0.605568  0.00702939   0.0832411
   0.605501    0.795786  0.00962854   0.0808116
 -0.0114246 -0.00340572    0.999929   -0.114032
          0           0           0           1
shift: 0.116016
-----------------------------------------------------------------
(Processed/Input): (424 / 424)
-----------------------------------------------------------------
Sequence number: 5354
Number of scan points: 7579 points.
Number of filtered scan points: 1124 points.
transformed_scan_ptr: 7579 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.7019
Number of iteration: 7
(x,y,z,roll,pitch,yaw):
(0.161138, 0.282898, -0.103892, 0.00053645, 0.00029883, 0.721962)
Transformation Matrix:
    0.750511    -0.660858  0.000578793     0.161138
    0.660858     0.750511 -0.000205127     0.282898
 -0.00029883   0.00053645            1    -0.103892
           0            0            0            1
shift: 0.325572
-----------------------------------------------------------------
(Processed/Input): (425 / 425)
-----------------------------------------------------------------
Sequence number: 5355
Number of scan points: 7718 points.
Number of filtered scan points: 1109 points.
transformed_scan_ptr: 7718 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 22.8194
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(0.153545, 0.417036, -0.102549, 0.00223669, -0.0117923, 0.814952)
Transformation Matrix:
   0.685856   -0.727709 -0.00646056    0.153545
   0.727642    0.685882  -0.0101151    0.417036
  0.0117921  0.00223654    0.999928   -0.102549
          0           0           0           1
shift: 0.444404
-----------------------------------------------------------------
(Processed/Input): (426 / 426)
-----------------------------------------------------------------
Sequence number: 5356
Number of scan points: 7712 points.
Number of filtered scan points: 1116 points.
transformed_scan_ptr: 7712 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.3309
Number of iteration: 14
(x,y,z,roll,pitch,yaw):
(0.167647, 0.120615, -0.09306, 0.00864881, -0.0191961, 0.902807)
Transformation Matrix:
   0.619295   -0.785142 -0.00509921    0.167647
   0.784924    0.619256  -0.0204258    0.120615
  0.0191949  0.00864711    0.999778    -0.09306
          0           0           0           1
shift: 0.206527
-----------------------------------------------------------------
(Processed/Input): (427 / 427)
-----------------------------------------------------------------
Sequence number: 5357
Number of scan points: 7697 points.
Number of filtered scan points: 1115 points.
transformed_scan_ptr: 7697 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.6614
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(0.0911674, -0.0892255, -0.0762434, 0.00829932, -0.0213044, 0.978562)
Transformation Matrix:
    0.55809   -0.829765 -0.00500532   0.0911674
   0.829507    0.558051   -0.022307  -0.0892255
  0.0213028  0.00829734    0.999739  -0.0762434
          0           0           0           1
shift: 0.127564
-----------------------------------------------------------------
(Processed/Input): (428 / 428)
-----------------------------------------------------------------
Sequence number: 5358
Number of scan points: 7717 points.
Number of filtered scan points: 1104 points.
transformed_scan_ptr: 7717 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.3478
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.00632107, -0.296766, -0.0510121, 0.0081852, -0.0226997, 1.05875)
Transformation Matrix:
   0.489834   -0.871807 -0.00398527 -0.00632107
   0.871521    0.489781  -0.0237964   -0.296766
  0.0226977    0.008183    0.999709  -0.0510121
          0           0           0           1
shift: 0.296833
-----------------------------------------------------------------
(Processed/Input): (429 / 429)
-----------------------------------------------------------------
Sequence number: 5359
Number of scan points: 7720 points.
Number of filtered scan points: 1126 points.
transformed_scan_ptr: 7720 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.6074
Number of iteration: 11
(x,y,z,roll,pitch,yaw):
(-0.0746878, -0.180207, -0.0373959, 0.00506008, -0.0210402, 1.10075)
Transformation Matrix:
  0.452829  -0.891583 -0.0050176 -0.0746878
  0.891349   0.452828 -0.0210485  -0.180207
 0.0210386 0.00505894   0.999766 -0.0373959
         0          0          0          1
shift: 0.195071
-----------------------------------------------------------------
(Processed/Input): (430 / 430)
-----------------------------------------------------------------
Sequence number: 5360
Number of scan points: 7806 points.
Number of filtered scan points: 1099 points.
transformed_scan_ptr: 7806 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.6069
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(-0.143547, -0.0585536, -0.0227612, 9.50338e-05, -0.0181475, 1.1342)
Transformation Matrix:
   0.422785   -0.906198 -0.00758719   -0.143547
   0.906048    0.422853  -0.0164845  -0.0585536
  0.0181465 9.50182e-05    0.999835  -0.0227612
          0           0           0           1
shift: 0.15503
-----------------------------------------------------------------
(Processed/Input): (431 / 431)
-----------------------------------------------------------------
Sequence number: 5361
Number of scan points: 7780 points.
Number of filtered scan points: 1111 points.
transformed_scan_ptr: 7780 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.2265
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.256996, 0.0402603, -0.015658, -0.00476131, -0.0157176, 1.1528)
Transformation Matrix:
   0.405882   -0.913862  -0.0107313   -0.256996
    0.91379    0.405996  -0.0124309   0.0402603
   0.015717 -0.00476071    0.999865   -0.015658
          0           0           0           1
shift: 0.26013
-----------------------------------------------------------------
(Processed/Input): (432 / 432)
-----------------------------------------------------------------
Sequence number: 5362
Number of scan points: 7803 points.
Number of filtered scan points: 1106 points.
transformed_scan_ptr: 7803 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.5569
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(-0.297008, 0.148344, -0.0102422, -0.00870434, -0.0141702, 1.15344)
Transformation Matrix:
     0.4053   -0.914081  -0.0137005   -0.297008
   0.914074    0.405438 -0.00942483    0.148344
  0.0141698 -0.00870335    0.999862  -0.0102422
          0           0           0           1
shift: 0.331993
-----------------------------------------------------------------
(Processed/Input): (433 / 433)
-----------------------------------------------------------------
Sequence number: 5363
Number of scan points: 7831 points.
Number of filtered scan points: 1117 points.
transformed_scan_ptr: 7831 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.1066
Number of iteration: 7
(x,y,z,roll,pitch,yaw):
(-0.268571, 0.0916317, -0.0096754, -0.0107421, -0.0137181, 1.1328)
Transformation Matrix:
   0.424083    -0.90549  -0.0155456   -0.268571
   0.905519    0.424232 -0.00786622   0.0916317
  0.0137177  -0.0107409    0.999848  -0.0096754
          0           0           0           1
shift: 0.283772
-----------------------------------------------------------------
(Processed/Input): (434 / 434)
-----------------------------------------------------------------
Sequence number: 5364
Number of scan points: 7818 points.
Number of filtered scan points: 1136 points.
transformed_scan_ptr: 7818 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.3474
Number of iteration: 5
(x,y,z,roll,pitch,yaw):
(-0.117914, 0.00558036, -0.0101565, -0.0137317, -0.0129893, 1.10901)
Transformation Matrix:
   0.445511   -0.895094  -0.0180797   -0.117914
   0.895182    0.445666 -0.00550946  0.00558036
   0.012989  -0.0137301    0.999821  -0.0101565
          0           0           0           1
shift: 0.118046
-----------------------------------------------------------------
(Processed/Input): (435 / 435)
-----------------------------------------------------------------
Sequence number: 5365
Number of scan points: 7822 points.
Number of filtered scan points: 1127 points.
transformed_scan_ptr: 7822 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 24.342
Number of iteration: 2
(x,y,z,roll,pitch,yaw):
(0.01453, -0.0822427, -0.0121266, -0.0143302, -0.0145031, 1.09486)
Transformation Matrix:
   0.458125   -0.888676  -0.0193811     0.01453
   0.888769    0.458311 -0.00632398  -0.0822427
  0.0145026  -0.0143282    0.999792  -0.0121266
          0           0           0           1
shift: 0.0835164
-----------------------------------------------------------------
(Processed/Input): (436 / 436)
-----------------------------------------------------------------
Sequence number: 5366
Number of scan points: 7832 points.
Number of filtered scan points: 1119 points.
transformed_scan_ptr: 7832 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.6973
Number of iteration: 4
(x,y,z,roll,pitch,yaw):
(0.100815, -0.175179, -0.0166982, -0.0155987, -0.0167408, 1.08619)
Transformation Matrix:
   0.465798   -0.884627  -0.0215997    0.100815
   0.884732    0.466038 -0.00754411   -0.175179
    0.01674  -0.0155959    0.999738  -0.0166982
          0           0           0           1
shift: 0.202117
-----------------------------------------------------------------
(Processed/Input): (437 / 437)
-----------------------------------------------------------------
Sequence number: 5367
Number of scan points: 7826 points.
Number of filtered scan points: 1117 points.
transformed_scan_ptr: 7826 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.6006
Number of iteration: 10
(x,y,z,roll,pitch,yaw):
(0.067527, -0.129874, -0.0169084, -0.0154992, -0.0181769, 1.09923)
Transformation Matrix:
   0.454204   -0.890624   -0.022063    0.067527
   0.890712    0.454476 -0.00914952   -0.129874
  0.0181759  -0.0154961    0.999715  -0.0169084
          0           0           0           1
shift: 0.14638
-----------------------------------------------------------------
(Processed/Input): (438 / 438)
-----------------------------------------------------------------
Sequence number: 5368
Number of scan points: 7794 points.
Number of filtered scan points: 1117 points.
transformed_scan_ptr: 7794 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.2523
Number of iteration: 6
(x,y,z,roll,pitch,yaw):
(-0.0363635, -0.153203, -0.0167384, -0.0141681, -0.0204996, 1.10955)
Transformation Matrix:
  0.444973  -0.895278 -0.0218092 -0.0363635
  0.895309   0.445282 -0.0120487  -0.153203
 0.0204982 -0.0141646   0.999689 -0.0167384
         0          0          0          1
shift: 0.15746
-----------------------------------------------------------------
(Processed/Input): (439 / 439)
-----------------------------------------------------------------
Sequence number: 5369
Number of scan points: 7776 points.
Number of filtered scan points: 1116 points.
transformed_scan_ptr: 7776 points.
map: 6294 points.
NDT has converged: 1
Fitness score: 23.2694
Number of iteration: 3
(x,y,z,roll,pitch,yaw):
(-0.121548, -0.143287, -0.0170433, -0.0134814, -0.0214885, 1.12053)
Transformation Matrix:
  0.435101  -0.900125 -0.0214876  -0.121548
  0.900125   0.435423 -0.0134767  -0.143287
 0.0214869 -0.0134779   0.999678 -0.0170433
         0          0          0          1
shift: 0.187897
-----------------------------------------------------------------
(Processed/Input): (440 / 440)
output_callback
filter_res: 0.2
filename: test_2.pcd
Original: 6294 points.
Filtered: 5131 points.
(Processed/Input): (441 / 440)
Saved 5131 data points to test_2.pcd.